00001
00002 from copy import deepcopy
00003 import os, yaml, string
00004 from rospy import logerr, loginfo, get_param
00005 import rospkg, genpy
00006 import moveit_msgs.msg
00007 from trajectory_msgs.msg import JointTrajectoryPoint
00008 from sr_robot_msgs.msg import GraspArray
00009
00010
00011 def reindent(s, numSpaces):
00012 """
00013 http://code.activestate.com/recipes/66055-changing-the-indentation-of-a-multi-line-string/
00014 """
00015 s = string.split(s, '\n')
00016 s = [(numSpaces * ' ') + line for line in s]
00017 s = string.join(s, '\n')
00018 return s
00019
00020
00021 class Grasp(moveit_msgs.msg.Grasp):
00022 """
00023 Represents a single grasp, basically a wrapper around moveit_msgs/Grasp
00024 with added functions and Shadow Hand specific knowledge.
00025 """
00026 def __init__(self):
00027 moveit_msgs.msg.Grasp.__init__(self)
00028 self.grasp_quality = 0.001
00029 self.joint_names = [
00030 'FFJ1', 'FFJ2', 'FFJ3', 'FFJ4',
00031 'LFJ1', 'LFJ2', 'LFJ3', 'LFJ4', 'LFJ5',
00032 'MFJ1', 'MFJ2', 'MFJ3', 'MFJ4',
00033 'RFJ1', 'RFJ2', 'RFJ3', 'RFJ4',
00034 'THJ1', 'THJ2', 'THJ3', 'THJ4', 'THJ5',
00035 'WRJ1', 'WRJ2']
00036
00037
00038 zero_joints = dict.fromkeys(self.joint_names, 0.0)
00039 self.set_pre_grasp_point(zero_joints)
00040
00041 @classmethod
00042 def from_msg(cls, msg):
00043 """Construct a shadow grasp object from moveit grasp object."""
00044 grasp = Grasp()
00045 grasp.id = msg.id
00046 grasp.pre_grasp_posture = deepcopy(msg.pre_grasp_posture)
00047 grasp.grasp_posture = deepcopy(msg.grasp_posture)
00048 grasp.grasp_pose = deepcopy(msg.grasp_pose)
00049 grasp.grasp_quality = msg.grasp_quality
00050 grasp.pre_grasp_approach = deepcopy(msg.pre_grasp_approach)
00051 grasp.post_grasp_retreat = deepcopy(msg.post_grasp_retreat)
00052 grasp.post_place_retreat = deepcopy(msg.post_place_retreat)
00053 grasp.max_contact_force = msg.max_contact_force
00054 grasp.allowed_touch_objects = deepcopy(msg.allowed_touch_objects)
00055 return grasp
00056
00057 def to_msg(self):
00058 """Return plain moveit_msgs/Grasp version of self."""
00059 raise Exception("TODO - to_msg")
00060
00061 @classmethod
00062 def from_yaml(self, y):
00063 """
00064 Construct a shadow grasp object from YAML object. For example YAML
00065 grabbed from rostopic to a file.
00066 """
00067 grasp = Grasp()
00068 genpy.message.fill_message_args(grasp, y)
00069 return grasp
00070
00071 def set_pre_grasp_point(self, *args, **kwargs):
00072 """
00073 Set the positions for a point (default 0) in the pre-grasp to a dict of
00074 joint positions.
00075 """
00076 self._set_posture_point(self.pre_grasp_posture, *args, **kwargs)
00077
00078 def set_grasp_point(self, *args, **kwargs):
00079 """
00080 Set the positions for a point (default 0) in the grasp to a dict of
00081 joint positions.
00082 """
00083 self._set_posture_point(self.grasp_posture, *args, **kwargs)
00084
00085 def _set_posture_point(self, posture, positions, point=0):
00086 """Set the posture positions using a dict of joint positions."""
00087
00088
00089 posture.joint_names = positions.keys()
00090
00091
00092 if len(posture.points) < point+1:
00093 for _ in range(point+1):
00094 posture.points.append(JointTrajectoryPoint())
00095
00096
00097 jtp = JointTrajectoryPoint()
00098 for _, pos in positions.iteritems():
00099 jtp.positions.append(pos)
00100 posture.points[point] = jtp
00101
00102
00103
00104
00105 class GraspStash(object):
00106 """
00107 Interface to the list of grasps stored in the system. Clients should all
00108 use this library so that it can deal with the detail of the undelying
00109 storage.
00110 """
00111 def __init__(self):
00112
00113 self._store = {}
00114 rp = rospkg.RosPack()
00115 self.grasps_file = get_param('~grasps_file',
00116 default = os.path.join(
00117 rp.get_path('sr_grasp'), 'resource', 'grasps.yaml') )
00118
00119 def get_all(self):
00120 """Return list of all grasps."""
00121 return self._store.values();
00122
00123 def get_grasp_array(self):
00124 arr = GraspArray()
00125 arr.grasps = self.get_all()
00126 return arr
00127
00128 def get_grasp(self, grasp_index):
00129 """Return a single grasp from the stash from it's id field."""
00130 return self._store.get(grasp_index)
00131
00132 def get_grasp_at(self, idx):
00133 """Return the Grasp at the given index."""
00134 return self.get_all()[idx]
00135
00136 def size(self):
00137 """Return the number of grasps."""
00138 return len(self._store)
00139
00140 def put_grasp(self, grasp):
00141 """Stash the given grasp, using it's id field, which must be set."""
00142 if grasp.id is None or grasp.id == "":
00143 raise Exception("Grasp has no id")
00144
00145
00146
00147 self._store[grasp.id] = grasp
00148
00149 def load_all(self):
00150 """Load all configured sources of grasps into the stash."""
00151 self.load_yaml_file(self.grasps_file)
00152
00153 def as_yaml(self):
00154 return genpy.message.strify_message(self.get_grasp_array().grasps)
00155
00156 def load_yaml_file(self, fname):
00157 """Load a set of grasps from a YAML file."""
00158 try:
00159 data = yaml.load(file(fname))
00160 self.load_yaml(data)
00161 except Exception as e:
00162 logerr("Failed to load YAML grasp file: %s error:%s"%(fname, e))
00163 return False
00164 else:
00165 loginfo("Loaded grasps from file: %s"%(fname))
00166 return True
00167
00168 def load_yaml(self, data):
00169 """Load a set of grasps from a YAML object. Throws exceptions on errors."""
00170 for g in data:
00171 grasp = Grasp.from_yaml(g)
00172 self.put_grasp(grasp)
00173
00174 def save_yaml_file(self, fname=""):
00175 if fname == "":
00176 fname = self.grasps_file
00177 with open(fname, "w") as txtfile:
00178 txtfile.write(self.as_yaml())
00179