hrl_common_code_darpa_m3: robot_config_to

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import xml.etree.cElementTree as ET
00004 
00005 def origin_tag(x,y,z,roll,pitch,yaw):
00006     return ET.Element('origin',
00007                       xyz=' '.join((str(x),str(y),str(z))),
00008                       rpy=' '.join((str(roll),str(pitch),str(yaw))))
00009 
00010 def mass_tag(mass):
00011     return ET.Element('mass', value=str(mass))
00012 
00013 def inertia_tag(i):
00014     print "WARNING: Inertia tag not using robot definition"
00015     return ET.Element('inertia', ixx='1.0', ixy='0.0', ixz='0.0',
00016                                  iyy='1.0', iyz='0.0', izz='1.0')
00017 
00018 def inertial_tag(i):
00019     it = ET.Element('inertial')
00020     it.append(origin_tag(0,0,0,0,0,0))
00021     #TODO: use robot definition for inertial tag
00022     print "WARNING: Inertial origin not using robot definition"
00023     it.append(mass_tag(d_robot.bod_mass[i]))
00024     it.append(inertia_tag(i))
00025     return it
00026 
00027 def cylinder_tag(i):
00028     dims = d_robot.bod_dimensions[i]
00029     r = str(dims[0])
00030     l = str(dims[1])
00031     return ET.Element('cylinder', radius=r, length=l)
00032 
00033 def sphere_tag(i):
00034     r = d_robot.bod_dimensions[i][0]
00035     return ET.Element('sphere', radius=str(r))
00036 
00037 def mesh_tag(i):
00038     print "WARNING: Using Mesh tag.  Scale not implemented"
00039     #TODO: Implement scale
00040     filename = d_robot.bod_dimensions[i][0]
00041     return ET.Element('mesh', filename=filename)
00042 
00043 def box_tag(i):
00044     dims = d_robot.bod_dimensions[i]
00045     x = str(dims[0])
00046     y = str(dims[1])
00047     z = str(dims[2])
00048     return ET.Element('box', size=' '.join((x,y,z))) 
00049 
00050 def geometry_tag(i):
00051     gt = ET.Element('geometry')
00052     shape = d_robot.bod_shapes[i]
00053     if shape == 'capsule':
00054         print "WARNING: Cannot handle capsule shape accurately, using box instead"
00055         shape_el = box_tag(i)
00056     if shape == 'cube':
00057         shape_el = box_tag(i)
00058     if shape == 'cylinder':
00059         shape_el = cylinder_tag(i)
00060     if shape == 'sphere':
00061         shape_el = sphere_tag(i)
00062     if shape == 'mesh':
00063         shape_el = mesh_tag(i)
00064     gt.append(shape_el)
00065     return gt
00066 
00067 def color_tag(i):
00068     colors = d_robot.bod_color[i]
00069     r = str(colors[0])
00070     g = str(colors[1])
00071     b = str(colors[2])
00072     a = str(colors[3])
00073     return ET.Element('color', rgba=' '.join((r,g,b,a)))
00074 
00075 def material_tag(i):
00076     mt = ET.Element('material', name="_".join(('material',str(i))))
00077     mt.append(color_tag(i))
00078     return mt
00079 
00080 def visual_tag(i):
00081     vt = ET.Element('visual')
00082     vt.append(origin_tag(0,0,0,0,0,0))
00083     vt.append(geometry_tag(i))
00084     vt.append(material_tag(i))
00085     return vt
00086 
00087 def collision_tag(i):
00088     ct = ET.Element('collision')
00089     ct.append(origin_tag(0,0,0,0,0,0))
00090     ct.append(geometry_tag(i))
00091     return ct
00092 
00093 def create_link(i):
00094     link = ET.Element('link', name=''.join((d_robot.bod_names[i],'_link')))
00095     link.append(inertial_tag(i))
00096     link.append(visual_tag(i))
00097     link.append(collision_tag(i))
00098     return link
00099 
00100 def parent_tag(link_name):
00101     return ET.Element('parent', link=link_name)
00102 
00103 def child_tag(link_name):
00104     return ET.Element('child', link=link_name)
00105 
00106 def joint_connection(i):
00107     conn = d_robot.b_jt_attach[i]
00108     parent_num = conn[1]
00109     child_num = conn[0]
00110     if parent_num < 0.:
00111         par_tag = parent_tag('root_link')
00112     else:
00113         par_tag = parent_tag(''.join((d_robot.bod_names[parent_num],'_link')))
00114     ch_tag = child_tag(''.join((d_robot.bod_names[child_num], '_link')))
00115     return par_tag, ch_tag
00116 
00117 def axis_tag(i):
00118     axes = d_robot.b_jt_axis[i]
00119     x = str(axes[0])
00120     y = str(axes[1])
00121     z = str(axes[2])
00122     return ET.Element('axis', xyz=' '.join((x,y,z)))
00123 
00124 def limit_tag(i):
00125     min_ = d_robot.b_jt_limits_min[i]
00126     max_ = d_robot.b_jt_limits_max[i]
00127     effort = 1000.0
00128     velocity = 1000.0
00129     return ET.Element('limit', lower=str(min_), upper=str(max_),
00130                               effort=str(effort), velocity=str(velocity))
00131 
00132 def create_joint(i):
00133     joint = ET.Element('joint', name=''.join((d_robot.bod_names[i],'_joint')),
00134                                 type='revolute')
00135     trans = d_robot.b_jt_anchor[i]
00136     joint.append(origin_tag(trans[0], trans[1], trans[2], 0, 0, 0))
00137     parent_tag, child_tag = joint_connection(i)
00138     joint.append(parent_tag)
00139     joint.append(child_tag)
00140     joint.append(axis_tag(i))
00141     joint.append(limit_tag(i))
00142     return joint
00143 
00144 def root_link():
00145     return ET.Element('link', name='root_link')
00146 
00147 def robot_description_to_urdf(d_robot, filename):
00148     tree = ET.Element('robot', name=filename)
00149     tree.append(root_link())
00150     for i in xrange(len(d_robot.bod_names)):
00151         tree.append(create_joint(i))
00152         tree.append(create_link(i))
00153     return ET.ElementTree(tree)
00154 
00155 if __name__=='__main__':
00156     import roslib; roslib.load_manifest('hrl_common_code_darpa_m3')
00157     import hrl_common_code_darpa_m3.robot_config.multi_link_seven_planar as d_robot
00158     tree = robot_description_to_urdf(d_robot, 'six_link_planar')
00159     with open('urdf_test.xml','wb') as f:
00160        tree.write(f)