示例#1
0
文件: pysdf.py 项目: wxmerkt/director
 def add_urdf_elements(self, node, prefix):
     full_prefix = prefix + '::' if prefix else ''
     jointnode = ET.SubElement(
         node, 'joint', {'name': sdf2tfname(full_prefix + self.name)})
     parentnode = ET.SubElement(
         jointnode, 'parent',
         {'link': sdf2tfname(full_prefix + self.parent)})
     childnode = ET.SubElement(
         jointnode, 'child', {'link': sdf2tfname(full_prefix + self.child)})
     # in SDF a joint's pose is given in child link frame, in URDF joint frame = child link frame, i.e. specifiy relative to parent joint (not parent link)
     parent_pose_world = self.tree_parent_link.tree_parent_joint.pose_world if self.tree_parent_link.tree_parent_joint else transformations.identity_matrix(
     )
     if self.tree_parent_link.parent_model == self.parent_model:
         pose2origin(
             jointnode,
             transformations.concatenate_matrices(
                 inverse_matrix(parent_pose_world), self.pose_world))
     else:  # joint crosses includes
         pose2origin(
             jointnode,
             transformations.concatenate_matrices(
                 inverse_matrix(parent_pose_world),
                 self.tree_child_link.pose_world))
     if self.type == 'revolute' and self.axis.lower_limit == 0 and self.axis.upper_limit == 0:
         jointnode.attrib['type'] = 'fixed'
     elif self.type == 'universal':
         # Simulate universal robot as
         # self.parent -> revolute joint (self) -> dummy link -> revolute joint -> self.child
         jointnode.attrib['type'] = 'revolute'
         dummylinknode = ET.SubElement(
             node, 'link', {
                 'name':
                 sdf2tfname(full_prefix + self.name +
                            '::revolute_dummy_link')
             })
         childnode.attrib['link'] = dummylinknode.attrib['name']
         dummyjointnode = ET.SubElement(
             node, 'joint', {
                 'name':
                 sdf2tfname(full_prefix + self.name +
                            '::revolute_dummy_joint')
             })
         ET.SubElement(dummyjointnode, 'parent',
                       {'link': dummylinknode.attrib['name']})
         ET.SubElement(dummyjointnode, 'child',
                       {'link': sdf2tfname(full_prefix + self.child)})
         dummyjointnode.attrib['type'] = 'revolute'
         self.axis2.add_urdf_elements(
             dummyjointnode,
             transformations.concatenate_matrices(
                 inverse_matrix(self.pose_world),
                 self.parent_model.pose_world))
     else:
         jointnode.attrib['type'] = self.type
     #print('self.pose_world\n', self.pose_world)
     #print('self.parent_model.pose_world\n', self.parent_model.pose_world)
     self.axis.add_urdf_elements(
         jointnode,
         transformations.concatenate_matrices(
             inverse_matrix(self.pose_world), self.parent_model.pose_world))
示例#2
0
 def add_urdf_elements(self, node, prefix):
   full_prefix = prefix + '::' if prefix else ''
   jointnode = ET.SubElement(node, 'joint', {'name': sdf2tfname(full_prefix + self.name)})
   parentnode = ET.SubElement(jointnode, 'parent', {'link': sdf2tfname(full_prefix + self.parent)})
   childnode = ET.SubElement(jointnode, 'child', {'link': sdf2tfname(full_prefix + self.child)})
   # in SDF a joint's pose is given in child link frame, in URDF joint frame = child link frame, i.e. specifiy relative to parent joint (not parent link)
   parent_pose_world = self.tree_parent_link.tree_parent_joint.pose_world if self.tree_parent_link.tree_parent_joint else transformations.identity_matrix()
   if self.tree_parent_link.parent_model == self.parent_model:
     pose2origin(jointnode, transformations.concatenate_matrices(inverse_matrix(parent_pose_world), self.pose_world))
   else: # joint crosses includes
     pose2origin(jointnode, transformations.concatenate_matrices(inverse_matrix(parent_pose_world), self.tree_child_link.pose_world))
   if self.type == 'revolute' and self.axis.lower_limit == 0 and self.axis.upper_limit == 0:
     jointnode.attrib['type'] = 'fixed'
   elif self.type == 'universal':
     # Simulate universal robot as
     # self.parent -> revolute joint (self) -> dummy link -> revolute joint -> self.child
     jointnode.attrib['type'] = 'revolute'
     dummylinknode = ET.SubElement(node, 'link', {'name': sdf2tfname(full_prefix + self.name + '::revolute_dummy_link')})
     childnode.attrib['link'] = dummylinknode.attrib['name']
     dummyjointnode = ET.SubElement(node, 'joint', {'name': sdf2tfname(full_prefix + self.name + '::revolute_dummy_joint')})
     ET.SubElement(dummyjointnode, 'parent', {'link': dummylinknode.attrib['name']})
     ET.SubElement(dummyjointnode, 'child', {'link': sdf2tfname(full_prefix + self.child)})
     dummyjointnode.attrib['type'] = 'revolute'
     self.axis2.add_urdf_elements(dummyjointnode, transformations.concatenate_matrices(inverse_matrix(self.pose_world), self.parent_model.pose_world))
   else:
     jointnode.attrib['type'] = self.type
   #print('self.pose_world\n', self.pose_world)
   #print('self.parent_model.pose_world\n', self.parent_model.pose_world)
   self.axis.add_urdf_elements(jointnode, transformations.concatenate_matrices(inverse_matrix(self.pose_world), self.parent_model.pose_world))
