Go to the documentation of this file.00001
00002
00003 from xml.dom import minidom
00004
00005 arm_joint_names = ['lr_shoulder_pan_joint',
00006 'lr_shoulder_lift_joint',
00007 'lr_upper_arm_roll_joint',
00008 'lr_elbow_flex_joint',
00009 'lr_forearm_roll_joint',
00010 'lr_wrist_flex_joint',
00011 'lr_wrist_roll_joint']
00012
00013 l_arm_joint_names = map(lambda s: s.replace('lr_', 'l_'), arm_joint_names )
00014 r_arm_joint_names = map(lambda s: s.replace('lr_', 'r_'), arm_joint_names )
00015
00016 urdf_fname = '../config/pr2.urdf'
00017 yaml_fname = '../config/r_arm_joint_limits.yaml'
00018 joint_names = r_arm_joint_names
00019
00020 print( 'parsing %s ...'%urdf_fname )
00021 xmldoc = minidom.parse( urdf_fname )
00022 joint_nodes = xmldoc.getElementsByTagName('joint')
00023 print( 'found %d joints'%len( joint_nodes ) )
00024
00025 print( 'writin to %s '%yaml_fname )
00026 yaml_file = open( yaml_fname, 'w')
00027
00028 for node in joint_nodes:
00029 if not node.hasAttribute( 'name' ):
00030 continue
00031 joint_name = node.getAttribute( 'name' )
00032 if joint_name not in joint_names:
00033 continue
00034 print( 'found joint %s'%joint_name )
00035
00036 limit_nodes = node.getElementsByTagName( 'limit' )
00037 if len( limit_nodes ) == 0:
00038 print( '\t no limit tag -> ignored' )
00039 continue
00040
00041 if len( limit_nodes ) > 1:
00042 print( '\t more than one limit tag -> ignored' )
00043 continue
00044
00045 effort = None
00046 velocity = None
00047 upper = None
00048 lower = None
00049
00050 limit_node = limit_nodes[0]
00051 if limit_node.hasAttribute( 'effort' ):
00052 effort = limit_node.getAttribute( 'effort' )
00053 if limit_node.hasAttribute( 'velocity' ):
00054 velocity = limit_node.getAttribute( 'velocity' )
00055 if limit_node.hasAttribute( 'upper' ):
00056 upper = limit_node.getAttribute( 'upper' )
00057 if limit_node.hasAttribute( 'lower' ):
00058 lower = limit_node.getAttribute( 'lower' )
00059
00060 safety_nodes = node.getElementsByTagName( 'safety_controller' )
00061 if len( safety_nodes ) == 0:
00062 print( '\t no safety_controller tag -> ignored' )
00063 continue
00064
00065 if len( safety_nodes ) > 1:
00066 print( '\t more than one safety_controller tag -> ignored' )
00067 continue
00068
00069 soft_lower_limit = None
00070 soft_upper_limit = None
00071
00072 safety_node = safety_nodes[0]
00073 if safety_node.hasAttribute( 'soft_upper_limit' ):
00074 soft_upper_limit = safety_node.getAttribute( 'soft_upper_limit' )
00075 if safety_node.hasAttribute( 'soft_lower_limit' ):
00076 soft_lower_limit = safety_node.getAttribute( 'soft_lower_limit' )
00077
00078 print( '%s:\n\teffort: %s\n\tvelocity: %s\n\tupper: %s\n\tlower: %s\n\tsoft_upper: %s\n\tsoft_lower: %s'%( joint_name, effort, velocity, upper, lower, soft_upper_limit, soft_lower_limit ) )
00079
00080 yaml_file.write( '%s:\n'%joint_name )
00081 if not effort is None:
00082 yaml_file.write( ' effort: %s\n'%effort )
00083 if not velocity is None:
00084 yaml_file.write( ' velocity: %s\n'%velocity )
00085 if not upper is None:
00086 yaml_file.write( ' upper: %s\n'%upper )
00087 if not lower is None:
00088 yaml_file.write( ' lower: %s\n'%lower )
00089 if not soft_upper_limit is None:
00090 yaml_file.write( ' soft_upper_limit: %s\n'%soft_upper_limit )
00091 if not soft_lower_limit is None:
00092 yaml_file.write( ' soft_lower_limit: %s\n'%soft_lower_limit )
00093
00094 yaml_file.close()
00095
00096
00097
00098
00099
00100