made attach_meshes better

Author: beverlylytle

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

@@ -117,44 +117,60 @@ def detach_tool_model(self):
         """
         self.attached_tool_model = None
 
-    def attach_mesh(self, mesh, link=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()
 
-        frame = link.parent_joint.origin.copy()
-        initial_transformation = Transformation.from_frame_to_frame(Frame.worldXY(), frame)
+        sample_geometry = None
 
-        sample_geometry = link.collision[0] if link.collision else link.visual[0] if link.visual else None
-
-        if hasattr(sample_geometry, 'current_transformation'):
-            relative_transformation = sample_geometry.current_transformation
-        else:
-            relative_transformation = Transformation()
+        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)
 
-        transformation = relative_transformation.concatenated(initial_transformation)
         native_mesh = self.create_geometry(mesh)
-        self.transform(native_mesh, transformation)
+        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 = transformation  # !!!
-        item.current_transformation = Transformation()  # !!!
+        item.init_transformation = init_transformation
+        item.current_transformation = sample_geometry.current_transformation
+
+        self.attached_items.setdefault(link.name, {})[name] = item
 
-        self.attached_items.setdefault(link.name, []).append(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.
@@ -300,7 +316,7 @@ 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, []):
+        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):
@@ -349,7 +365,7 @@ def draw_collision(self):
     def draw_attached_meshes(self):
         """Draws all meshes attached to the robot model."""
         for items in self.attached_items.values():
-            for item in items:
+            for item in items.values():
                 for native_mesh in item.native_geometry:
                     yield native_mesh
 

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_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()