graspingutils.py
Go to the documentation of this file.
00001 #!/usr/bin/python
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 * #GetPositionIKExtended
00017 from arm_navigation_msgs.srv import * #PlanningScene
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                 # parse pose_target
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                 # parse pose_origin
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                         #hack for gazebo simulation
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                         #real robot
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: #category=="TOP":
00373                         o = pregrasps_offsets[1] - os;
00374                         offset = (o, 0)[o<=0];
00375                         pre.position.z += offset;
00376 
00377                 return pre;


srs_grasping
Author(s): Robotnik Automation SLL
autogenerated on Mon Oct 6 2014 08:59:42