示例#3
0
 def calculate_absolute_pose(self, worldMVparent = transformations.identity_matrix()):
   worldMVmodel = transformations.concatenate_matrices(worldMVparent, self.pose)
   self.pose_world = worldMVmodel
   for submodel in self.submodels:
     submodel.calculate_absolute_pose(worldMVmodel)
   for link in self.links:
     link.pose_world = transformations.concatenate_matrices(worldMVmodel, link.pose)
   for joint in self.joints:
     joint.pose_world = transformations.concatenate_matrices(joint.tree_child_link.pose_world, joint.pose)
示例#4
0
文件: pysdf.py 项目: wxmerkt/director
 def calculate_absolute_pose(
     self, worldMVparent=transformations.identity_matrix()):
     worldMVmodel = transformations.concatenate_matrices(
         worldMVparent, self.pose)
     self.pose_world = worldMVmodel
     for submodel in self.submodels:
         submodel.calculate_absolute_pose(worldMVmodel)
     for link in self.links:
         link.pose_world = transformations.concatenate_matrices(
             worldMVmodel, link.pose)
     for joint in self.joints:
         joint.pose_world = transformations.concatenate_matrices(
             joint.tree_child_link.pose_world, joint.pose)
示例#5
0
文件: pysdf.py 项目: wxmerkt/director
 def add_urdf_elements(self, node, prefix, link_pose, part_type):
     if not self.geometry_type:
         return
     partnode = ET.SubElement(
         node, part_type, {'name': sdf2tfname(prefix + '::' + self.name)})
     pose2origin(partnode,
                 transformations.concatenate_matrices(link_pose, self.pose))
     geometrynode = ET.SubElement(partnode, 'geometry')
     if self.geometry_type == 'box':
         boxnode = ET.SubElement(geometrynode, 'box',
                                 {'size': self.geometry_data['size']})
     elif self.geometry_type == 'cylinder':
         cylindernode = ET.SubElement(
             geometrynode, 'cylinder', {
                 'radius': self.geometry_data['radius'],
                 'length': self.geometry_data['length']
             })
     elif self.geometry_type == 'sphere':
         spherenode = ET.SubElement(
             geometrynode, 'sphere',
             {'radius': self.geometry_data['radius']})
     elif self.geometry_type == 'mesh':
         mesh_file = '/'.join(self.geometry_data['uri'].split('/')[3:])
         mesh_found = find_mesh_in_catkin_ws(mesh_file)
         if mesh_found:
             mesh_path = 'package://' + mesh_found
         else:
             print('Could not find mesh %s in %s' %
                   (mesh_file, catkin_ws_path))
             mesh_path = 'package://PATHTOMESHES/' + mesh_file
         meshnode = ET.SubElement(geometrynode, 'mesh', {
             'filename': mesh_path,
             'scale': self.geometry_data['scale']
         })
示例#6
0
文件: pysdf.py 项目: wxmerkt/director
 def add_urdf_elements(self, node, link_pose):
     inertialnode = ET.SubElement(node, 'inertial')
     massnode = ET.SubElement(inertialnode, 'mass',
                              {'value': str(self.mass)})
     pose2origin(inertialnode,
                 transformations.concatenate_matrices(link_pose, self.pose))
     self.inertia.add_urdf_elements(inertialnode)
