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)
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)
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)
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)
def from_file(self, filename, **kwargs): if not os.path.exists(filename): print('Failed to open Model because %s does not exist' % filename) return tree = ET.parse(filename) root = tree.getroot() if root.tag != 'sdf': print('Not a SDF file. Aborting.') return self.version = float(root.attrib['version']) if not self.version in supported_sdf_versions: print('Unsupported SDF version in %s. Aborting.\n' % filename) return modelnode = get_node(root, 'model') self.from_tree(modelnode, **kwargs) # Override name (e.g. for <include>) kwargs_name = kwargs.get('name') if kwargs_name: self.name = kwargs_name # External pose offset (from <include>) self.pose = numpy.dot(kwargs.get('pose', transformations.identity_matrix()), self.pose)
def from_file(self, filename, **kwargs): if not os.path.exists(filename): print('Failed to open Model because %s does not exist' % filename) return tree = ET.parse(filename) root = tree.getroot() if root.tag != 'sdf': print('Not a SDF file. Aborting.') return self.version = float(root.attrib['version']) if not self.version in supported_sdf_versions: print('Unsupported SDF version in %s. Aborting.\n' % filename) return modelnode = get_node(root, 'model') self.from_tree(modelnode, **kwargs) # Override name (e.g. for <include>) kwargs_name = kwargs.get('name') if kwargs_name: self.name = kwargs_name # External pose offset (from <include>) self.pose = numpy.dot( kwargs.get('pose', transformations.identity_matrix()), self.pose)
def __init__(self, **kwargs): self.pose = transformations.identity_matrix() self.mass = 0 self.inertia = Inertia() if 'tree' in kwargs: self.from_tree(kwargs['tree'])
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))
def __init__(self, **kwargs): self.name = '' self.pose = transformations.identity_matrix() self.pose_world = transformations.identity_matrix()
def homogeneous_times_vector(homogeneous, vector): vector_as_hom = transformations.identity_matrix() vector_as_hom[:3,3] = vector.T res = numpy.dot(homogeneous, vector_as_hom) return res[:3,3].T
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))
def homogeneous_times_vector(homogeneous, vector): vector_as_hom = transformations.identity_matrix() vector_as_hom[:3, 3] = vector.T res = numpy.dot(homogeneous, vector_as_hom) return res[:3, 3].T