hrl_motion_planners_darpa_m3: write_xml

Go to the documentation of this file.
00001 
00002 #
00003 # helper functions to write an XML file that OpenRAVE can parse.
00004 #
00005 
00006 
00007 import os 
00008 import math 
00009 
00010 
00011 #convert list to string separated by sep (e.g ' ', or ',')
00012 def list_str(l, sep=' '):
00013     return ' '.join(map(lambda x: str(x), l))
00014 
00015 # translation, rotationaxis, diffuse_color are lists.
00016 def write_openrave_cylinder_KinBody(file_object, name, translation,
00017                         rotationaxis, radius, height, diffuse_color):
00018     translation = list_str(translation)
00019     rotationaxis = list_str(rotationaxis)
00020     diffuse_color = list_str(diffuse_color)
00021     radius = str(radius)
00022     height = str(height)
00023     file_object.write('\t <KinBody name="'+name+'"> \n'+
00024                 '\t \t <translation> '+translation+' </translation> \n'+
00025                 '\t \t <Body type="dynamic"> \n'+
00026                 '\t \t \t <Geom type="cylinder"> \n'+
00027                 '\t \t \t \t <rotationaxis> ' + rotationaxis + ' </rotationaxis> \n'+
00028                 '\t \t \t \t <radius> ' + radius + ' </radius> \n'+
00029                 '\t \t \t \t <height> ' + height + ' </height> \n'+
00030                 '\t \t \t \t <diffuseColor> ' + diffuse_color + ' </diffuseColor> \n'+
00031                 '\t \t \t </Geom> \n'+
00032                 '\t \t </Body> \n'+
00033                 '\t </KinBody>\n\n')
00034 
00035 # translation, rotationaxis, diffuse_color are lists.
00036 def write_openrave_box_KinBody(file_object, name, translation,
00037                                extent, diffuse_color):
00038     translation = list_str(translation)
00039     extent = list_str(extent)
00040     diffuse_color = list_str(diffuse_color)
00041     file_object.write('\t <KinBody name="'+name+'"> \n'+
00042                 '\t \t <translation> '+translation+' </translation> \n'+
00043                 '\t \t <Body type="dynamic"> \n'+
00044                 '\t \t \t <Geom type="box"> \n'+
00045                 '\t \t \t \t <Extents>' + extent + '</Extents> \n'+
00046                 '\t \t \t \t <diffuseColor> ' + diffuse_color + ' </diffuseColor> \n'+
00047                 '\t \t \t </Geom> \n'+
00048                 '\t \t </Body> \n'+
00049                 '\t </KinBody>\n\n')
00050 
00051 def environment_start(file_object):
00052     file_object.write('<Environment> \n\n')
00053 
00054 def environment_end(file_object):
00055     file_object.write('</Environment> \n\n')
00056 
00057 def write_robot(file_object, robot_file, effector, base,
00058                   translation, rotationaxis):
00059     translation = list_str(translation)
00060     rotationaxis = list_str(rotationaxis)
00061     file_object.write('\t <Robot file="'+robot_file+'"> \n' +
00062                 '\t \t <Manipulator> \n' +
00063                 '\t \t \t <effector> ' + effector + '</effector> \n'+
00064                 '\t \t \t <base> ' + base + '</base> \n'+
00065                 '\t\t\t <!-- grasp goal with respect to the effector-->\n' +
00066                 '\t \t \t <Translation> ' + translation + '</Translation> \n'+
00067                 '\t \t \t <RotationAxis> ' + rotationaxis + '</RotationAxis> \n'+
00068                 '\t \t </Manipulator> \n'+
00069                 '\t </Robot> \n\n')
00070 
00071 def write_linkage_xml_file(b_jts, bodies, fname):
00072     f_xml = open(fname, 'w')
00073     f_xml.write('<!--THIS FILE IS AUTO-GENERATED, please do not mess with it directly -->\n'+
00074                 '<!--modify the file referred to in planar_openrave.py instead -->\n'+
00075                 '<Robot name="3DOFRobot"> \n'+
00076                 '\t<KinBody> \n'+
00077                 '\t\t<!-- Create the base body, it should never move--> \n'+
00078                 '\t\t<!-- Note that all translations and rotations are with respect to this base-->\n'+
00079                 '\t\t<!-- For example, the robot at the identity transformation is equivalent to the identity transformation of the first body.-->\n'+
00080                 '\t\t<Body name="Base" type="dynamic"> \n'+
00081                 '\t\t\t<Translation>0.0  0.0  0.0</Translation> \n'+
00082                 '\t\t\t<Geom type="cylinder"> \n'+
00083                 '\t\t\t\t<rotationaxis>1 0 0 90</rotationaxis> \n'+
00084                 '\t\t\t\t<radius>0.03</radius> \n'+
00085                 '\t\t\t\t<height>0.04</height> \n'+
00086                 '\t\t\t\t<diffuseColor>0 0 1</diffuseColor> \n'+
00087                 '\t\t\t</Geom> \n'+
00088                 '\t\t</Body> \n')
00089 
00090     for i in xrange(bodies['num_links']):
00091         if i == 0:
00092             name_prev = "Base"
00093             trans1 = '0 0 0'
00094         else:
00095             name_prev = bodies['name'][i-1]
00096             trans1 = str(b_jts['anchor'][i][0]-b_jts['anchor'][i-1][0])+' '+str(b_jts['anchor'][i][1]-b_jts['anchor'][i-1][1])+' '+str(b_jts['anchor'][i][2]-b_jts['anchor'][i-1][2])
00097 
00098         trans2 = '0 '+str(-bodies['dim'][i][2]/2.0)+' 0'
00099 
00100         trans_sphere = '0 '+str(-bodies['dim'][i][2])+' 0'
00101         radius = str(bodies['dim'][i][1]/2.0)
00102 
00103         extent = str(bodies['dim'][i][1]/2.0)+' '+str(bodies['dim'][i][2]/2.0)+' '+str(bodies['dim'][i][0]/2.0)
00104         name = bodies['name'][i]
00105         axis = str(b_jts['axis'][i][0])+' '+str(b_jts['axis'][i][1])+' '+str(b_jts['axis'][i][2])
00106 
00107         jt_limits = str(math.degrees(b_jts['jt_lim_min'][i]))+' '+str(math.degrees(b_jts['jt_lim_max'][i]))
00108         color = str(bodies['color'][i][0])+' '+str(bodies['color'][i][1])+' '+str(bodies['color'][i][2])
00109         f_xml.write('\t\t<!-- the second movable link--> \n'+
00110                     '\t\t<Body name="'+name+'" type="dynamic"> \n'+
00111                     '\t\t\t<offsetfrom>'+name_prev+'</offsetfrom> \n'+
00112                     '\t\t\t<Translation>'+trans1+'</Translation> \n'+
00113 
00114                     '\t\t\t<Geom type="box"> \n'+
00115                     '\t\t\t\t<Translation>'+trans2+'</Translation> \n'+
00116                     '\t\t\t\t<Extents>'+extent+'</Extents> \n'+
00117                     '\t\t\t\t<diffuseColor>'+color+'</diffuseColor> \n'+
00118                     '\t\t\t</Geom> \n'+
00119 
00120                     '\t\t\t<Geom type="sphere"> \n'+
00121                     '\t\t\t\t<Translation>'+trans_sphere+'</Translation> \n'+
00122                     '\t\t\t\t<Radius>'+radius+'</Radius> \n'+
00123                     '\t\t\t\t<diffuseColor>'+color+'</diffuseColor> \n'+
00124                     '\t\t\t</Geom> \n'+
00125 
00126                     '\t\t</Body> \n'+
00127                     '\t\t<!-- declare a circular hinge joint (circular joints have no limits) --> \n'+
00128                     '\t\t<Joint circular="false" name="'+name+'" type="hinge"> \n'+
00129                     '\t\t\t<Body>'+name_prev+'</Body> \n'+
00130                     '\t\t\t<Body>'+name+'</Body> \n'+
00131                     '\t\t\t<offsetfrom>'+name+'</offsetfrom> \n'+
00132                     '\t\t\t<weight>4</weight> \n'+
00133                     '\t\t\t<limitsdeg>'+jt_limits+'</limitsdeg> \n'+
00134                     '\t\t\t<axis>'+axis+'</axis> \n'+
00135                     '\t\t\t<maxvel>4</maxvel> \n'+
00136                     '\t\t\t<resolution>1</resolution> \n'+
00137                     '\t\t</Joint> \n\n')
00138 
00139     trans = '0 '+str(-1*bodies['dim'][-1][2])+' 0'
00140     f_xml.write('\t\t<!-- set the transparency of every geometry in the KinBody--> \n'+
00141                 '\t\t<transparency>0.01</transparency> \n'+
00142                 '\t</KinBody> \n\n\n'+
00143                 
00144                 '\t<!-- Specifying the manipulator structure--> \n'+
00145                 '\t<Manipulator name="arm"> \n'+
00146                 '\t\t<effector>'+bodies['name'][-1]+'</effector>   <!-- last link where end effector is attached--> \n'+
00147                 '\t\t<base>Base</base>           <!-- base link--> \n'+
00148                 '\t\t<!-- grasp goal with respect to the effector--> \n'+
00149                 '\t\t<Translation>'+trans+'</Translation> \n'+
00150                 '\t\t<RotationAxis>1 0 0 0</RotationAxis> \n'+
00151                 '\t</Manipulator> \n'+
00152                 '</Robot>')
00153     f_xml.close()
00154     print "wrote new sim.robot.xml file"
00155 
00156 def write_environment_xml_file(d, fname, robot_xml):
00157     f_xml = open(fname, 'w')
00158 
00159     f_xml.write('<Environment> \n'+
00160                 '\t <Robot name="3DOFRobot" file="'+robot_xml+'"> \n'+
00161                 '\t </Robot> \n')
00162 
00163     for i in xrange(d['num_move_used']):
00164         trans = [2.0, 0., 0.0]
00165         name = "movable"+str(i+1).zfill(2)
00166         rotation = [1, 0, 0, 90]
00167         diffuse_color = [1, 1, 1]
00168         radius = d['moveable_dimen'][i][0]
00169         height = d['moveable_dimen'][i][2]
00170         write_openrave_cylinder_KinBody(f_xml, name, trans, rotation,
00171                                         radius, height, diffuse_color)
00172             
00173 
00174     for i in xrange(d['num_fixed_used']):        
00175         trans = [0.8, 0.6, 0.0]
00176         name = "fixed"+str(i+1).zfill(2)
00177         rotation = [1, 0, 0, 90]
00178         radius = d['fixed_dimen'][i][0]
00179         height = d['fixed_dimen'][i][2]
00180         diffuse_color = [1, 0.2, 0.2]
00181         write_openrave_cylinder_KinBody(f_xml, name, trans, rotation,
00182                                         radius, height, diffuse_color)
00183             
00184     trans = [0.9, 0.3, 0.05]
00185     name = 'target'
00186     rotation = [1, 0, 0, 90]
00187     radius = 0.02
00188     height = 0.01
00189     diffuse_color = [0, 1, 0]
00190     write_openrave_cylinder_KinBody(f_xml, name, trans, rotation,
00191                                     radius, height, diffuse_color)
00192 
00193     f_xml.write('</Environment>')
00194     f_xml.close()
00195     print "wrote new planar.environment.xml file"
00196 
00197 def write_cuboid_obstacles(file_obj, n_obs, init_x, extent):
00198     obstacle_nm_l = ['obstacle'+str(i) for i in range(1,n_obs+1)]
00199 
00200     # initial position of obstacles
00201     pos_list = [[(i+1) * init_x, 0. , 0.] for i in range(n_obs)]
00202     diffuse_color = [1, 0.2, 0.2]
00203 
00204     for i in range(n_obs):
00205         nm = obstacle_nm_l[i]
00206         trans = pos_list[i]
00207         write_openrave_box_KinBody(file_obj, nm, trans, extent,
00208                                    diffuse_color)
00209 
00210