示例#7
0
def pose_msg2homogeneous(pose):
    trans = transformations.translation_matrix(
        (pose.position.x, pose.position.y, pose.position.z))
    rot = transformations.quaternion_matrix(
        (pose.orientation.x, pose.orientation.y, pose.orientation.z,
         pose.orientation.w))
    return transformations.concatenate_matrices(trans, rot)
示例#8
0
 def add_urdf_elements(self, node, prefix):
   full_prefix = prefix + '::' if prefix else ''
   linknode = ET.SubElement(node, 'link', {'name': sdf2tfname(full_prefix + self.name)})
   # urdf links do not have a coordinate system themselves, only their parts (inertial, collision, visual) have one
   if self.tree_parent_joint:
     if self.tree_parent_joint.parent_model == self.parent_model:
       urdf_pose = transformations.concatenate_matrices(inverse_matrix(self.tree_parent_joint.pose_world), self.pose_world)
     else: # joint crosses includes
       urdf_pose = transformations.identity_matrix()
   else: # root
     urdf_pose = self.pose_world
   self.inertial.add_urdf_elements(linknode, urdf_pose)
   self.collision.add_urdf_elements(linknode, prefix, urdf_pose)
   self.visual.add_urdf_elements(linknode, prefix, urdf_pose)
示例#9
0
文件: pysdf.py 项目: wxmerkt/director
 def add_urdf_elements(self, node, prefix):
     full_prefix = prefix + '::' if prefix else ''
     linknode = ET.SubElement(node, 'link',
                              {'name': sdf2tfname(full_prefix + self.name)})
     # urdf links do not have a coordinate system themselves, only their parts (inertial, collision, visual) have one
     if self.tree_parent_joint:
         if self.tree_parent_joint.parent_model == self.parent_model:
             urdf_pose = transformations.concatenate_matrices(
                 inverse_matrix(self.tree_parent_joint.pose_world),
                 self.pose_world)
         else:  # joint crosses includes
             urdf_pose = transformations.identity_matrix()
     else:  # root
         urdf_pose = self.pose_world
     self.inertial.add_urdf_elements(linknode, urdf_pose)
     self.collision.add_urdf_elements(linknode, prefix, urdf_pose)
     self.visual.add_urdf_elements(linknode, prefix, urdf_pose)
示例#10
0
 def add_urdf_elements(self, node, prefix, link_pose, part_type):
   if not self.geometry_type:
     return
   partnode = ET.SubElement(node, part_type, {'name': sdf2tfname(prefix + '::' + self.name)})
   pose2origin(partnode, transformations.concatenate_matrices(link_pose, self.pose))
   geometrynode = ET.SubElement(partnode, 'geometry')
   if self.geometry_type == 'box':
     boxnode = ET.SubElement(geometrynode, 'box', {'size': self.geometry_data['size']})
   elif self.geometry_type == 'cylinder':
     cylindernode = ET.SubElement(geometrynode, 'cylinder', {'radius': self.geometry_data['radius'], 'length': self.geometry_data['length']})
   elif self.geometry_type == 'sphere':
     spherenode = ET.SubElement(geometrynode, 'sphere', {'radius': self.geometry_data['radius']})
   elif self.geometry_type == 'mesh':
     mesh_file = '/'.join(self.geometry_data['uri'].split('/')[3:])
     mesh_found = find_mesh_in_catkin_ws(mesh_file)
     if mesh_found:
       mesh_path = 'package://' + mesh_found
     else:
       print('Could not find mesh %s in %s' % (mesh_file, catkin_ws_path))
       mesh_path = 'package://PATHTOMESHES/' + mesh_file
     meshnode = ET.SubElement(geometrynode, 'mesh', {'filename': mesh_path, 'scale': self.geometry_data['scale']})
示例#11
0
 def add_urdf_elements(self, node, link_pose):
   inertialnode = ET.SubElement(node, 'inertial')
   massnode = ET.SubElement(inertialnode, 'mass', {'value': str(self.mass)})
   pose2origin(inertialnode, transformations.concatenate_matrices(link_pose, self.pose))
   self.inertia.add_urdf_elements(inertialnode)
示例#12
0
def pose_msg2homogeneous(pose):
  trans = transformations.translation_matrix((pose.position.x, pose.position.y, pose.position.z))
  rot = transformations.quaternion_matrix((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w))
  return transformations.concatenate_matrices(trans, rot)