Added 4 methods for extracting meshes from links in RobotModel

CHANGELOG.md

@@ -9,6 +9,12 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+* Added new functions to extract Visual and Collision meshes from `RobotModel`.
+* Added `RobotModel.get_link_visual_meshes` and `RobotModel.get_link_collision_meshes`
+  for extracting meshes from a specific link.
+  Added `RobotModel.get_link_visual_meshes_joined` and `RobotModel.get_link_collision_meshes_joined`
+  for extracting a single joined mesh from a specific link.
+
 ### Changed
 
 ### Removed

src/compas_robots/model/base.py

@@ -132,6 +132,9 @@ def scale(self, factor):
         """
         self.point = self.point * factor
 
+    def to_compas_frame(self):
+        return Frame(self.point, self.xaxis, self.yaxis)
+
 
 class ColorProxy(ProxyObject):
     """Proxy class that adds URDF functionality to an instance of :class:`Color`.

src/compas_robots/model/robot.py

@@ -5,6 +5,7 @@
 import itertools
 import random
 
+from compas import IPY
 from compas.colors import Color
 from compas.data import Data
 from compas.datastructures import Mesh
@@ -34,6 +35,13 @@
 from .link import Link
 from .link import Visual
 
+if not IPY:
+    from typing import TYPE_CHECKING
+
+    if TYPE_CHECKING:
+        from typing import List  # noqa: F401
+        from typing import Optional  # noqa: F401
+
 
 class RobotModel(Data):
     """RobotModel is the root element of the model.
@@ -822,6 +830,10 @@ def scale(self, factor, link=None):
 
         self._scale_factor = factor
 
+    # --------------------------------------------------------------------------
+    # Methods for computing frames and axes from the Robot Model
+    # --------------------------------------------------------------------------
+
     def compute_transformations(self, joint_state, link=None, parent_transformation=None):
         """Recursive function to calculate the transformations of each joint.
 
