nxt_lxf2urdf: nxt_lxf2urdf: lxf2urdf.py Source File

00001 #! /usr/bin/env python
00002 
00003 """
00004 usage: %(progname)s file.lxf file.ldr
00005 """
00006 
00007 import os, sys, getopt
00008 import zipfile
00009 import xml.dom.minidom
00010 import numpy
00011 from numpy.linalg import inv
00012 
00013 import transformations as TF
00014 rigid_tree=[]
00015 brick_tree=[]
00016 
00017 def parseInts(s):
00018   if s:
00019     return [int(x) for x in s.split(',')]
00020   else:
00021     return None
00022 
00023 def parseFloats(s):
00024   if s:
00025     return [float(x) for x in s.split(',')]
00026   else:
00027     return None
00028 
00029 motor_template = """
00030     <link name="ref_%(refID)s_link">
00031       <inertial>
00032         <mass value="0.010000" />
00033         <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
00034         <origin xyz="0.0 0.0 0" />
00035         <inertia  ixx="0.01" ixy="0.0"  ixz="0.0"  iyy="0.01"  iyz="0.0"  izz="0.01" />
00036       </inertial>
00037       <visual>
00038         <!-- visual origin is defined w.r.t. link local coordinate system -->
00039         <origin xyz="0 0 0" rpy="0 0 0" />
00040         <geometry>
00041           <mesh filename="%(mesh)s" scale="%(m_scale)s %(m_scale)s %(m_scale)s"/>
00042         </geometry>
00043       </visual>
00044       <collision>
00045         <!-- collision origin is defined w.r.t. link local coordinate system -->
00046         <origin xyz="%(bound_x)s %(bound_y)s %(bound_z)s" rpy="%(bound_roll)s %(bound_pitch)s %(bound_yaw)s" />
00047         <geometry>
00048           <box size="%(dim_x)s %(dim_y)s %(dim_z)s"/>
00049         </geometry>
00050       </collision>
00051     </link>
00052 
00053     <link name="ref_%(refID)s_link_hub">
00054       <inertial>
00055         <mass value="0.010000" />
00056         <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
00057         <origin xyz="0.0 0.0 0" />
00058         <inertia  ixx="0.01" ixy="0.0"  ixz="0.0"  iyy="0.01"  iyz="0.0"  izz="0.01" />
00059       </inertial>
00060       <visual>
00061         <!-- visual origin is defined w.r.t. link local coordinate system -->
00062         <origin xyz="0 0 0" rpy="0 0 0" />
00063         <geometry>
00064           <mesh filename="package://nxt_description/meshes/parts/servo_hubs.dae" scale="%(m_scale)s %(m_scale)s %(m_scale)s"/>
00065         </geometry>
00066       </visual>
00067       <collision>
00068         <!-- collision origin is defined w.r.t. link local coordinate system -->
00069         <origin xyz="%(bound_x)s %(bound_y)s %(bound_z)s" rpy="%(bound_roll)s %(bound_pitch)s %(bound_yaw)s" />
00070         <geometry>
00071           <box size="%(dim_x)s %(dim_y)s %(dim_z)s"/>
00072         </geometry>
00073       </collision>
00074     </link>
00075 
00076     <!--THIS IS THE MOTOR JOINT RENAME AND FIX BY 180 WHEN NEEDED-->
00077     <joint name="mot_%(refID)s_joint" type="continuous">
00078       <parent link="ref_%(refID)s_link"/>
00079       <child link="ref_%(refID)s_link_hub"/>
00080       <origin xyz="0 0 0" rpy="0 0 0" />
00081       <axis xyz="1 0 0" />
00082     </joint>
00083 
00084 """
00085 
00086 link_template = """
00087     <link name="ref_%(refID)s_link">
00088       <inertial>
00089         <mass value="0.010000" />
00090         <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
00091         <origin xyz="0.0 0.0 0" />
00092         <inertia  ixx="0.01" ixy="0.0"  ixz="0.0"  iyy="0.01"  iyz="0.0"  izz="0.01" />
00093       </inertial>
00094       <visual>
00095         <!-- visual origin is defined w.r.t. link local coordinate system -->
00096         <origin xyz="0 0 0" rpy="%(rot_x)s %(rot_y)s %(rot_z)s" />
00097         <geometry>
00098           <mesh filename="%(mesh)s" scale="%(m_scale)s %(m_scale)s %(m_scale)s"/>
00099         </geometry>
00100       </visual>
00101       <collision>
00102         <!-- collision origin is defined w.r.t. link local coordinate system -->
00103         <origin xyz="%(bound_x)s %(bound_y)s %(bound_z)s" rpy="%(bound_roll)s %(bound_pitch)s %(bound_yaw)s" />
00104         <geometry>
00105           <box size="%(dim_x)s %(dim_y)s %(dim_z)s"/>
00106         </geometry>
00107       </collision>
00108     </link>
00109 """
00110 
00111 joint_template = """
00112     <joint name="ref_%(refID)s_joint" type="%(joint_type)s">
00113       <parent link="%(parent_link)s"/>
00114       <child link="%(child_link)s"/>
00115       <origin xyz="%(origin_x)s %(origin_y)s %(origin_z)s" rpy="%(origin_roll)s %(origin_pitch)s %(origin_yaw)s" />
00116       <axis xyz="%(axis_x)s %(axis_y)s %(axis_z)s" />
00117     </joint>
00118 """
00119 
00120 ultrasonic_link = """
00121   <!--THIS IS THE ULTRASONIC LINK RENAME ME-->
00122   <link name="ref_%(refID)s_link">
00123     <inertial>
00124       <mass value="0.023900" />
00125       <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
00126       <origin xyz="0 0 0" />
00127       <inertia  ixx="0.01" ixy="0.0"  ixz="0.0"  iyy="0.01"  iyz="0.0"  izz="0.01" />
00128     </inertial>
00129     <visual>
00130       <!-- visual origin is defined w.r.t. link local coordinate system -->
00131       <origin xyz=" -0.026 0 -0.018" rpy="0 0 1.57079633" />
00132       <geometry>
00133         <mesh filename="%(mesh)s" scale="%(m_scale)s %(m_scale)s %(m_scale)s"/>
00134       </geometry>
00135     </visual>
00136       <collision>
00137         <!-- collision origin is defined w.r.t. link local coordinate system -->
00138         <origin xyz="%(bound_x)s %(bound_y)s %(bound_z)s" rpy="%(bound_roll)s %(bound_pitch)s %(bound_yaw)s" />
00139         <geometry>
00140           <box size="%(dim_x)s %(dim_y)s %(dim_z)s"/>
00141         </geometry>
00142       </collision>
00143   </link>
00144 """
00145 
00146 generic_sensor_link = """
00147   <!--THIS IS THE SENSOR LINK RENAME ME-->
00148   <link name="ref_%(refID)s_link">
00149     <inertial>
00150       <mass value="0.023900" />
00151       <!-- center of mass (com) is defined w.r.t. link local coordinate system -->
00152       <origin xyz="0 0 0" />
00153       <inertia  ixx="0.01" ixy="0.0"  ixz="0.0"  iyy="0.01"  iyz="0.0"  izz="0.01" />
00154     </inertial>
00155     <visual>
00156       <!-- visual origin is defined w.r.t. link local coordinate system -->
00157       <origin xyz=" -0.013 0 -0.018" rpy="0 0 1.57079633" />
00158       <geometry>
00159         <mesh filename="%(mesh)s" scale="%(m_scale)s %(m_scale)s %(m_scale)s"/>
00160       </geometry>
00161     </visual>
00162       <collision>
00163         <!-- collision origin is defined w.r.t. link local coordinate system -->
00164         <origin xyz="%(bound_x)s %(bound_y)s %(bound_z)s" rpy="%(bound_roll)s %(bound_pitch)s %(bound_yaw)s" />
00165         <geometry>
00166           <box size="%(dim_x)s %(dim_y)s %(dim_z)s"/>
00167         </geometry>
00168       </collision>
00169   </link>
00170 """
00171 
00172 def parseLXFML(handle, lxffile,ldrfile):
00173   #begin urdf
00174 
00175   print "<!--this file was autogenerated from %s -->" % lxffile
00176   ldr_file = open('%s' % ldrfile,'r')
00177   print "<robot name=nxt_%s>" % lxffile.strip('.lxf').rsplit("/")[0]
00178   lxf_doc = xml.dom.minidom.parse(handle)
00179 
00180   bricks = {}
00181   parts = {}
00182   bones = {}
00183 
00184   for brick in lxf_doc.getElementsByTagName('Brick'):
00185     b = {}
00186     b['refID']= int(brick.getAttribute('refID'))
00187     b['parts'] = []
00188     b['designID'] = int(brick.getAttribute('designID'))
00189     b['handled']=False
00190     i = parseInts(brick.getAttribute('itemNos'))
00191     if i:
00192       b['itemNos'] = i
00193     bricks[int(brick.getAttribute('refID'))] = b
00194 
00195     for part in brick.getElementsByTagName('Part'):
00196       p = {}
00197       p['bones'] = []
00198       p['designID'] = part.getAttribute('designID')
00199       p['materials'] = parseInts(part.getAttribute('materials'))
00200       parts[int(part.getAttribute('refID'))] = p
00201       b['parts'].append(p)
00202       p['parent'] = b
00203 
00204       for bone in part.getElementsByTagName('Bone'):
00205         d = {}
00206         d['transformation'] = parseFloats(bone.getAttribute('transformation'))
00207         bones[int(bone.getAttribute('refID'))] = d
00208         p['bones'].append(d)
00209         d['parent'] = p
00210 
00211   rigids = {}
00212   for rigid in lxf_doc.getElementsByTagName('Rigid'):
00213     r = {}
00214     r['refID'] = int(rigid.getAttribute('refID'))
00215     r['transformation'] = parseFloats(rigid.getAttribute('transformation'))
00216     r['boneRefs'] = parseInts(rigid.getAttribute('boneRefs'))
00217     r['handled'] = False
00218     rigids[r['refID']] = r
00219 
00220   joints = []
00221   for joint in lxf_doc.getElementsByTagName('Joint'):
00222     j = []
00223     for rigid_ref in joint.getElementsByTagName('RigidRef'):
00224       r = {}
00225       r['rigidRef'] = int(rigid_ref.getAttribute('rigidRef'))
00226       r['a'] = parseFloats(rigid_ref.getAttribute('a'))
00227       r['z'] = parseFloats(rigid_ref.getAttribute('z'))
00228       r['t'] = parseFloats(rigid_ref.getAttribute('t'))
00229       j.append(r)
00230     joints.append(j)
00231 
00232 
00233   ldr_trans={}
00234   count=0
00235   for line in ldr_file:
00236     if line.startswith('1'):
00237       ldr= [x for x in line.split(' ')]
00238       l={}
00239       l['ldraw'] = ldr[14].strip('.dat\r\n')
00240       l['transformation'] = [float(ldr[2]), float(ldr[3]), float(ldr[4]), float(ldr[5]), float(ldr[6]), float(ldr[7]), float(ldr[8]), float(ldr[9]), float(ldr[10]), float(ldr[11]), float(ldr[12]), float(ldr[13])]
00241       ldr_trans[count]=l
00242       count=count+1
00243 
00244   for refID in sorted(bricks.keys()):
00245     designID = ldr_trans[refID]['ldraw']
00246     ldrID =ldr_trans[refID]['ldraw']
00247     rot_x=rot_y=rot_z=0
00248     #these parts with odd bends need a 180 rotation
00249     if ldrID == '6629' or  ldrID == '32348' or ldrID == '32140' or ldrID == '32526':
00250       rot_y = 3.14159
00251     #scaling factor from the .dat models to meters
00252     scale =0.0004
00253     d = {
00254       'refID' : refID,
00255       'mesh' : "package://nxt_description/meshes/parts/%s.dae" % designID,
00256       'bound_x' : 0,
00257       'bound_y' : 0,
00258       'bound_z' : 0,
00259       'm_scale' :' %s' % str(scale),
00260       'bound_roll' : 0,
00261       'bound_pitch' : 0,
00262       'bound_yaw' : 0,
00263       'dim_x' : 0,
00264       'dim_y' : 0,
00265       'dim_z' : 0,
00266       'rot_x': '%s' % str(rot_x),
00267       'rot_y': '%s' % str(rot_y),
00268       'rot_z': '%s' % str(rot_z),
00269     }
00270     #special templates for sensors and motors
00271     if ldr_trans[refID]['ldraw'] == '53787':
00272       print motor_template % d
00273     elif ldr_trans[refID]['ldraw'] == '53792':
00274       print ultrasonic_link % d
00275     elif ldr_trans[refID]['ldraw'] == '1044' or ldr_trans[refID]['ldraw'] == '64892' or  ldr_trans[refID]['ldraw'] == '53793':
00276       print generic_sensor_link % d
00277     else:
00278       print link_template % d
00279   #create tree with no loops 
00280   create_rigid_tree(0, joints, rigids)
00281   create_brick_tree(rigids, bricks, bones)
00282   #for refID, joint_list in enumerate(joints):
00283   for refID, branch in enumerate(brick_tree):
00284     joint_type = "fixed"
00285     child_refID = branch[1]
00286     parent_refID = branch[0] 
00287     #all units are in CM
00288     #the models are in LDU which are 0.4mm
00289     #let's compute some transforms
00290     world_to_c = homogeneous_matrix(ldr_trans[child_refID]['transformation'])
00291     world_to_p = homogeneous_matrix(ldr_trans[parent_refID]['transformation'])
00292     #now let's get the stuff for the URDF
00293     p_to_c = world_to_p.I*world_to_c
00294     rpy = TF.euler_from_matrix(p_to_c, 'sxzy')
00295       
00296     d = {
00297     'refID' : refID,
00298     'joint_type' : joint_type,
00299     'parent_link' : 'ref_%s_link' % parent_refID,
00300     'child_link' : 'ref_%s_link' % child_refID,
00301     #a straight transform doesn't seem to work properly (sign changes as needed)
00302     'origin_x' : '%s' % str(-1*float(p_to_c[0,3])*scale),
00303     'origin_y' : '%s'% str(1*float(p_to_c[2,3])*scale),
00304     'origin_z' : '%s' % str(-1*float(p_to_c[1,3])*scale),
00305     'origin_roll' : '%s' % str(1*rpy[0]),
00306     'origin_pitch' : '%s' % str(-1*rpy[1]),
00307     'origin_yaw' : '%s' % str(1*rpy[2]),
00308     'axis_x' : 0, 'axis_y' : 0, 'axis_z' : 0,
00309     }
00310     print joint_template % d
00311   #end urdf 
00312   print "</robot>"
00313 
00314 def create_rigid_tree(refID, joints, rigids):
00315   if not rigids[refID]['handled']:
00316     rigids[refID]['handled']=True
00317     parent =refID
00318     for joint_list in joints:
00319       if joint_list[0]['rigidRef'] == parent:
00320         if not rigids[joint_list[1]['rigidRef']]['handled']:
00321           rigid_tree.append([parent, joint_list[1]['rigidRef']])
00322           create_rigid_tree(joint_list[1]['rigidRef'], joints, rigids)
00323       if joint_list[1]['rigidRef'] == parent:
00324         if not rigids[joint_list[0]['rigidRef']]['handled']:
00325           rigid_tree.append([parent, joint_list[0]['rigidRef']])
00326           create_rigid_tree(joint_list[0]['rigidRef'], joints, rigids)
00327 
00328 def create_brick_tree(rigids, bricks, bones):
00329   for branch in rigid_tree:
00330     parent_brick = bones[rigids[branch[0]]['boneRefs'][0]]['parent']['parent']['refID']
00331     for bone in rigids[branch[1]]['boneRefs']:
00332       child_brick = bones[bone]['parent']['parent']['refID']
00333       if not bricks[child_brick]['handled']:
00334         brick_tree.append([parent_brick,child_brick])
00335         bricks[child_brick]['handled']=True
00336 
00337 def homogeneous_matrix(transform):
00338   tmp = numpy.ones((4,4))
00339   tmp[:3,:3] = (numpy.array(transform[3:]).reshape(3,3))
00340   tmp[0,3] = numpy.transpose(numpy.array(transform[0]))
00341   tmp[1,3] = numpy.transpose(numpy.array(transform[1]))
00342   tmp[2,3] = numpy.transpose(numpy.array(transform[2]))
00343   tmp[3,:3] = numpy.zeros((1,3))
00344   return numpy.matrix(tmp)
00345 
00346 def handleLXF(lxffile,ldrfile):
00347   z = zipfile.ZipFile(lxffile)
00348   for item in z.namelist():
00349     if item.endswith('.LXFML'):
00350       f = z.open(item)
00351       parseLXFML(f,lxffile,ldrfile)
00352 
00353 def usage(progname):
00354   print __doc__ % vars()
00355 
00356 def main(argv, stdout, environ):
00357   progname = argv[0]
00358   optlist, args = getopt.getopt(argv[1:], "", ["help", "debug"])
00359 
00360   if len(args) == 0:
00361     usage(progname)
00362     return
00363   
00364   for (field, val) in optlist:
00365     if field == "--help":
00366       usage(progname)
00367       return
00368     elif field == "--debug":
00369       debugfull()
00370   try:
00371      handleLXF(args[0], args[1])
00372   except Exception, reason:
00373      print >> sys.stderr, "Unable to handle '%s': %s" % (args[0], reason)
00374 
00375 if __name__ == "__main__":
00376   main(sys.argv, sys.stdout, os.environ)