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 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 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, 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']
})
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)
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 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']})
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)
vis = vis["group1"]
box_vis = vis["box"]
sphere_vis = vis["sphere"]
box = Box([1, 1, 1])
geom = GeometryData(box, color=[0, 1, 0, 0.5])
box_vis.setgeometry(geom)
sphere_vis.setgeometry(Sphere(0.5))
sphere_vis.settransform(transformations.translation_matrix([1, 0, 0]))
vis["test"].setgeometry(Triad())
vis["test"].settransform(
transformations.concatenate_matrices(
transformations.rotation_matrix(1.0, [0, 0, 1]),
transformations.translation_matrix([-1, 0, 1])))
vis["triad"].setgeometry(Triad())
# Setting the geometry preserves the transform at that path.
# Call settransform(np.eye(4)) if you want to clear the transform.
vis["test"].setgeometry(Triad())
# bug, the sphere is loaded and replaces the previous
# geometry but it is not drawn with the correct color mode
vis["test"].setgeometry(Sphere(0.5))
for theta in np.linspace(0, 2 * np.pi, 100):
vis.settransform(transformations.rotation_matrix(theta, [0, 0, 1]))
time.sleep(0.01)
# asked for
vis = vis["group1"]
box_vis = vis["box"]
sphere_vis = vis["sphere"]
box = Box([1, 1, 1])
geom = GeometryData(box, color=[0, 1, 0, 0.5])
box_vis.setgeometry(geom)
sphere_vis.setgeometry(Sphere(0.5))
sphere_vis.settransform(transformations.translation_matrix([1, 0, 0]))
vis["test"].setgeometry(Triad())
vis["test"].settransform(transformations.concatenate_matrices(
transformations.rotation_matrix(1.0, [0, 0, 1]),
transformations.translation_matrix([-1, 0, 1])))
vis["triad"].setgeometry(Triad())
# Setting the geometry preserves the transform at that path.
# Call settransform(np.eye(4)) if you want to clear the transform.
vis["test"].setgeometry(Triad())
# bug, the sphere is loaded and replaces the previous
# geometry but it is not drawn with the correct color mode
vis["test"].setgeometry(Sphere(0.5))
for theta in np.linspace(0, 2 * np.pi, 100):
vis.settransform(transformations.rotation_matrix(theta, [0, 0, 1]))
time.sleep(0.01)
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)
