1 from copy
import deepcopy
5 from rospy
import logerr, loginfo, get_param
9 from trajectory_msgs.msg
import JointTrajectoryPoint
10 from sr_robot_msgs.msg
import GraspArray
15 http://code.activestate.com/recipes/66055-changing-the-indentation-of-a-multi-line-string/ 17 s = string.split(s,
'\n')
18 s = [(numSpaces *
' ') + line
for line
in s]
19 s = string.join(s,
'\n')
23 class Grasp(moveit_msgs.msg.Grasp):
25 Represents a single grasp, basically a wrapper around moveit_msgs/Grasp 26 with added functions and Shadow Hand specific knowledge. 30 moveit_msgs.msg.Grasp.__init__(self)
33 'FFJ1',
'FFJ2',
'FFJ3',
'FFJ4',
34 'LFJ1',
'LFJ2',
'LFJ3',
'LFJ4',
'LFJ5',
35 'MFJ1',
'MFJ2',
'MFJ3',
'MFJ4',
36 'RFJ1',
'RFJ2',
'RFJ3',
'RFJ4',
37 'THJ1',
'THJ2',
'THJ3',
'THJ4',
'THJ5',
46 """Construct a shadow grasp object from moveit grasp object.""" 49 grasp.pre_grasp_posture = deepcopy(msg.pre_grasp_posture)
50 grasp.grasp_posture = deepcopy(msg.grasp_posture)
51 grasp.grasp_pose = deepcopy(msg.grasp_pose)
52 grasp.grasp_quality = msg.grasp_quality
53 grasp.pre_grasp_approach = deepcopy(msg.pre_grasp_approach)
54 grasp.post_grasp_retreat = deepcopy(msg.post_grasp_retreat)
55 grasp.post_place_retreat = deepcopy(msg.post_place_retreat)
56 grasp.max_contact_force = msg.max_contact_force
57 grasp.allowed_touch_objects = deepcopy(msg.allowed_touch_objects)
61 """Return plain moveit_msgs/Grasp version of self.""" 62 raise Exception(
"TODO - to_msg")
67 Construct a shadow grasp object from YAML object. For example YAML 68 grabbed from rostopic to a file. 71 genpy.message.fill_message_args(grasp, y)
76 Set the positions for a point (default 0) in the pre-grasp to a dict of 83 Set the positions for a point (default 0) in the grasp to a dict of 89 """Set the posture positions using a dict of joint positions.""" 92 posture.joint_names = positions.keys()
95 if len(posture.points) < point + 1:
96 for _
in range(point + 1):
97 posture.points.append(JointTrajectoryPoint())
100 jtp = JointTrajectoryPoint()
101 for _, pos
in positions.iteritems():
102 jtp.positions.append(pos)
103 posture.points[point] = jtp
108 Interface to the list of grasps stored in the system. Clients should all 109 use this library so that it can deal with the detail of the undelying 116 rp = rospkg.RosPack()
118 default=os.path.join(
119 rp.get_path(
'sr_grasp'),
'resource',
'grasps.yaml'))
122 """Return list of all grasps.""" 123 return self._store.values()
131 """Return a single grasp from the stash from it's id field.""" 132 return self._store.get(grasp_index)
135 """Return the Grasp at the given index.""" 139 """Return the number of grasps.""" 143 """Stash the given grasp, using it's id field, which must be set.""" 144 if grasp.id
is None or grasp.id ==
"":
145 raise Exception(
"Grasp has no id")
149 self.
_store[grasp.id] = grasp
152 """Load all configured sources of grasps into the stash.""" 159 """Load a set of grasps from a YAML file.""" 161 data = yaml.load(file(fname))
163 except Exception
as e:
164 logerr(
"Failed to load YAML grasp file: %s error:%s" % (fname, e))
167 loginfo(
"Loaded grasps from file: %s" % (fname))
171 """Load a set of grasps from a YAML object. Throws exceptions on errors.""" 173 grasp = Grasp.from_yaml(g)
179 with open(fname,
"w")
as txtfile:
def set_pre_grasp_point(self, args, kwargs)
def set_grasp_point(self, args, kwargs)
def reindent(s, numSpaces)
def put_grasp(self, grasp)
def get_grasp(self, grasp_index)
def get_grasp_array(self)
def load_yaml(self, data)
def load_yaml_file(self, fname)
def save_yaml_file(self, fname="")
def _set_posture_point(self, posture, positions, point=0)
def get_grasp_at(self, idx)