Merge pull request #883 from compas-dev/draw_acm

Attach meshes to a RobotModelArtist and draw them

Author: gonzalocasas

CHANGELOG.md

@@ -14,6 +14,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 * Added `compas.data.is_sequence_of_uint`.
 * Added general plotter for geometry objects and data structures based on the artist registration mechanism.
 * Added support for multimesh files to OBJ reader/writer.
+* Added support for attaching and detaching meshes in `compas.robots.RobotModelArtist` and drawing them.
 
 ### Changed
 

src/compas/robots/base_artist/_artist.py

@@ -8,6 +8,7 @@
 from compas.geometry import Scale
 from compas.geometry import Transformation
 from compas.robots import Geometry
+from compas.robots.model.link import Item
 
 
 __all__ = [
@@ -75,6 +76,7 @@ def __init__(self, model):
         self.create()
         self.scale_factor = 1.
         self.attached_tool_model = None
+        self.attached_items = {}
 
     def attach_tool_model(self, tool_model):
         """Attach a tool to the robot artist.
@@ -114,6 +116,61 @@ def detach_tool_model(self):
         """
         self.attached_tool_model = None
 
+    def attach_mesh(self, mesh, name, link=None, frame=None):
+        """Rigidly attaches a compas mesh to a given link.
+
+        Parameters
+        ----------
+        mesh : :class:`compas.datastructures.Mesh`
+            The mesh to attach to the robot model.
+        name : :obj:`str`
+            The identifier of the mesh.
+        link : :class:`compas.robots.Link`
+            The link within the robot model or tool model to attach the mesh to. Optional.
+            Defaults to the model's end effector link.
+        frame : :class:`compas.geometry.Frame`
+            The frame of the mesh. Defaults to :meth:`compas.geometry.Frame.worldXY`.
+
+        Returns
+        -------
+        ``None``
+        """
+        if not link:
+            link = self.model.get_end_effector_link()
+        transformation = Transformation.from_frame(frame) if frame else Transformation()
+
+        sample_geometry = None
+
+        while sample_geometry is None:
+            sample_geometry = link.collision[0] if link.collision else link.visual[0] if link.visual else None
+            link = self.model.get_link_by_name(link.parent_joint.parent.link)
+
+        native_mesh = self.create_geometry(mesh)
+        init_transformation = transformation * sample_geometry.init_transformation
+        self.transform(native_mesh, sample_geometry.current_transformation * init_transformation)
+
+        item = Item()
+        item.native_geometry = [native_mesh]
+        item.init_transformation = init_transformation
+        item.current_transformation = sample_geometry.current_transformation
+
+        self.attached_items.setdefault(link.name, {})[name] = item
+
+    def detach_mesh(self, name):
+        """Removes attached collision meshes with a given name.
+
+        Parameters
+        ----------
+        name : :obj:`str`
+            The identifier of the mesh.
+
+        Returns
+        -------
+        ``None``
+        """
+        for _, items in self.attached_items:
+            items.pop(name, None)
+
     def create(self, link=None, context=None):
         """Recursive function that triggers the drawing of the robot model's geometry.
 
@@ -258,6 +315,8 @@ def _transform_link_geometry(self, link, transformation, collision=True):
                 # some links have only collision geometry, not visual. These meshes have not been loaded.
                 if item.native_geometry:
                     self._apply_transformation_on_transformed_link(item, transformation)
+        for item in self.attached_items.get(link.name, {}).values():
+            self._apply_transformation_on_transformed_link(item, transformation)
 
     def update_tool(self, joint_state=None, visual=True, collision=True, transformation=None):
         """Triggers the update of the robot geometry of the tool.
@@ -302,6 +361,13 @@ def draw_collision(self):
             for native_geometry in self._iter_geometry(self.attached_tool_model, 'collision'):
                 yield native_geometry
 
+    def draw_attached_meshes(self):
+        """Draws all meshes attached to the robot model."""
+        for items in self.attached_items.values():
+            for item in items.values():
+                for native_mesh in item.native_geometry:
+                    yield native_mesh
+
     @staticmethod
     def _iter_geometry(model, geometry_type):
         for link in model.iter_links():

src/compas/robots/model/joint.py

@@ -460,7 +460,9 @@ class Joint(Data):
     SUPPORTED_TYPES = ('revolute', 'continuous', 'prismatic', 'fixed',
                        'floating', 'planar')
 
-    def __init__(self, name, type, parent, child, origin=None, axis=None, calibration=None, dynamics=None, limit=None, safety_controller=None, mimic=None, **kwargs):
+    def __init__(self, name, type, parent, child, origin=None, axis=None,
+                 calibration=None, dynamics=None, limit=None,
+                 safety_controller=None, mimic=None, **kwargs):
         type_idx = Joint.SUPPORTED_TYPES.index(type) if isinstance(type, str) else type
 
         if type_idx not in range(len(Joint.SUPPORTED_TYPES)):

src/compas/robots/model/link.py

@@ -168,7 +168,14 @@ def data(self, data):
         self.inertia = Inertia.from_data(data['inertia']) if data['inertia'] else None
 
 
-class Visual(Data):
+class Item(object):
+    def __init__(self):
+        self.init_transformation = None  # to store the init transformation
+        self.current_transformation = None  # to store the current transformation
+        self.native_geometry = None  # to store the link's CAD native geometry
+
+
+class Visual(Item, Data):
     """Visual description of a link.
 
     Attributes
@@ -195,10 +202,6 @@ def __init__(self, geometry, origin=None, name=None, material=None, **kwargs):
         self.material = material
         self.attr = kwargs
 
-        self.init_transformation = None  # to store the init transformation
-        self.current_transformation = None  # to store the current transformation
-        self.native_geometry = None  # to store the link's CAD native geometry
-
     def get_urdf_element(self):
         attributes = {}
         if self.name is not None:
@@ -265,7 +268,7 @@ def from_primitive(cls, primitive, **kwargs):
         return cls(geometry, origin=origin, **kwargs)
 
 
-class Collision(Data):
+class Collision(Item, Data):
     """Collidable description of a link.
 
     Attributes
@@ -289,10 +292,6 @@ def __init__(self, geometry, origin=None, name=None, **kwargs):
         self.name = name
         self.attr = kwargs
 
-        self.init_transformation = None  # to store the init transformation
-        self.current_transformation = None  # to store the current transformation
-        self.native_geometry = None  # to store the link's CAD native geometry
-
     def get_urdf_element(self):
         attributes = {}
         if self.name is not None:

src/compas_blender/artists/robotmodelartist.py

@@ -57,3 +57,8 @@ def draw_collision(self):
         collisions = super(RobotModelArtist, self).draw_collision()
         for collision in collisions:
             collision.hide_set(False)
+
+    def draw_attached_meshes(self):
+        meshes = super(RobotModelArtist, self).draw_attached_meshes()
+        for mesh in meshes:
+            mesh.hide_set(False)

src/compas_rhino/artists/robotmodelartist.py

@@ -150,6 +150,27 @@ def draw_visual(self):
         self._exit_layer()
         return new_guids
 
+    def draw_attached_meshes(self):
+        """Draw all the geometries attached to the robot model.
+
+        Returns
+        -------
+        list
+            The GUIDs of the created Rhino objects.
+        """
+        acms = super(RobotModelArtist, self).draw_attached_meshes()
+        acms = list(acms)
+
+        self._enter_layer()
+
+        new_guids = []
+        for mesh in acms:
+            guids = self._add_mesh_to_doc(mesh)
+            new_guids.extend(guids)
+
+        self._exit_layer()
+        return new_guids
+
     def draw(self):
         """Same as draw_visual."""
         return self.draw_visual()