00001
00002
00003 import roslib
00004 roslib.load_manifest('srs_grasping')
00005 import rospy
00006 import openravepy
00007 import time
00008 import sys
00009 import tf
00010 import os
00011 import copy
00012
00013
00014 from numpy import *
00015 from traceback import print_exc
00016 from xml.dom.minidom import Document
00017 import re
00018 from srs_msgs.msg import GraspingErrorCodes
00019 class openraveutils():
00020
00021 def __init__(self, databaseutils=None,
00022 robotName="care-o-bot3.zae",
00023 robotManipulator="arm",
00024 env=None,
00025 robot=None,
00026 pregrasp_offset=0.2):
00027
00028 self.robotName = roslib.packages.get_pkg_dir("srs_grasping")+"/robots/"+robotName;
00029 self.manipulator = robotManipulator;
00030 self.env = env;
00031 self.robot = robot;
00032 self.pregrasp_offset = pregrasp_offset;
00033 self.databaseutils = databaseutils
00034
00035
00036 if self.databaseutils is None:
00037 self.databaseutils = grasping_functions.databaseutils();
00038
00039 self.graspingutils = self.databaseutils.get_graspingutils();
00040
00041 try:
00042 if self.env is None:
00043 self.env = openravepy.Environment();
00044 if self.robot is None:
00045 self.robot = self.env.ReadRobotXMLFile(self.robotName);
00046
00047 self.robot.SetActiveManipulator(self.manipulator);
00048 self.env.AddRobot(self.robot);
00049 except:
00050 rospy.logerr("The robot file %s does not exists.", self.robotName);
00051 return GraspingErrorCodes.CORRUPTED_ROBOT_MESH_FILE;
00052
00053
00054 def init_env(self,object_id):
00055 mesh_file = self.databaseutils.get_mesh(object_id);
00056 if mesh_file < 0:
00057 return mesh_file
00058
00059 try:
00060 target = self.env.ReadKinBodyXMLFile(mesh_file)
00061 self.env.AddKinBody(target);
00062 os.remove(mesh_file);
00063 except:
00064 rospy.logerr("The mesh data does not work correctly.");
00065 os.remove(mesh_file);
00066 return GraspingErrorCodes.CORRUPTED_MESH_FILE;
00067
00068 return openravepy.databases.grasping.GraspingModel(robot=self.robot,target=target);
00069
00070
00071 def generate_grasp_file_OLD(self, object_id):
00072
00073 gmodel = self.init_env(object_id);
00074 if gmodel < 0:
00075 return gmodel
00076
00077 self.generate_grasps_OLD(gmodel);
00078
00079 f_name = "/tmp/"+str(object_id)+".xml"
00080
00081 f = open(f_name,'w')
00082 f.write("<?xml version=\"1.0\" ?>\n")
00083 f.write("<GraspList>\n")
00084 f.write("<object_id>"+str(object_id)+"</object_id>\n")
00085 f.write("<hand_type>SDH</hand_type>\n")
00086 f.write("<joint_names>[sdh_knuckle_joint, sdh_thumb_2_joint, sdh_thumb_3_joint, sdh_finger_12_joint, sdh_finger_13_joint, sdh_finger_22_joint, sdh_finger_23_joint]</joint_names>\n")
00087 f.write("<tip_link>sdh_palm_link</tip_link>\n")
00088
00089 cont = 0
00090 for i in range(0,len(gmodel.grasps)):
00091 grasp = gmodel.grasps[i]
00092 print '\r[%s%%][%s/%s] Adding grasps to the new XML file...' % (int((float(float((i+1))/float(len(gmodel.grasps))))*100),(i+1),len(gmodel.grasps)),
00093 sys.stdout.flush()
00094 try:
00095 contacts,finalconfig,mindist,volume = gmodel.testGrasp(grasp=grasp,translate=True,forceclosure=True)
00096 (err, values) = self.graspingutils.joint_filter(finalconfig[0]);
00097 if not err:
00098 continue
00099
00100 self.robot.GetController().Reset(0)
00101 self.robot.SetDOFValues(finalconfig[0])
00102 self.robot.SetTransform(finalconfig[1])
00103 self.env.UpdatePublishedBodies()
00104 index = (self.robot.GetLink("sdh_palm_link")).GetIndex()
00105 matrix = (self.robot.GetLinkTransformations())[index]
00106 t = tf.transformations.translation_from_matrix(matrix)
00107 e = tf.transformations.euler_from_matrix(matrix, axes='sxyz')
00108
00109 category = self.graspingutils.get_grasping_direction(matrix)
00110 if category is GraspingErrorCodes.UNKNOWN_CATEGORY:
00111 continue
00112
00113 tp = []
00114 tp = self.graspingutils.set_pregrasp(t, category, self.pregrasp_offset);
00115 if tp is GraspingErrorCodes.UNKNOWN_CATEGORY:
00116 continue;
00117
00118 f.write("<Grasp Index=\""+str(cont)+"\">\n")
00119 f.write("<joint_values>"+str(values)+"</joint_values>\n")
00120 f.write("<GraspPose>\n")
00121 f.write("<Translation>["+str(t[0])+", "+str(t[1])+", "+str(t[2])+"]</Translation>\n")
00122 f.write("<Rotation>["+str(e[0])+", "+str(e[1])+", "+str(e[2])+"]</Rotation>\n")
00123 f.write("</GraspPose>\n")
00124 f.write("<PreGraspPose>\n")
00125 f.write("<Translation>["+str(tp[0])+", "+str(tp[1])+", "+str(tp[2])+"]</Translation>\n")
00126 f.write("<Rotation>["+str(e[0])+", "+str(e[1])+", "+str(e[2])+"]</Rotation>\n")
00127 f.write("</PreGraspPose>\n")
00128 f.write("<category>"+category+"</category>\n")
00129 f.write("</Grasp>\n")
00130
00131 cont += 1
00132 except:
00133 continue
00134
00135
00136 f.write("<NumberOfGrasps>"+str(cont)+"</NumberOfGrasps>\n")
00137 f.write("</GraspList>")
00138 f.close()
00139
00140 f = open(f_name, 'r')
00141 res = f.read()
00142 f.close()
00143 os.remove(f_name)
00144
00145 print "%d grasps have been added to the XML file." %cont
00146 return res;
00147
00148
00149 def generate_grasps_OLD(self, gmodel):
00150 x = time.time();
00151 if not gmodel.load():
00152 rospy.loginfo("GENERATING GRASPS...")
00153 gmodel.numthreads = 1
00154 gmodel.autogenerate()
00155 rospy.loginfo("GRASPS GENERATION HAS FINISHED. Time employed: %s", str(time.time() - x))
00156
00157 def generator_OLD(self, object_id):
00158 grasps = self.generate_grasp_file_OLD(object_id)
00159 if grasps < 0:
00160 return grasps;
00161
00162 return self.databaseutils.insert_grasps(object_id, grasps);
00163
00164
00165 def show_all_grasps(self, object_id, grasps):
00166
00167 gmodel = self.init_env(object_id);
00168 if gmodel < 0:
00169 return gmodel
00170
00171 self.init_simulator();
00172 manip = self.robot.GetManipulator(self.manipulator)
00173 with openravepy.databases.grasping.GraspingModel.GripperVisibility(manip):
00174 for i in range(0,len(grasps)):
00175 grasp = grasps[i]
00176 print 'grasp %d/%d'%(i,len(grasps))
00177
00178 dof_values = self.graspingutils.COB_to_OR(grasp.sdh_joint_values);
00179 self.robot.SetDOFValues(dof_values);
00180 Tgrasp = self.graspingutils.array_from_pose(grasp.grasp.pose)
00181 index = (self.robot.GetLink("sdh_palm_link")).GetIndex()
00182 matrix = (self.robot.GetLinkTransformations())[index]
00183 Tdelta = dot(Tgrasp,linalg.inv(matrix))
00184 for link in manip.GetChildLinks():
00185 link.SetTransform(dot(Tdelta,link.GetTransform()))
00186 self.env.UpdatePublishedBodies()
00187
00188 raw_input('Next config.')
00189 return GraspingErrorCodes.SUCCESS;
00190
00191
00192 def grasp_view(self, object_id, grasp, object_pose):
00193
00194 gmodel = self.init_env(object_id);
00195 if gmodel < 0:
00196 return gmodel
00197
00198 self.init_simulator();
00199
00200 m = self.graspingutils.matrix_from_pose(grasp.grasp.pose);
00201 rot = self.graspingutils.matrix_from_pose(object_pose);
00202 sol = rot.I * m;
00203 t = tf.transformations.translation_from_matrix(sol)
00204 q = tf.transformations.quaternion_from_matrix(sol)
00205
00206 grasp.grasp.pose.position.x = t[0]
00207 grasp.grasp.pose.position.y = t[1]
00208 grasp.grasp.pose.position.z = t[2]
00209 grasp.grasp.pose.orientation.x = q[0]
00210 grasp.grasp.pose.orientation.y = q[1]
00211 grasp.grasp.pose.orientation.z = q[2]
00212 grasp.grasp.pose.orientation.w = q[3]
00213
00214 manip = self.robot.GetManipulator(self.manipulator)
00215
00216 with openravepy.databases.grasping.GraspingModel.GripperVisibility(manip):
00217 dof_values = self.graspingutils.COB_to_OR(grasp.sdh_joint_values)
00218 self.robot.SetDOFValues(dof_values)
00219 Tgrasp = self.graspingutils.array_from_pose(grasp.grasp.pose)
00220 index = (self.robot.GetLink("sdh_palm_link")).GetIndex()
00221 matrix = (self.robot.GetLinkTransformations())[index]
00222 Tdelta = dot(Tgrasp,linalg.inv(matrix))
00223 for link in manip.GetChildLinks():
00224 link.SetTransform(dot(Tdelta,link.GetTransform()))
00225 self.env.UpdatePublishedBodies()
00226 time.sleep(10.0)
00227 return GraspingErrorCodes.SUCCESS;
00228
00229
00230 def init_simulator(self):
00231 self.env.SetViewer('qtcoin');
00232 time.sleep(1.0)
00233
00234
00235
00236
00237 def generator(self, object_id):
00238 grasps = self.generate_grasp_file(object_id)
00239 if grasps < 0:
00240 return grasps;
00241 elif self.databaseutils.insert_grasps(object_id, grasps) < 0:
00242 return GraspingErrorCodes.SERVICE_DID_NOT_PROCESS_REQUEST;
00243 else:
00244 return GraspingErrorCodes.SUCCESS;
00245
00246
00247 def generate_grasps(self, gmodel, doc):
00248 starttime = time.time();
00249 rospy.loginfo("GENERATING GRASPS...")
00250 counter = self.autogenerate(gmodel, doc);
00251 rospy.loginfo("GRASPS GENERATION HAS FINISHED. Time employed: %s", str(time.time() - starttime))
00252 return counter;
00253
00254
00255 def autogenerate(self, gmodel, doc, options=None):
00256 counter = self.generate(gmodel, doc, *gmodel.autogenerateparams(options))
00257 starttime = time.time();
00258 gmodel.save()
00259 return counter
00260
00261
00262 def generate(self, gmodel, doc, *args, **kwargs):
00263 graspingnoise = args[4]
00264 forceclosure = args[5]
00265 forceclosurethreshold = args[6]
00266 checkgraspfn = args[7]
00267
00268 starttime = time.time()
00269 statesaver = self.robot.CreateRobotStateSaver()
00270 bodies = [(b,b.IsEnabled()) for b in self.env.GetBodies() if b != self.robot and b != gmodel.target]
00271 if gmodel.disableallbodies:
00272 for b in bodies:
00273 b[0].Enable(False)
00274 try:
00275 if gmodel.numthreads is not None and gmodel.numthreads > 1:
00276 gmodel._generateThreaded(*args,**kwargs)
00277 else:
00278 with gmodel.GripperVisibility(gmodel.manip):
00279 if self.env.GetViewer() is not None:
00280 self.env.UpdatePublishedBodies()
00281
00282 aux = [item for item in args]
00283 aux[1] = array([0.03, 0.05])
00284
00285 producer,consumer,gatherer,numjobs = gmodel.generatepcg(*aux,**kwargs)
00286 counter = 0
00287 counter_isValid = 0
00288 for work in producer():
00289 print '\r[grasp %d/%d][Good grasps: %d]'%(counter,numjobs,counter_isValid),
00290 sys.stdout.flush()
00291 counter += 1
00292 results = self.consumer(gmodel, doc, counter_isValid, graspingnoise, forceclosure, forceclosurethreshold, checkgraspfn, *work)
00293 if results is not None:
00294 gatherer(results)
00295 counter_isValid+=1;
00296
00297 gatherer()
00298 finally:
00299 for b,enable in bodies:
00300 b.Enable(enable)
00301 statesaver = None
00302
00303 print 'grasping finished in %fs'%(time.time()-starttime)
00304 return counter_isValid
00305
00306
00307 def consumer(self, gmodel, doc, counter, graspingnoise, forceclosure, forceclosurethreshold, checkgraspfn, approachray, roll, preshape, standoff, manipulatordirection):
00308
00309 grasp = zeros(gmodel.totaldof)
00310 grasp[gmodel.graspindices.get('igrasppos')] = approachray[0:3]
00311 grasp[gmodel.graspindices.get('igraspdir')] = -approachray[3:6]
00312 grasp[gmodel.graspindices.get('igrasproll')] = roll
00313 grasp[gmodel.graspindices.get('igraspstandoff')] = standoff
00314 grasp[gmodel.graspindices.get('igrasppreshape')] = preshape
00315 grasp[gmodel.graspindices.get('imanipulatordirection')] = manipulatordirection
00316
00317 try:
00318 contacts,finalconfig,mindist,volume = gmodel.testGrasp(grasp=grasp,graspingnoise=graspingnoise,translate=True,forceclosure=forceclosure,forceclosurethreshold=forceclosurethreshold)
00319
00320 (err, values) = self.graspingutils.joint_filter(finalconfig[0]);
00321 if not err:
00322 return None
00323 except:
00324 print 'Grasp Failed: '
00325 print_exc(e)
00326 return None
00327
00328 Tlocalgrasp = eye(4)
00329 with self.robot:
00330 self.robot.SetTransform(finalconfig[1])
00331 Tgrasp = gmodel.manip.GetEndEffectorTransform()
00332 Tlocalgrasp = dot(linalg.inv(gmodel.target.GetTransform()),Tgrasp)
00333
00334 gmodel.setPreshape(grasp)
00335 direction = gmodel.getGlobalApproachDir(grasp)
00336 Tgrasp_nocol = array(Tgrasp)
00337 while gmodel.manip.CheckEndEffectorCollision(Tgrasp_nocol):
00338 Tgrasp_nocol[0:3,3] -= direction*gmodel.collision_escape_offset
00339 Tlocalgrasp_nocol = dot(linalg.inv(gmodel.target.GetTransform()),Tgrasp_nocol)
00340 self.robot.SetDOFValues(finalconfig[0])
00341 if self.env.GetViewer() is not None:
00342 gmodel.contactgraph = gmodel.drawContacts(contacts) if len(contacts) > 0 else None
00343 self.env.UpdatePublishedBodies()
00344 grasp[gmodel.graspindices.get('igrasptrans')] = reshape(transpose(Tlocalgrasp[0:3,0:4]),12)
00345 grasp[gmodel.graspindices.get('grasptrans_nocol')] = reshape(transpose(Tlocalgrasp_nocol[0:3,0:4]),12)
00346 grasp[gmodel.graspindices.get('forceclosure')] = mindist if mindist is not None else 0
00347 self.robot.SetTransform(self.robot.GetTransform())
00348 if not forceclosure or mindist >= forceclosurethreshold:
00349 grasp[gmodel.graspindices.get('performance')] = gmodel._ComputeGraspPerformance(grasp, graspingnoise=graspingnoise,translate=True,forceclosure=False)
00350
00351 if checkgraspfn is None or gmodel.checkgraspfn(contacts,finalconfig,grasp,{'mindist':mindist,'volume':volume}):
00352
00353 if self.add_valid_grasp_to_file(doc, counter, values):
00354 return grasp
00355 else:
00356 return None
00357
00358 return None
00359
00360
00361 def generate_grasp_file(self, object_id):
00362
00363 gmodel = self.init_env(object_id);
00364 if gmodel < 0:
00365 return gmodel
00366
00367 doc = Document()
00368 GraspList = doc.createElement("GraspList")
00369 doc.appendChild(GraspList)
00370
00371 obj_id = doc.createElement("object_id")
00372 GraspList.appendChild(obj_id)
00373 obj_id_text = doc.createTextNode(str(object_id))
00374 obj_id.appendChild(obj_id_text)
00375
00376 hand_type = doc.createElement("hand_type")
00377 GraspList.appendChild(hand_type)
00378 hand_type_text = doc.createTextNode("SDH")
00379 hand_type.appendChild(hand_type_text)
00380
00381 joint_names = doc.createElement("joint_names")
00382 GraspList.appendChild(joint_names)
00383 joint_names_text = doc.createTextNode("[sdh_knuckle_joint, sdh_thumb_2_joint, sdh_thumb_3_joint, sdh_finger_12_joint, sdh_finger_13_joint, sdh_finger_22_joint, sdh_finger_23_joint]")
00384 joint_names.appendChild(joint_names_text)
00385
00386 tip_link = doc.createElement("tip_link")
00387 GraspList.appendChild(tip_link)
00388 tip_link_text = doc.createTextNode("sdh_palm_link")
00389 tip_link.appendChild(tip_link_text)
00390
00391 cont = self.generate_grasps(gmodel, doc);
00392
00393 NumberOfGrasps = doc.createElement("NumberOfGrasps")
00394 GraspList.appendChild(NumberOfGrasps)
00395 NumberOfGrasps_text = doc.createTextNode(str(cont))
00396 NumberOfGrasps.appendChild(NumberOfGrasps_text)
00397
00398 uglyXml = doc.toprettyxml(indent=" ")
00399 text_re = re.compile('>\n\s+([^<>\s].*?)\n\s+</', re.DOTALL)
00400 prettyXml = text_re.sub('>\g<1></', uglyXml)
00401
00402 print str(cont)+" grasps have been added to the XML file."
00403 return prettyXml;
00404
00405
00406 def add_valid_grasp_to_file(self, doc, counter, values):
00407
00408 index = (self.robot.GetLink("sdh_palm_link")).GetIndex()
00409 matrix = (self.robot.GetLinkTransformations())[index]
00410 t = tf.transformations.translation_from_matrix(matrix)
00411 e = tf.transformations.euler_from_matrix(matrix, axes='sxyz')
00412
00413 category = self.graspingutils.get_grasping_direction(matrix)
00414 if category is GraspingErrorCodes.UNKNOWN_CATEGORY:
00415 return False;
00416
00417 tp = []
00418 tp = self.graspingutils.set_pregrasp(t, category, self.pregrasp_offset);
00419 if tp is GraspingErrorCodes.UNKNOWN_CATEGORY:
00420 return False;
00421
00422 GraspList = (doc.getElementsByTagName("GraspList"))[0]
00423 Grasp = doc.createElement("Grasp")
00424 Grasp.setAttribute("Index", str(counter))
00425 GraspList.appendChild(Grasp)
00426
00427 joint_values = doc.createElement("joint_values")
00428 Grasp.appendChild(joint_values)
00429 joint_values_text = doc.createTextNode(str(values))
00430 joint_values.appendChild(joint_values_text)
00431
00432 GraspPose = doc.createElement("GraspPose")
00433 Grasp.appendChild(GraspPose)
00434 Translation = doc.createElement("Translation")
00435 GraspPose.appendChild(Translation)
00436 Translation_text = doc.createTextNode("["+str(t[0])+", "+str(t[1])+", "+str(t[2])+"]")
00437 Translation.appendChild(Translation_text)
00438 Rotation = doc.createElement("Rotation")
00439 GraspPose.appendChild(Rotation)
00440 Rotation_text = doc.createTextNode("["+str(e[0])+", "+str(e[1])+", "+str(e[2])+"]")
00441 Rotation.appendChild(Rotation_text)
00442
00443 PreGraspPose = doc.createElement("PreGraspPose")
00444 Grasp.appendChild(PreGraspPose)
00445 Translation2 = doc.createElement("Translation")
00446 PreGraspPose.appendChild(Translation2)
00447 Translation2_text = doc.createTextNode("["+str(tp[0])+", "+str(tp[1])+", "+str(tp[2])+"]")
00448 Translation2.appendChild(Translation2_text)
00449 Rotation2 = doc.createElement("Rotation")
00450 PreGraspPose.appendChild(Rotation2)
00451 Rotation2_text = doc.createTextNode("["+str(e[0])+", "+str(e[1])+", "+str(e[2])+"]")
00452 Rotation2.appendChild(Rotation2_text)
00453
00454 cat = doc.createElement("category")
00455 Grasp.appendChild(cat)
00456 category_text = doc.createTextNode(category)
00457 cat.appendChild(category_text)
00458
00459 return True;