srs_grasping: srs_grasping: openraveutils.py Source File

00001 #!/usr/bin/python
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 #import multiprocessing
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 #multiprocessing.cpu_count()
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):           #Group of grasps (grasp of GraspConfiguration)
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):    #Individual grasp (grasp of GraspSubConfiguration)
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 ########################## NEW FUNCTIONS ########################################################
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])        #standoffs
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() # gather results
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                         # find a non-colliding transform
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()) # transform back to original position for checkgraspfn
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;