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
00017 class graspingutils():
00018
00019 def __init__(self, simulation=True):
00020 self.simulation = simulation
00021 self.ik_service = rospy.ServiceProxy('/arm_kinematics/get_constraint_aware_ik', GetConstraintAwarePositionIK)
00022 self.is_grasped_service = rospy.ServiceProxy('/sdh_controller/is_grasped', Trigger)
00023 self.is_cylindric_grasped_service = rospy.ServiceProxy('/sdh_controller/is_cylindric_grasped', Trigger)
00024
00025
00026 def joint_filter(self, finalconfig):
00027 OR = finalconfig[7:14]
00028 fc = [OR[2], OR[3], OR[4], OR[0], OR[1], OR[5], OR[6]]
00029 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):
00030 return (True, [fc[0], fc[5], fc[6], fc[1], fc[2], fc[3], fc[4]]);
00031 else:
00032 return (False, []);
00033
00034
00035 def get_grasping_direction(self,matrix):
00036
00037 x = (matrix)[0][2]
00038 y = (matrix)[1][2]
00039 z = (matrix)[2][2]
00040
00041 if x > 0.85:
00042 return "X"
00043
00044 elif x < -0.85:
00045 return "-X"
00046
00047 elif y > 0.85:
00048 return "Y"
00049
00050 elif y < -0.85:
00051 return "-Y"
00052
00053 elif z > 0.85:
00054 return "Z"
00055
00056 elif z < -0.85:
00057 return "-Z"
00058
00059 else:
00060 return GraspingErrorCodes.UNKNOWN_CATEGORY;
00061
00062
00063 def valid_grasp(self, category):
00064 return ((category == "TOP") or (category == "SIDE") or (category == "-SIDE") or (category == "FRONT"));
00065
00066
00067 def get_grasps(self, file_name):
00068
00069 try:
00070 xmldoc = minidom.parse(file_name)
00071 object_id = int(((xmldoc.firstChild)).getElementsByTagName('object_id')[0].firstChild.nodeValue)
00072 res = []
00073 hijos = (xmldoc.firstChild).getElementsByTagName('Grasp')
00074 grasps = []
00075 for hijo in hijos:
00076 sdh_joint_values = ((hijo.getElementsByTagName('joint_values'))[0]).firstChild.nodeValue
00077 aux = (hijo.getElementsByTagName('GraspPose'))[0]
00078 Translation = eval((aux.getElementsByTagName('Translation')[0]).firstChild.nodeValue)
00079 Rotation = eval((aux.getElementsByTagName('Rotation')[0]).firstChild.nodeValue)
00080 grasp = PoseStamped()
00081 grasp.header.frame_id = "/base_link";
00082 grasp.pose.position.x = float(Translation[0])
00083 grasp.pose.position.y = float(Translation[1])
00084 grasp.pose.position.z = float(Translation[2])
00085 Rotation = tf.transformations.quaternion_from_euler(Rotation[0], Rotation[1], Rotation[2], axes='sxyz')
00086 grasp.pose.orientation.x = float(Rotation[0])
00087 grasp.pose.orientation.y = float(Rotation[1])
00088 grasp.pose.orientation.z = float(Rotation[2])
00089 grasp.pose.orientation.w = float(Rotation[3])
00090 aux = (hijo.getElementsByTagName('PreGraspPose'))[0]
00091
00092 Translation = eval((aux.getElementsByTagName('Translation')[0]).firstChild.nodeValue)
00093 Rotation = eval((aux.getElementsByTagName('Rotation')[0]).firstChild.nodeValue)
00094
00095 pre_grasp = PoseStamped()
00096 pre_grasp.header.frame_id = "/base_link";
00097 pre_grasp.pose.position.x = float(Translation[0])
00098 pre_grasp.pose.position.y = float(Translation[1])
00099 pre_grasp.pose.position.z = float(Translation[2])
00100 Rotation = tf.transformations.quaternion_from_euler(Rotation[0], Rotation[1], Rotation[2], axes='sxyz')
00101 pre_grasp.pose.orientation.x = float(Rotation[0])
00102 pre_grasp.pose.orientation.y = float(Rotation[1])
00103 pre_grasp.pose.orientation.z = float(Rotation[2])
00104 pre_grasp.pose.orientation.w = float(Rotation[3])
00105 category = ((hijo.getElementsByTagName('category'))[0]).firstChild.nodeValue
00106
00107 GC = DBGrasp()
00108 GC.object_id = object_id
00109 GC.hand_type = "SDH"
00110 GC.sdh_joint_values = eval(sdh_joint_values)
00111 GC.pre_grasp = pre_grasp
00112 GC.grasp = grasp
00113 GC.category= str(category)
00114 grasps.append(GC)
00115
00116 return grasps
00117 except:
00118 print "There are not generated files for this object."
00119 return GraspingErrorCodes.CORRUPTED_GRASP_FILE;
00120
00121
00122 def get_grasp_category(self, pre, g):
00123
00124 x = pre.x - g.x;
00125 y = pre.y - g.y;
00126 z = pre.z - g.z;
00127
00128 if (x > 0.08):
00129 category = "FRONT";
00130 elif (x < -0.08):
00131 category = "BACK";
00132 elif (y > 0.08):
00133 category = "SIDE";
00134 elif (y < -0.08):
00135 category = "-SIDE";
00136 elif (z > 0.08):
00137 category = "TOP";
00138 elif (z < -0.08):
00139 category = "DOWN";
00140 else:
00141 category = "UNKNOWN";
00142
00143 return category;
00144
00145
00146 def callIKSolver(self,current_pose, goal_pose):
00147 req = GetConstraintAwarePositionIKRequest();
00148 req.ik_request.ik_link_name = "sdh_palm_link";
00149 req.ik_request.ik_seed_state.joint_state.position = current_pose;
00150 req.ik_request.pose_stamped = goal_pose;
00151 resp = self.ik_service(req);
00152 return (list(resp.solution.joint_state.position), resp.error_code);
00153
00154
00155 def matrix_from_pose(self, gp):
00156 return numpy.matrix(self.array_from_pose(gp))
00157
00158
00159 def pose_from_matrix(self, matrix):
00160 t = tf.transformations.translation_from_matrix(matrix);
00161 q = tf.transformations.quaternion_from_matrix(matrix);
00162
00163 ps = PoseStamped();
00164 ps.header.stamp = rospy.Time.now();
00165 ps.header.frame_id = "/base_link";
00166 ps.pose.position.x = t[0];
00167 ps.pose.position.y = t[1];
00168 ps.pose.position.z = t[2];
00169 ps.pose.orientation.x = q[0];
00170 ps.pose.orientation.y = q[1];
00171 ps.pose.orientation.z = q[2];
00172 ps.pose.orientation.w = q[3];
00173 return ps;
00174
00175
00176 def array_from_pose(self, gp):
00177
00178 q = []
00179 q.append(gp.orientation.x)
00180 q.append(gp.orientation.y)
00181 q.append(gp.orientation.z)
00182 q.append(gp.orientation.w)
00183 e = tf.transformations.euler_from_quaternion(q, axes='sxyz')
00184
00185 matrix = tf.transformations.euler_matrix(e[0],e[1],e[2] ,axes='sxyz')
00186 matrix[0][3] = gp.position.x
00187 matrix[1][3] = gp.position.y
00188 matrix[2][3] = gp.position.z
00189 return matrix
00190
00191
00192 def rotation_matrix(self, obj):
00193
00194 if self.simulation:
00195
00196 e = tf.transformations.euler_from_quaternion([obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w],axes='sxzy');
00197 rotacion = tf.transformations.euler_matrix(e[0],e[1],-e[2], axes='sxyz');
00198 else:
00199
00200 e = tf.transformations.euler_from_quaternion([obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w],axes='sxyz');
00201 rotacion = tf.transformations.euler_matrix(e[0],e[1],e[2], axes='sxyz');
00202
00203 rotacion[0,3] = obj.position.x;
00204 rotacion[1,3] = obj.position.y;
00205 rotacion[2,3] = obj.position.z;
00206
00207 return rotacion;
00208
00209
00210 def OR_to_COB(self, values):
00211
00212 OR = values[7:14]
00213 values = [OR[2], OR[3], OR[4], OR[0], OR[1], OR[5], OR[6]]
00214 return values
00215
00216
00217 def COB_to_OR(self, values):
00218
00219 res = [0 for i in range(28)]
00220 values = [values[5], values[6], values[0], values[3], values[4], values[1], values[2]]
00221
00222 for i in range (0,len(values)):
00223 res[i+7] = float(values[i])
00224 return eval(str(res))
00225
00226
00227 def sdh_tactil_sensor_result(self):
00228 rospy.loginfo("Reading SDH tactil sensors...");
00229
00230 try:
00231 resp1 = self.is_grasped_service()
00232 resp2 = self.is_cylindric_grasped_service()
00233 response = resp1.success.data or resp2.success.data
00234 except rospy.ServiceException, e:
00235 rospy.logerr("Service did not process request: %s", str(e))
00236 return GraspingErrorCodes.SERVICE_DID_NOT_PROCESS_REQUEST
00237
00238 return response
00239
00240
00241 def set_pregrasp(self, t, category, pregrasp_offset):
00242 t2 = [t[0], t[1], t[2]]
00243
00244 if category == "X":
00245 t2[0] -= pregrasp_offset
00246 elif category == "-X":
00247 t2[0] += pregrasp_offset
00248 elif category == "Y":
00249 t2[1] -= pregrasp_offset
00250 elif category == "-Y":
00251 t2[1] += pregrasp_offset
00252 elif category == "Z":
00253 t2[2] -= pregrasp_offset
00254 elif category == "-Z":
00255 t2[2] += pregrasp_offset
00256 else:
00257 return GraspingErrorCodes.UNKNOWN_CATEGORY
00258
00259 return t2;
00260
00261
00262 def set_pregrasp_offsets(self, category, pre, pregrasps_offsets):
00263 if len(pregrasps_offsets) != 2:
00264 return pre;
00265
00266 if category=="FRONT":
00267 o = pregrasps_offsets[0] - 0.2;
00268 offset = (o, 0)[o<=0];
00269 pre.position.x += offset;
00270 if pregrasps_offsets[1] > 0.0:
00271 pre.position.z += pregrasps_offsets[1];
00272 elif category=="SIDE":
00273 o = pregrasps_offsets[0] - 0.2;
00274 offset = (o, 0)[o<=0];
00275 pre.position.y += offset;
00276 if pregrasps_offsets[1] > 0.0:
00277 pre.position.z += pregrasps_offsets[1];
00278 elif category=="-SIDE":
00279 o = pregrasps_offsets[0] - 0.2;
00280 offset = (o, 0)[o<=0];
00281 pre.position.y -= offset;
00282 if pregrasps_offsets[1] > 0.0:
00283 pre.position.z += pregrasps_offsets[1];
00284 else:
00285 o = pregrasps_offsets[1] - 0.2;
00286 offset = (o, 0)[o<=0];
00287 pre.position.z += offset;
00288
00289 return pre;
