def add_link(self, name, visual_mesh=None, visual_color=None, collision_mesh=None, **kwargs):
"""Adds a link to the robot model.
Provides an easy way to programmatically add a link to the robot model.
Parameters
----------
name : str
The name of the link
visual_mesh : :class:`compas.datastructures.Mesh`, optional
The link's visual mesh.
visual_color : list of 3 float, optional
The rgb color of the mesh. Defaults to (0.8, 0.8, 0.8)
collision_mesh : :class:`compas.datastructures.Mesh`, optional
The link's collision mesh.
Returns
-------
:class:`Link`
The created `Link`
Raises
------
ValueError
If the link name is already used in the chain.
Examples
--------
>>> sphere = Sphere((0, 0, 0), 1)
>>> mesh = Mesh.from_shape(sphere)
>>> robot = RobotModel('robot')
>>> link = robot.add_link('link0', visual_mesh=mesh)
"""
all_link_names = [l.name for l in self.links]
if name in all_link_names:
raise ValueError("Link name '%s' already used in chain." % name)
visual = []
collision = []
if visual_mesh:
if not visual_color:
visual_color = (0.8, 0.8, 0.8)
v = Visual(Geometry(MeshDescriptor("")))
v.material = Material(color=Color("%f %f %f 1" % visual_color))
v.geometry.shape.geometry = visual_mesh
visual.append(v)
if collision_mesh: # use visual_mesh as collision_mesh if none passed?
c = Collision(Geometry(MeshDescriptor("")))
c.geometry.shape.geometry = collision_mesh
collision.append(c)
link = Link(name, visual=visual, collision=collision, **kwargs)
self.links.append(link)
return link
def from_primitive(cls, primitive, **kwargs):
"""Create collision link from a primitive shape.
Parameters
----------
primitive : :compas:`compas.geometry.Shape`
A primitive shape.
**kwargs : dict[str, Any], optional
The keyword arguments (kwargs) collected in a dict.
These allow using non-standard attributes absent in the URDF specification.
Returns
-------
:class:`compas.datastructures.Mesh`
A collision description object.
"""
geometry = Geometry()
geometry.shape = primitive
return cls(geometry, **kwargs)
def data(self, data):
self.geometry = Geometry.from_data(data['geometry'])
self.origin = Origin.from_data(
data['origin']) if data['origin'] else None
self.name = data['name']
self.attr = _attr_from_data(data['attr'])
self.init_transformation = Transformation.from_data(
data['init_transformation']
) if data['init_transformation'] else None
self.current_transformation = Transformation.from_data(
data['current_transformation']
) if data['current_transformation'] else None
def _get_geometry_and_origin(primitive):
shape = None
origin = None
if isinstance(primitive, compas.geometry.Box):
shape = Box.from_geometry(primitive)
origin = Origin(*primitive.frame)
if isinstance(primitive, compas.geometry.Capsule):
shape = Capsule.from_geometry(primitive)
point = primitive.line.midpoint
normal = primitive.line.vector
plane = Plane(point, normal)
origin = Origin.from_plane(plane)
if isinstance(primitive, compas.geometry.Cylinder):
shape = Cylinder.from_geometry(primitive)
origin = Origin.from_plane(primitive.circle.plane)
if isinstance(primitive, compas.geometry.Sphere):
shape = Sphere.from_geometry(primitive)
origin = Origin(primitive.point, [1, 0, 0], [0, 1, 0])
if not shape:
raise Exception('Unrecognized primitive type {}'.format(
primitive.__class__))
geometry = Geometry(shape)
return geometry, origin
def from_data(cls, data):
visual = cls(Geometry.from_data(data['geometry']))
visual.data = data
return visual
def add_link(self, name, visual_meshes=None, visual_color=None, collision_meshes=None, **kwargs):
"""Adds a link to the robot model.
Provides an easy way to programmatically add a link to the robot model.
Parameters
----------
name : str
The name of the link
visual_meshes : list of :class:`compas.datastructures.Mesh` or :class:`compas.geometry.Shape`, optional
The link's visual mesh.
visual_color : list of 3 float, optional
The rgb color of the mesh. Defaults to (0.8, 0.8, 0.8)
collision_meshes : list of :class:`compas.datastructures.Mesh` or :class:`compas.geometry.Shape`, optional
The link's collision mesh.
Returns
-------
:class:`Link`
The created `Link`
Raises
------
ValueError
If the link name is already used in the chain.
Examples
--------
>>> sphere = Sphere((0, 0, 0), 1)
>>> mesh = Mesh.from_shape(sphere)
>>> robot = RobotModel('robot')
>>> link = robot.add_link('link0', visual_mesh=mesh)
"""
self._check_link_name(name)
visual_meshes, kwargs = self._consolidate_meshes(visual_meshes, 'visual_mesh', **kwargs)
collision_meshes, kwargs = self._consolidate_meshes(collision_meshes, 'collision_mesh', **kwargs)
if not visual_color:
visual_color = (0.8, 0.8, 0.8)
visuals = []
collisions = []
for visual in visual_meshes:
if isinstance(visual, Mesh):
v = Visual(Geometry(MeshDescriptor("")))
v.geometry.shape.geometry = visual
else:
v = Visual.from_primitive(visual)
v.material = Material(color=Color("%f %f %f 1" % visual_color))
visuals.append(v)
for collision in collision_meshes: # use visual_mesh as collision_mesh if none passed?
if isinstance(collision, Mesh):
c = Collision(Geometry(MeshDescriptor("")))
c.geometry.shape.geometry = collision
else:
c = Collision.from_primitive(collision)
collisions.append(c)
link = Link(name, visual=visuals, collision=collisions, **kwargs)
self.links.append(link)
# Must build the tree structure, if adding the first link to an empty robot
if len(self.links) == 1:
self._rebuild_tree()
self._create(self.root, Transformation())
return link
def from_data(cls, data):
collision = cls(Geometry.from_data(data['geometry']))
collision.data = data
return collision
