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 __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
