diff --git a/CHANGELOG.md b/CHANGELOG.md index 73b784e1a..d6efbb208 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/src/compas_robots/model/robot.py b/src/compas_robots/model/robot.py index fb8d6e223..d959341ad 100644 --- a/src/compas_robots/model/robot.py +++ b/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,180 @@ 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): + # type: (List[Visual] | List[Collision]) -> List[Mesh] + """Extracts the list of compas meshes from a link's Visual or Collision elements. + + Note that each link may have multiple visual or collision nodes, each can have + a different origin frame. Therefore, the returned meshes are transformed + according to the ``.origin`` frame of each visual or collision element. + + This transformation is time consuming for large meshes and should be done only once. + This transformation is automatically skipped if the origin is None or the identity frame. + + 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. + + Returns + ------- + list of :class:`~compas.datastructures.Mesh` + A list of meshes belonging to the link elements. + If there are no meshes, an empty list is returned. + + """ + meshes = [] + # Note: Each Link can have multiple visual nodes + for element in link_elements: + # Some elements may have a non-identity origin frame + t_origin = None + if element.origin: + origin = element.origin if isinstance(element.origin, Frame) else element.origin._proxied_object + if Frame.worldXY() != origin: + t_origin = Transformation.from_frame(element.origin) + + # Note: the MeshDescriptor.meshes object supports a list of compas meshes. + # There can be multiple mesh in a single MeshDescriptor + for mesh in LinkGeometry._get_item_meshes(element): + # Transform the mesh + if t_origin: + meshes.append(mesh.transformed(t_origin)) + else: + meshes.append(mesh) + + 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 + The list is empty 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) + 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 + The list is empty 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) + 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. diff --git a/src/compas_robots/rhino/scene/robotmodelobject.py b/src/compas_robots/rhino/scene/robotmodelobject.py index c762eea74..5ddce2dbe 100644 --- a/src/compas_robots/rhino/scene/robotmodelobject.py +++ b/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)