#NEWLINE# def add_urdf_elements( self , node , prefix ) : #NEWLINE# #INDENT# full_prefix = ( ( prefix + '::' ) if prefix else '' ) #NEWLINE# jointnode = ET . SubElement ( node , 'joint' , { #NEWLINE# #INDENT# 'name' : sdf2tfname ( ( full_prefix + self . name ) ) , #NEWLINE# #UNINDENT# } ) #NEWLINE# parentnode = ET . SubElement ( jointnode , 'parent' , { #NEWLINE# #INDENT# 'link' : sdf2tfname ( ( full_prefix + self . parent ) ) , #NEWLINE# #UNINDENT# } ) #NEWLINE# childnode = ET . SubElement ( jointnode , 'child' , { #NEWLINE# #INDENT# 'link' : sdf2tfname ( ( full_prefix + self . child ) ) , #NEWLINE# #UNINDENT# } ) #NEWLINE# parent_pose_world = ( self . tree_parent_link . tree_parent_joint . pose_world if self . tree_parent_link . tree_parent_joint else transformations . identity_matrix ( ) ) #NEWLINE# if ( self . tree_parent_link . parent_model == self . parent_model ) : #NEWLINE# #INDENT# pose2origin ( jointnode , transformations . concatenate_matrices ( inverse_matrix ( parent_pose_world ) , self . pose_world ) ) #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# pose2origin ( jointnode , transformations . concatenate_matrices ( inverse_matrix ( parent_pose_world ) , self . tree_child_link . pose_world ) ) #NEWLINE# #UNINDENT# if ( ( self . type == 'revolute' ) and ( self . axis . lower_limit == 0 ) and ( self . axis . upper_limit == 0 ) ) : #NEWLINE# #INDENT# jointnode . attrib [ 'type' ] = 'fixed' #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# if ( self . type == 'universal' ) : #NEWLINE# #INDENT# jointnode . attrib [ 'type' ] = 'revolute' #NEWLINE# dummylinknode = ET . SubElement ( node , 'link' , { #NEWLINE# #INDENT# 'name' : sdf2tfname ( ( ( full_prefix + self . name ) + '::revolute_dummy_link' ) ) , #NEWLINE# #UNINDENT# } ) #NEWLINE# childnode . attrib [ 'link' ] = dummylinknode . attrib [ 'name' ] #NEWLINE# dummyjointnode = ET . SubElement ( node , 'joint' , { #NEWLINE# #INDENT# 'name' : sdf2tfname ( ( ( full_prefix + self . name ) + '::revolute_dummy_joint' ) ) , #NEWLINE# #UNINDENT# } ) #NEWLINE# ET . SubElement ( dummyjointnode , 'parent' , { #NEWLINE# #INDENT# 'link' : dummylinknode . attrib [ 'name' ] , #NEWLINE# #UNINDENT# } ) #NEWLINE# ET . SubElement ( dummyjointnode , 'child' , { #NEWLINE# #INDENT# 'link' : sdf2tfname ( ( full_prefix + self . child ) ) , #NEWLINE# #UNINDENT# } ) #NEWLINE# dummyjointnode . attrib [ 'type' ] = 'revolute' #NEWLINE# self . axis2 . add_urdf_elements ( dummyjointnode , transformations . concatenate_matrices ( inverse_matrix ( self . pose_world ) , self . parent_model . pose_world ) ) #NEWLINE# #UNINDENT# else : #NEWLINE# #INDENT# jointnode . attrib [ 'type' ] = self . type #NEWLINE# #UNINDENT# #UNINDENT# self . axis . add_urdf_elements ( jointnode , transformations . concatenate_matrices ( inverse_matrix ( self . pose_world ) , self . parent_model . pose_world ) )