00001
00002
00003 import roslib
00004 roslib.load_manifest('srs_grasping')
00005 import rospy
00006 import tf
00007 import numpy
00008
00009 from xml.dom import minidom
00010 from srs_msgs.msg import *
00011 from kinematics_msgs.srv import *
00012 from cob_srvs.srv import *
00013 from geometry_msgs.msg import *
00014 from numpy import *
00015 from srs_msgs.msg import GraspingErrorCodes
00016 from cob_kinematics.srv import *
00017 from arm_navigation_msgs.srv import *
00018
00019 class graspingutils():
00020
00021 def __init__(self, simulation=True):
00022 self.simulation = simulation
00023 self.ik_service_name = rospy.get_param("/srs/ik_solver", "/cob_ik_wrapper/arm/get_ik")
00024 self.env_service_name = rospy.get_param("/srs/env_planner", "/environment_server/set_planning_scene_diff")
00025 self.SetPlanningSceneDiffService = rospy.ServiceProxy(self.env_service_name, SetPlanningSceneDiff)
00026 self.ik_service = rospy.ServiceProxy(self.ik_service_name, self.get_ik_srv_type())
00027 self.is_grasped_service = rospy.ServiceProxy('/sdh_controller/is_grasped', Trigger)
00028 self.is_cylindric_grasped_service = rospy.ServiceProxy('/sdh_controller/is_cylindric_grasped', Trigger)
00029 self.is_grasped_aux = rospy.ServiceProxy('/srs_grasping/is_grasped', Trigger)
00030
00031 def get_ik_srv_name(self):
00032 return self.ik_service_name;
00033
00034 def get_ik_srv_type(self):
00035 if self.ik_service_name == "/cob_ik_wrapper/arm/get_ik_extended":
00036 return GetPositionIKExtended()
00037 elif self.ik_service_name == "/srs_arm_kinematics/get_ik" or self.ik_service_name == "/cob_arm_kinematics/get_ik" or self.ik_service_name == "/cob_ik_wrapper/arm/get_ik":
00038 return GetPositionIK();
00039 elif self.ik_service_name == "/srs_arm_kinematics/get_constraint_aware_ik" or self.ik_service_name == "/cob_arm_kinematics/get_constraint_aware_ik" or self.ik_service_name == "/cob_ik_wrapper/arm/get_constraint_aware_ik":
00040 return GetConstraintAwarePositionIK();
00041
00042 def joint_filter(self, finalconfig):
00043 OR = finalconfig[7:14]
00044 fc = [OR[2], OR[3], OR[4], OR[0], OR[1], OR[5], OR[6]]
00045 if (fc[1]>=-1.15 and fc[3]>=-1.15 and fc[5]>=-1.25 and fc[1]<=1 and fc[3]<=1 and fc[5]<=1 and fc[2]>-0.5 and fc[4]>-0.5 and fc[6]>-0.5):
00046 return (True, [fc[0], fc[5], fc[6], fc[1], fc[2], fc[3], fc[4]]);
00047 else:
00048 return (False, []);
00049
00050
00051 def get_grasping_direction(self,matrix):
00052
00053 x = (matrix)[0][2]
00054 y = (matrix)[1][2]
00055 z = (matrix)[2][2]
00056
00057 if x > 0.85:
00058 return "X"
00059
00060 elif x < -0.85:
00061 return "-X"
00062
00063 elif y > 0.85:
00064 return "Y"
00065
00066 elif y < -0.85:
00067 return "-Y"
00068
00069 elif z > 0.85:
00070 return "Z"
00071
00072 elif z < -0.85:
00073 return "-Z"
00074
00075 else:
00076 return GraspingErrorCodes.UNKNOWN_CATEGORY;
00077
00078
00079 def valid_grasp(self, category):
00080 return ((category == "TOP") or (category == "SIDE") or (category == "-SIDE") or (category == "FRONT"));
00081
00082
00083 def get_grasps(self, file_name):
00084
00085 try:
00086 xmldoc = minidom.parse(file_name)
00087 object_id = int(((xmldoc.firstChild)).getElementsByTagName('object_id')[0].firstChild.nodeValue)
00088 res = []
00089 hijos = (xmldoc.firstChild).getElementsByTagName('Grasp')
00090 grasps = []
00091 for hijo in hijos:
00092 sdh_joint_values = ((hijo.getElementsByTagName('joint_values'))[0]).firstChild.nodeValue
00093 aux = (hijo.getElementsByTagName('GraspPose'))[0]
00094 Translation = eval((aux.getElementsByTagName('Translation')[0]).firstChild.nodeValue)
00095 Rotation = eval((aux.getElementsByTagName('Rotation')[0]).firstChild.nodeValue)
00096 grasp = PoseStamped()
00097 grasp.header.frame_id = "/base_link";
00098 grasp.pose.position.x = float(Translation[0])
00099 grasp.pose.position.y = float(Translation[1])
00100 grasp.pose.position.z = float(Translation[2])
00101 Rotation = tf.transformations.quaternion_from_euler(Rotation[0], Rotation[1], Rotation[2], axes='sxyz')
00102 grasp.pose.orientation.x = float(Rotation[0])
00103 grasp.pose.orientation.y = float(Rotation[1])
00104 grasp.pose.orientation.z = float(Rotation[2])
00105 grasp.pose.orientation.w = float(Rotation[3])
00106 aux = (hijo.getElementsByTagName('PreGraspPose'))[0]
00107
00108 Translation = eval((aux.getElementsByTagName('Translation')[0]).firstChild.nodeValue)
00109 Rotation = eval((aux.getElementsByTagName('Rotation')[0]).firstChild.nodeValue)
00110
00111 pre_grasp = PoseStamped()
00112 pre_grasp.header.frame_id = "/base_link";
00113 pre_grasp.pose.position.x = float(Translation[0])
00114 pre_grasp.pose.position.y = float(Translation[1])
00115 pre_grasp.pose.position.z = float(Translation[2])
00116 Rotation = tf.transformations.quaternion_from_euler(Rotation[0], Rotation[1], Rotation[2], axes='sxyz')
00117 pre_grasp.pose.orientation.x = float(Rotation[0])
00118 pre_grasp.pose.orientation.y = float(Rotation[1])
00119 pre_grasp.pose.orientation.z = float(Rotation[2])
00120 pre_grasp.pose.orientation.w = float(Rotation[3])
00121 category = ((hijo.getElementsByTagName('category'))[0]).firstChild.nodeValue
00122
00123 GC = DBGrasp()
00124 GC.object_id = object_id
00125 GC.hand_type = "SDH"
00126 GC.sdh_joint_values = eval(sdh_joint_values)
00127 GC.pre_grasp = pre_grasp
00128 GC.grasp = grasp
00129 GC.category= str(category)
00130 grasps.append(GC)
00131
00132 return grasps
00133 except:
00134 print "There are not generated files for this object."
00135 return GraspingErrorCodes.CORRUPTED_GRASP_FILE;
00136
00137
00138 def get_grasp_category(self, pre, g):
00139
00140 x = pre.x - g.x;
00141 y = pre.y - g.y;
00142 z = pre.z - g.z;
00143
00144 if (x > 0.08):
00145 category = "FRONT";
00146 elif (x < -0.08):
00147 category = "BACK";
00148 elif (y > 0.08):
00149 category = "SIDE";
00150 elif (y < -0.08):
00151 category = "-SIDE";
00152 elif (z > 0.08):
00153 category = "TOP";
00154 elif (z < -0.08):
00155 category = "DOWN";
00156 else:
00157 category = "UNKNOWN";
00158
00159 return category;
00160
00161 def parse_cartesian_param(self, param, now = None):
00162 if now is None:
00163 now = rospy.Time.now()
00164
00165 ps = PoseStamped()
00166 ps.pose.orientation.w = 1.0
00167 ps.header.stamp = now
00168 if type(param) is not PoseStamped and param is not None:
00169 ps.header.frame_id = param[0]
00170 if len(param) > 1:
00171 ps.pose.position.x,ps.pose.position.y,ps.pose.position.z = param[1]
00172 if len(param) > 2:
00173 ps.pose.orientation.x,ps.pose.orientation.y,ps.pose.orientation.z,ps.pose.orientation.w = quaternion_from_euler(*param[2])
00174 else:
00175 ps = param
00176 return ps
00177
00178 def parse_cartesian_parameters(self, arm_name, parameters):
00179 now = rospy.Time.now()
00180
00181
00182 param = parameters
00183 second_param = None
00184 if type(parameters) is list and len(parameters) > 0:
00185 if type(parameters[0]) is not str:
00186 param = parameters[0]
00187 if len(parameters) > 1:
00188 second_param = parameters[1]
00189
00190 pose_target = self.parse_cartesian_param(param, now)[0]
00191
00192
00193 param = second_param
00194 ps = PoseStamped()
00195 ps.pose.orientation.w = 1.0
00196 ps.header.stamp = pose_target.header.stamp
00197 ps.header.frame_id = rospy.get_param("/cob_arm_kinematics/"+arm_name+"/tip_name")
00198 if type(param) is not PoseStamped:
00199 if param is not None and len(param) >=1:
00200 ps.header.frame_id = param[0]
00201 if len(param) > 1:
00202 ps.pose.position.x,ps.pose.position.y,ps.pose.position.z = param[1]
00203 if len(param) > 2:
00204 ps.pose.orientation.x,ps.pose.orientation.y,ps.pose.orientation.z,ps.pose.orientation.w = quaternion_from_euler(*param[2])
00205 else:
00206 ps = param
00207 return pose_target,ps
00208
00209 def callIKSolver(self,current_pose, goal_pose):
00210
00211 req = GetPositionIKRequest();
00212 if self.ik_service_name == "/srs_arm_kinematics/get_ik" or self.ik_service_name == "/cob_arm_kinematics/get_ik" or self.ik_service_name == "/cob_ik_wrapper/arm/get_ik":
00213 req = GetPositionIKRequest();
00214
00215 elif self.ik_service_name == "/srs_arm_kinematics/get_constraint_aware_ik" or self.ik_service_name == "/cob_arm_kinematics/get_constraint_aware_ik" or self.ik_service_name == "/cob_ik_wrapper/arm/get_constraint_aware_ik":
00216 planning_scene_request = SetPlanningSceneDiffRequest()
00217 planning_scene_response = self.SetPlanningSceneDiffService(planning_scene_request)
00218 req = GetConstraintAwarePositionIKRequest();
00219
00220 elif self.ik_service_name == "/cob_ik_wrapper/arm/get_ik_extended":
00221 planning_scene_request = SetPlanningSceneDiffRequest()
00222 planning_scene_response = self.SetPlanningSceneDiffService(planning_scene_request)
00223 ps_target, ps_origin = self.parse_cartesian_parameters('arm', [[goal_pose], ['sdh_palm_link']])
00224 req = GetPositionIKExtendedRequest()
00225 req.ik_pose = ps_origin.pose
00226 req.constraint_aware = True
00227 goal_pose = ps_target
00228 else:
00229 rospy.logerr("Unknown IK service name");
00230 return ([],-1)
00231
00232 req.timeout = rospy.Duration(5.0)
00233 req.ik_request.ik_link_name = "sdh_palm_link"
00234 req.ik_request.pose_stamped = goal_pose
00235 req.ik_request.ik_seed_state.joint_state.position = current_pose
00236 req.ik_request.ik_seed_state.joint_state.name = ["arm_%d_joint" % (d+1) for d in range(7)]
00237 resp = self.ik_service(req);
00238 return (list(resp.solution.joint_state.position), resp.error_code);
00239
00240
00241 def matrix_from_pose(self, gp):
00242 return numpy.matrix(self.array_from_pose(gp))
00243
00244
00245 def pose_from_matrix(self, matrix):
00246 t = tf.transformations.translation_from_matrix(matrix);
00247 q = tf.transformations.quaternion_from_matrix(matrix);
00248
00249 ps = PoseStamped();
00250 ps.header.stamp = rospy.Time.now();
00251 ps.header.frame_id = "/base_link";
00252 ps.pose.position.x = t[0];
00253 ps.pose.position.y = t[1];
00254 ps.pose.position.z = t[2];
00255 ps.pose.orientation.x = q[0];
00256 ps.pose.orientation.y = q[1];
00257 ps.pose.orientation.z = q[2];
00258 ps.pose.orientation.w = q[3];
00259 return ps;
00260
00261
00262 def array_from_pose(self, gp):
00263
00264 q = []
00265 q.append(gp.orientation.x)
00266 q.append(gp.orientation.y)
00267 q.append(gp.orientation.z)
00268 q.append(gp.orientation.w)
00269 e = tf.transformations.euler_from_quaternion(q, axes='sxyz')
00270
00271 matrix = tf.transformations.euler_matrix(e[0],e[1],e[2] ,axes='sxyz')
00272 matrix[0][3] = gp.position.x
00273 matrix[1][3] = gp.position.y
00274 matrix[2][3] = gp.position.z
00275 return matrix
00276
00277
00278 def rotation_matrix(self, obj):
00279
00280 if self.simulation:
00281
00282 e = tf.transformations.euler_from_quaternion([obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w],axes='sxzy');
00283 rotacion = tf.transformations.euler_matrix(e[0],e[1],-e[2], axes='sxyz');
00284 else:
00285
00286 e = tf.transformations.euler_from_quaternion([obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w],axes='sxyz');
00287 rotacion = tf.transformations.euler_matrix(e[0],e[1],e[2], axes='sxyz');
00288
00289 rotacion[0,3] = obj.position.x;
00290 rotacion[1,3] = obj.position.y;
00291 rotacion[2,3] = obj.position.z;
00292
00293 return rotacion;
00294
00295
00296 def OR_to_COB(self, values):
00297
00298 OR = values[7:14]
00299 values = [OR[2], OR[3], OR[4], OR[0], OR[1], OR[5], OR[6]]
00300 return values
00301
00302
00303 def COB_to_OR(self, values):
00304
00305 res = [0 for i in range(28)]
00306 values = [values[5], values[6], values[0], values[3], values[4], values[1], values[2]]
00307
00308 for i in range (0,len(values)):
00309 res[i+7] = float(values[i])
00310 return eval(str(res))
00311
00312
00313 def sdh_tactil_sensor_result(self):
00314 rospy.loginfo("Reading SDH tactil sensors...");
00315
00316 try:
00317 resp1 = self.is_grasped_service()
00318 resp2 = self.is_cylindric_grasped_service()
00319 resp_aux = self.is_grasped_aux()
00320 response = resp1.success.data or resp2.success.data or resp_aux.success.data
00321 except rospy.ServiceException, e:
00322 rospy.logerr("Service did not process request: %s", str(e))
00323 return GraspingErrorCodes.SERVICE_DID_NOT_PROCESS_REQUEST
00324
00325 return response
00326
00327
00328 def set_pregrasp(self, t, category, pregrasp_offset):
00329 t2 = [t[0], t[1], t[2]]
00330
00331 if category == "X":
00332 t2[0] -= pregrasp_offset
00333 elif category == "-X":
00334 t2[0] += pregrasp_offset
00335 elif category == "Y":
00336 t2[1] -= pregrasp_offset
00337 elif category == "-Y":
00338 t2[1] += pregrasp_offset
00339 elif category == "Z":
00340 t2[2] -= pregrasp_offset
00341 elif category == "-Z":
00342 t2[2] += pregrasp_offset
00343 else:
00344 return GraspingErrorCodes.UNKNOWN_CATEGORY
00345
00346 return t2;
00347
00348
00349 def set_pregrasp_offsets(self, category, pre, pregrasps_offsets):
00350 if len(pregrasps_offsets) != 2:
00351 return pre;
00352
00353 os = 0.6;
00354 if category=="FRONT":
00355 o = pregrasps_offsets[0] - os;
00356 offset = (o, 0)[o<=0];
00357 pre.position.x += offset;
00358 if pregrasps_offsets[1] > 0.0:
00359 pre.position.z += pregrasps_offsets[1];
00360 elif category=="SIDE":
00361 o = pregrasps_offsets[0] - os;
00362 offset = (o, 0)[o<=0];
00363 pre.position.y += offset;
00364 if pregrasps_offsets[1] > 0.0:
00365 pre.position.z += pregrasps_offsets[1];
00366 elif category=="-SIDE":
00367 o = pregrasps_offsets[0] - os;
00368 offset = (o, 0)[o<=0];
00369 pre.position.y -= offset;
00370 if pregrasps_offsets[1] > 0.0:
00371 pre.position.z += pregrasps_offsets[1];
00372 else:
00373 o = pregrasps_offsets[1] - os;
00374 offset = (o, 0)[o<=0];
00375 pre.position.z += offset;
00376
00377 return pre;