hiro_gripper_action.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('hironx_ros_bridge')
00004 import rospy
00005 
00006 import site
00007 site.addsitedir(roslib.rospack.rospackexec(['find','hironx_ros_bridge']) + '/src/hironx_ros_bridge')
00008 site.addsitedir(roslib.rospack.rospackexec(['find','hrpsys']) + '/share/hrpsys/python')
00009 
00010 import os, sys
00011 nameserver = os.environ['RTCTREE_NAMESERVERS']
00012 sys.argv += ['-ORBInitRef','NameService=corbaloc:iiop:'+nameserver+':2809/NameService']
00013 
00014 import rtm
00015 from OpenHRP import GrasperService
00016 import math
00017 
00018 #
00019 # ROS actionlib server
00020 #
00021 
00022 import actionlib
00023 from pr2_controllers_msgs.msg import *
00024 
00025 class HiroGripperCommandAction(object):
00026     def __init__(self, name, hand):
00027         self.createComps()
00028         # create messages that are used to publish feedback/result
00029         self._feedback = Pr2GripperCommandFeedback()
00030         self._result   = Pr2GripperCommandResult()
00031         self._hand = hand
00032         self._action_name = name
00033         self._as = actionlib.SimpleActionServer(self._action_name, Pr2GripperCommandAction, execute_cb=self.execute_cb)
00034         self._as.start()
00035 
00036     # utility from bodyinfo.py
00037     def anglesFromDistance(self, gripDist):
00038         # gripDist is [mm]
00039         safetyMargin = 3
00040 
00041         l1 = 33
00042         l2 = 41.9
00043         l3 = 30
00044         l4 = l2 - safetyMargin*2
00045         l5 = 19
00046 
00047         # if gripDist < 0.0 or gripDist > (l1+l4)*2:
00048         if gripDist < 0.0 or gripDist > (l2+l5 - safetyMargin)*2:
00049             return None
00050 
00051         xPos   = gripDist/2.0 + safetyMargin
00052         # print 'xPos =', xPos
00053         a2Pos  = xPos - l5
00054         # print 'a2Pos =', a2Pos
00055         a1radH = math.acos(a2Pos/l2)
00056         # print 'a1radH = ', a1radH
00057         a1rad  = math.pi/2.0 - a1radH
00058         a2rad  = -a1rad
00059         # dEnd   = l2 * math.cos(a1rad) + l3
00060 
00061         return a1rad, a2rad, -a1rad, -a2rad #, dEnd
00062 
00063     def createComps(self):
00064         self.grsp_svc = None
00065         grsp = rtm.findRTC('grsp')
00066         if not grsp:
00067             return None
00068         grsp_obj = grsp.service('service0')
00069         if not grsp_obj:
00070             return None
00071         self.grsp_svc = grsp_obj._narrow(GrasperService)
00072 
00073     def execute_cb(self, goal):
00074         success = False
00075 
00076         pos = goal.command.position * 1000 # target position of gripper [mm]
00077         effort = goal.command.max_effort   # ignore
00078 
00079         # call Hiro RTC command
00080         if not self.grsp_svc:
00081             rospy.logwarn('grasp_svc is None')
00082         elif self._hand == 'RHAND' or self._hand == 'LHAND':
00083             angles = self.anglesFromDistance(pos)
00084             status, ttm = self.grsp_svc.setJointAngles(self._hand, angles, 1.0)
00085             rospy.logdebug('move %s gripper (%s[mm])' % (self._hand, pos))
00086             success = True
00087         else:
00088             rospy.logwarn('%s is invalid hand type' % self._hand)
00089 
00090         # set actionlib result
00091         self._result.position = pos
00092         self._result.effort = effort
00093 
00094         if success:
00095             self._result.stalled = False
00096             self._result.reached_goal = True
00097             self._as.set_succeeded(self._result)
00098         else:
00099             self._result.stalled = False
00100             self._result.reached_goal = False
00101             self._as.set_aborted(self._result)
00102         return success
00103 
00104 if __name__ == '__main__':
00105     rospy.init_node('hiro_gripper_action')
00106     hand_name = rospy.get_param('~hand_name','')
00107     if not (hand_name in ['LHAND', 'RHAND']):
00108         rospy.logerr("hand_name %s is not LHAND or RHAND, exit..." % hand_name)
00109         exit(0)
00110     HiroGripperCommandAction("gripper_action", hand_name)
00111     rospy.spin()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hironx_ros_bridge_old
Author(s): k-okada
autogenerated on Tue Jul 23 2013 11:49:38