@@ -874,7 +886,9 @@ def compute_transformations(self, joint_state, link=None, parent_transformation=
         return transformations
 
     def transformed_frames(self, joint_state):
-        """Returns the transformed frames based on the joint_state.
+        """Returns the transformed Joint frames (relative to the Robot Coordinate Frame) based on the joint_state (:class:`~compas_robots.Configuration`).
+
+        The order of the frames is the same as the order returned by :meth:`iter_joints`.
 
         Parameters
         ----------
@@ -990,6 +1004,181 @@ def _check_link_name(self, name):
         if name in all_link_names:
             raise ValueError("Link name '%s' already used in chain." % name)
 
+    # --------------------------------------------------------------------------
+    # Methods for accessing the visual and collision geometry
+    # --------------------------------------------------------------------------
+
+    def _extract_link_meshes(self, link_elements, meshes_at_link_origin=True):
+        # type: (List[Visual] | List[Collision], Optional[bool]) -> List[Mesh]
+        """Extracts the list of compas meshes from a link's Visual or Collision elements.
+
+        Parameters
+        ----------
+        link_elements : list of :class:`~compas_robots.model.Visual` or :class:`~compas_robots.model.Collision`
+            The list of Visual or Collision elements of a link.
+        meshes_at_link_origin : bool, optional
+            Defaults to True, which means that the meshes will be transformed according to
+            `element.origin`, such that the mesh origin matches with the link's origin frame.
+            If False, the meshes will be extracted as it is loaded from the
+            robot model package. Note that the `.origin` for each element is not necessarily
+            the same.
+
+        Returns
+        -------
+        list of :class:`~compas.datastructures.Mesh`
+            A list of meshes belonging to the link elements.
+
+        Notes
+        -----
+        Only MeshDescriptor in `element.geometry.shape` is supported. Other shapes are ignored.
+
+        """
+        if not link_elements:
+            return None
+
+        meshes = []
+        # Note: Each Link can have multiple visual nodes
+        for element in link_elements:
+            # Some elements may have a non-identity origin frame
+            origin = element.origin.to_compas_frame() if element.origin else Frame.worldXY()
+            t_origin = Transformation.from_frame(origin)
+            # If `meshes_at_link_origin` is False, we use an identity transformation
+            t_origin = t_origin if meshes_at_link_origin else Transformation()
+
+            shape = element.geometry.shape
+            # Note: the MeshDescriptor.meshes object supports a list of compas meshes.
+            if isinstance(shape, MeshDescriptor):
+                # There can be multiple mesh in a single MeshDescriptor
+                for mesh in shape.meshes:
+                    # Transform the mesh (even if t_origin is identity) so we always get a new mesh object
+                    meshes.append(mesh.transformed(t_origin))
+
+        return meshes
+
+    def get_link_visual_meshes(self, link):
+        # type: (Link) -> List[Mesh]
+        """Get a list of visual meshes from a Link.
+
+        The origin of the visual meshes are transformed according to the `element.origin`
+        of the Visual nodes. This means that the returned mesh will match with the link's origin frame.
+
+        Parameters
+        ----------
+        link : :class:`~compas_robots.model.Link`
+            The link to extract the visual meshes from.
+
+        Returns
+        -------
+        list of :class:`~compas.datastructures.Mesh`
+            A list of visual meshes belonging to the link
+
+        Notes
+        -----
+        Only MeshDescriptor in `element.geometry.shape` is supported. Other shapes are ignored.
+        """
+        visual_meshes = self._extract_link_meshes(link.visual, True)
+        return visual_meshes
+
+    def get_link_visual_meshes_joined(self, link, weld=False, weld_precision=None):
+        # type: (Link, Optional[bool], Optional[int]) -> Mesh | None
+        """Get the visual meshes of a Link joined into a single mesh.
+
+        The origin of the visual meshes are transformed according to the `element.origin`
+        of the Visual nodes. This means that the returned mesh will match with the link's origin frame.
+
+        Parameters
+        ----------
+        link : :class:`~compas_robots.model.Link`
+            The link to extract the visual meshes from.
+        weld : bool, optional
+            If True, weld close vertices after joining. Defaults to False.
+        weld_precision : int, optional
+            The precision used for welding the mesh.
+            Default is :attr:`TOL.precision`.
+
+        Returns
+        -------
+        :class:`~compas.datastructures.Mesh` | None
+            A single mesh representing the visual meshes of the link.
+            None if no visual meshes are found.
+
+        Notes
+        -----
+        Only MeshDescriptor in `element.geometry.shape` is supported. Other shapes are ignored.
+        """
+        visual_meshes = self._extract_link_meshes(link.visual, True)
+        if not visual_meshes:
+            return None
+
+        joined_mesh = Mesh()
+        for mesh in visual_meshes:
+            joined_mesh.join(mesh, weld, weld_precision)
+        return joined_mesh
+
+    def get_link_collision_meshes(self, link):
+        # type: (Link) -> List[Mesh]
+        """Get the list of collision meshes of a link.
+
+        The origin of the visual meshes are transformed according to the `element.origin`
+        of the Visual nodes. This means that the returned mesh will match with the link's origin frame.
+
+        Parameters
+        ----------
+        link : :class:`~compas_robots.model.Link`
+            The link to extract the collision meshes from.
+
+        Returns
+        -------
+        list of :class:`~compas.datastructures.Mesh`
+            A list of collision meshes belonging to the link
+
+        Notes
+        -----
+        Only MeshDescriptor in `element.geometry.shape` is supported. Other shapes are ignored.
+        """
+        collision_meshes = self._extract_link_meshes(link.collision, True)
+        return collision_meshes
+
+    def get_link_collision_meshes_joined(self, link, weld=False, weld_precision=None):
+        # type: (Link, Optional[bool], Optional[int]) -> Mesh | None
+        """Get the collision meshes of a Link joined into a single mesh.
+
+        The origin of the visual meshes are transformed according to the `element.origin`
+        of the Visual nodes. This means that the returned mesh will match with the link's origin frame.
+
+        Parameters
+        ----------
+        link : :class:`~compas_robots.model.Link`
+            The link to extract the collision meshes from.
+        weld : bool, optional
+            If True, weld close vertices after joining. Defaults to False.
+        weld_precision : int, optional
+            The precision used for welding the mesh.
+            Default is :attr:`TOL.precision`.
+
+        Returns
+        -------
+        :class:`~compas.datastructures.Mesh` | None
+            A single mesh representing the collision meshes of the link.
+            None if no collision meshes are found.
+
+        Notes
+        -----
+        Only MeshDescriptor in `element.geometry.shape` is supported. Other shapes are ignored.
+        """
+        collision_meshes = self._extract_link_meshes(link.collision, True)
+        if not collision_meshes:
+            return None
+
+        joined_mesh = Mesh()
+        for mesh in collision_meshes:
+            joined_mesh.join(mesh, weld, weld_precision)
+        return joined_mesh
+
+    # --------------------------------------------------------------------------
+    # Methods for modifying the Robot Model structure
+    # --------------------------------------------------------------------------
+
     def add_link(self, name, visual_meshes=None, visual_color=None, collision_meshes=None, **kwargs):
         """Adds a link to the robot model.
 

src/compas_robots/rhino/scene/robotmodelobject.py

@@ -93,7 +93,7 @@ def _enter_layer(self):
 
         if self.layer:
             if not rs.IsLayer(self.layer):
-                compas_rhino.create_layers_from_path(self.layer)
+                compas_rhino.layers.create_layers_from_path(self.layer)
             self._previous_layer = rs.CurrentLayer(self.layer)
 
         rs.EnableRedraw(False)
@@ -208,9 +208,9 @@ def clear_layer(self):
 
         """
         if self.layer:
-            compas_rhino.clear_layer(self.layer)
+            compas_rhino.layers.clear_layer(self.layer)
         else:
-            compas_rhino.clear_current_layer()
+            compas_rhino.layers.clear_current_layer()
 
     def _add_mesh_to_doc(self, mesh):
         guid = sc.doc.Objects.AddMesh(mesh)