From e48fdb07de50f595d581cc6e662f85ebf366404d Mon Sep 17 00:00:00 2001 From: Victor LEUNG Date: Tue, 29 Oct 2024 23:37:32 +0800 Subject: [PATCH] GH SceneObject for RigidBody, RobotModel and RobotCell --- src/compas_fab/ghpython/scene/__init__.py | 2 + .../ghpython/scene/gh_scene_object.py | 60 +++++ .../ghpython/scene/rigid_body_object.py | 77 +++--- .../ghpython/scene/robot_cell_object.py | 30 ++- .../ghpython/scene/robot_model_object.py | 11 + src/compas_fab/robots/state.py | 21 +- src/compas_fab/scene/__init__.py | 2 + .../scene/base_robot_cell_object.py | 85 ++++--- .../scene/base_robot_model_object.py | 230 ++++++++++++++++++ 9 files changed, 432 insertions(+), 86 deletions(-) create mode 100644 src/compas_fab/ghpython/scene/gh_scene_object.py create mode 100644 src/compas_fab/ghpython/scene/robot_model_object.py create mode 100644 src/compas_fab/scene/base_robot_model_object.py diff --git a/src/compas_fab/ghpython/scene/__init__.py b/src/compas_fab/ghpython/scene/__init__.py index d23bc3d86..2e9e122c4 100644 --- a/src/compas_fab/ghpython/scene/__init__.py +++ b/src/compas_fab/ghpython/scene/__init__.py @@ -14,6 +14,7 @@ from .reachabilitymapobject import ReachabilityMapObject from .rigid_body_object import RigidBodyObject from .robot_cell_object import RobotCellObject + from .robot_model_object import RobotModelObject @plugin(category="factories", requires=["Rhino"]) def register_scene_objects(): @@ -29,4 +30,5 @@ def register_scene_objects(): "ReachabilityMapObject", "RobotCellObject", "RigidBodyObject", + "RobotModelObject", ] diff --git a/src/compas_fab/ghpython/scene/gh_scene_object.py b/src/compas_fab/ghpython/scene/gh_scene_object.py new file mode 100644 index 000000000..bc079f6a4 --- /dev/null +++ b/src/compas_fab/ghpython/scene/gh_scene_object.py @@ -0,0 +1,60 @@ +from __future__ import absolute_import +from __future__ import division +from __future__ import print_function + + +from compas_ghpython.drawing import draw_mesh +from compas_rhino.conversions import transformation_to_rhino + +from compas_ghpython.scene import GHSceneObject as compas_ghpython_GHSceneObject + +from compas import IPY + +if not IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import Optional # noqa: F401 + from typing import List # noqa: F401 + + from compas.datastructures import Mesh # noqa: F401 + from compas.geometry import Transformation # noqa: F401 + + +class GHSceneObject(compas_ghpython_GHSceneObject): + """Base class for all GH scene objects.""" + + def _transform(self, native_mesh, transformation): + # type: (object, Transformation) -> object + T = transformation_to_rhino(transformation) + native_mesh.Transform(T) + return native_mesh + + def _create_geometry(self, geometry, name=None, color=None): + # type: (Mesh, Optional[str], Optional[List[int]]) -> object + """Create the scene object representing one mesh geometry. + + Parameters + ---------- + geometry : :class:`~compas.datastructures.Mesh` + Instance of a mesh data structure + name : str, optional + The name of the mesh to draw. + color : :class:`~compas.colors.Color` + The color of the object.` + + Returns + ------- + :rhino:`Rhino.Geometry.Mesh` + """ + mesh = geometry # type: Mesh + color = color.rgba255 if color else None + + vertices, faces = geometry.to_vertices_and_faces(triangulated=False) + mesh = draw_mesh(vertices, faces, color=color) + + # Try to fix invalid meshes + if not mesh.IsValid: + mesh.FillHoles() + + return mesh diff --git a/src/compas_fab/ghpython/scene/rigid_body_object.py b/src/compas_fab/ghpython/scene/rigid_body_object.py index 485cc0d07..c2dbdbb8d 100644 --- a/src/compas_fab/ghpython/scene/rigid_body_object.py +++ b/src/compas_fab/ghpython/scene/rigid_body_object.py @@ -4,7 +4,7 @@ from compas_ghpython.drawing import draw_mesh -from compas_ghpython.scene import GHSceneObject +from .gh_scene_object import GHSceneObject from compas_rhino.conversions import transformation_to_rhino @@ -22,40 +22,41 @@ from compas.geometry import Transformation # noqa: F401 -class RigidBodyObject(BaseRigidBodyObject, GHSceneObject): - """Scene object for drawing a RigidBody.""" - - def _transform(self, native_mesh, transformation): - # type: (object, Transformation) -> object - T = transformation_to_rhino(transformation) - native_mesh.Transform(T) - return native_mesh - - def _create_geometry(self, geometry, name=None, color=None): - # type: (Mesh, Optional[str], Optional[List[int]]) -> object - """Create the scene object representing one mesh geometry. - - Parameters - ---------- - geometry : :class:`~compas.datastructures.Mesh` - Instance of a mesh data structure - name : str, optional - The name of the mesh to draw. - color : :class:`~compas.colors.Color` - The color of the object.` - - Returns - ------- - :rhino:`Rhino.Geometry.Mesh` - """ - mesh = geometry # type: Mesh - color = color.rgba255 if color else None - - vertices, faces = geometry.to_vertices_and_faces(triangulated=False) - mesh = draw_mesh(vertices, faces, color=color) - - # Try to fix invalid meshes - if not mesh.IsValid: - mesh.FillHoles() - - return mesh +class RigidBodyObject(GHSceneObject, BaseRigidBodyObject): + """Scene object for drawing a RigidBody in GHPython.""" + + pass + # def _transform(self, native_mesh, transformation): + # # type: (object, Transformation) -> object + # T = transformation_to_rhino(transformation) + # native_mesh.Transform(T) + # return native_mesh + + # def _create_geometry(self, geometry, name=None, color=None): + # # type: (Mesh, Optional[str], Optional[List[int]]) -> object + # """Create the scene object representing one mesh geometry. + + # Parameters + # ---------- + # geometry : :class:`~compas.datastructures.Mesh` + # Instance of a mesh data structure + # name : str, optional + # The name of the mesh to draw. + # color : :class:`~compas.colors.Color` + # The color of the object.` + + # Returns + # ------- + # :rhino:`Rhino.Geometry.Mesh` + # """ + # mesh = geometry # type: Mesh + # color = color.rgba255 if color else None + + # vertices, faces = geometry.to_vertices_and_faces(triangulated=False) + # mesh = draw_mesh(vertices, faces, color=color) + + # # Try to fix invalid meshes + # if not mesh.IsValid: + # mesh.FillHoles() + + # return mesh diff --git a/src/compas_fab/ghpython/scene/robot_cell_object.py b/src/compas_fab/ghpython/scene/robot_cell_object.py index f9be4bcc7..31b4754a8 100644 --- a/src/compas_fab/ghpython/scene/robot_cell_object.py +++ b/src/compas_fab/ghpython/scene/robot_cell_object.py @@ -1,10 +1,30 @@ # from compas.colors import ColorMap -# from compas.scene import SceneObject -from compas_ghpython.scene import GHSceneObject +from compas.scene import SceneObject from compas_fab.scene import BaseRobotCellObject +from .robot_model_object import RobotModelObject +from .rigid_body_object import RigidBodyObject -class RobotCellObject(BaseRobotCellObject, GHSceneObject): - """Scene object for drawing a RobotCell.""" +class RobotCellObject(BaseRobotCellObject): + """Scene object for drawing a RobotCell in GHPython.""" - pass + def __init__(self, *args, **kwargs): + super(RobotCellObject, self).__init__(*args, **kwargs) + + # Native Geometry handles + self.robot_model_scene_object = None # type: RobotModelObject + self.robot_model_scene_object = SceneObject( + item=self.robot_cell.robot_model, + sceneobject_type=RobotModelObject, + ) + + self.rigid_body_scene_objects = {} # type: dict[str, RigidBodyObject] + for id, rigid_body in self.robot_cell.rigid_body_models.items(): + self.rigid_body_scene_objects[id] = SceneObject( + item=rigid_body, + sceneobject_type=RigidBodyObject, + ) + + # self.tool_scene_objects = {} # type: dict[str, BaseToolObject] + # for id, tool in self.robot_cell.tool_models.items(): + # self.tool_scene_objects[id] = SceneObject(item=tool) diff --git a/src/compas_fab/ghpython/scene/robot_model_object.py b/src/compas_fab/ghpython/scene/robot_model_object.py new file mode 100644 index 000000000..5051c3e97 --- /dev/null +++ b/src/compas_fab/ghpython/scene/robot_model_object.py @@ -0,0 +1,11 @@ +# from compas.colors import ColorMap +# from compas.scene import SceneObject +from .gh_scene_object import GHSceneObject +from compas_fab.scene import BaseRobotModelObject + + +# Overrides the RobotModelObject in compas_robot +class RobotModelObject(GHSceneObject, BaseRobotModelObject): + """Scene object for drawing a Robot Model in GHPython.""" + + pass diff --git a/src/compas_fab/robots/state.py b/src/compas_fab/robots/state.py index 1ae5e05ce..24193a3b0 100644 --- a/src/compas_fab/robots/state.py +++ b/src/compas_fab/robots/state.py @@ -34,10 +34,18 @@ class RobotCellState(Data): """ - def __init__(self, robot_flange_frame, robot_configuration=None, tool_states=None, rigid_body_states=None): - # type: (Frame, Optional[Configuration], Dict[str, ToolState], Dict[str, RigidBodyState]) -> None + def __init__( + self, + robot_base_frame=None, + robot_flange_frame=None, + robot_configuration=None, + tool_states=None, + rigid_body_states=None, + ): + # type: (Optional[Frame], Optional[Frame], Optional[Configuration], Dict[str, ToolState], Dict[str, RigidBodyState]) -> None super(RobotCellState, self).__init__() - self.robot_flange_frame = robot_flange_frame # type: Frame + self.robot_base_frame = robot_base_frame or Frame.worldXY() # type: Frame + self.robot_flange_frame = robot_flange_frame or Frame.worldXY() # type: Frame self.robot_configuration = robot_configuration # type: Optional[Configuration] self.tool_states = tool_states or {} # type: Dict[str, ToolState] self.rigid_body_states = rigid_body_states or {} # type: Dict[str, RigidBodyState] @@ -55,6 +63,7 @@ def rigid_body_ids(self): @property def __data__(self): return { + "robot_base_frame": self.robot_base_frame, "robot_flange_frame": self.robot_flange_frame, "robot_configuration": self.robot_configuration, "tool_states": self.tool_states, @@ -65,6 +74,8 @@ def __eq__(self, value): # type: (RobotCellState) -> bool if value is None or not isinstance(value, RobotCellState): return False + if self.robot_base_frame != value.robot_base_frame: + return False if self.robot_flange_frame != value.robot_flange_frame: return False if self.robot_configuration: @@ -100,6 +111,7 @@ def from_robot_cell(cls, robot_cell, robot_configuration=None): This function ensures that all the tools and workpieces in the robot cell are represented in the robot cell state. This function should be called after the robot cell is created and all objects are added to it. + The robot's base frame will be assumed to be at worldXY frame. All tools will be assumed to be in their zero configuration and positioned at worldXY frame. All workpieces will be assumed to be in their base frame and not attached to any tool or link. All tools and workpieces are assumed to be visible in the scene (is_hidden=False). @@ -112,6 +124,7 @@ def from_robot_cell(cls, robot_cell, robot_configuration=None): The configuration of the robot. If the configuration is not provided, the robot's zero configuration will be used. """ robot_configuration = robot_configuration or robot_cell.zero_full_configuration() + base_frame = Frame.worldXY() flange_frame = robot_cell.robot_model.forward_kinematics(robot_configuration) tool_states = {} @@ -126,7 +139,7 @@ def from_robot_cell(cls, robot_cell, robot_configuration=None): rigid_body_state = RigidBodyState(Frame.worldXY(), None, None, None) rigid_body_states[rigid_body_id] = rigid_body_state - return cls(flange_frame, robot_configuration, tool_states, rigid_body_states) + return cls(base_frame, flange_frame, robot_configuration, tool_states, rigid_body_states) def get_attached_tool_id(self, group): # type: (str) -> Optional[str] diff --git a/src/compas_fab/scene/__init__.py b/src/compas_fab/scene/__init__.py index 62c6e7a0a..4966a8e0b 100644 --- a/src/compas_fab/scene/__init__.py +++ b/src/compas_fab/scene/__init__.py @@ -4,8 +4,10 @@ from .base_rigid_body_object import BaseRigidBodyObject from .base_robot_cell_object import BaseRobotCellObject +from .base_robot_model_object import BaseRobotModelObject __all__ = [ "BaseRigidBodyObject", "BaseRobotCellObject", + "BaseRobotModelObject", ] diff --git a/src/compas_fab/scene/base_robot_cell_object.py b/src/compas_fab/scene/base_robot_cell_object.py index 3e17f5ca2..a3980b633 100644 --- a/src/compas_fab/scene/base_robot_cell_object.py +++ b/src/compas_fab/scene/base_robot_cell_object.py @@ -1,5 +1,6 @@ from compas import IPY from compas.scene import SceneObject +from compas_fab.robots import RobotCellState if not IPY: from typing import TYPE_CHECKING @@ -8,13 +9,13 @@ from typing import List # noqa: F401 from typing import Optional # noqa: F401 - from compas_robots.scene import BaseRobotModelObject # noqa: F401 + from .base_robot_model_object import BaseRobotModelObject # noqa: F401 # from compas_fab.scene import BaseToolObject from compas_fab.robots import RigidBody # noqa: F401 from compas_fab.robots import RigidBodyState # noqa: F401 from compas_fab.robots import RobotCell # noqa: F401 - from compas_fab.robots import RobotCellState # noqa: F401 + from compas_fab.scene import BaseRigidBodyObject # noqa: F401 @@ -37,7 +38,6 @@ class BaseRobotCellObject(SceneObject): If caching function is not desired, simply remove the native geometry and create a new instance of the RobotCellObject every time the robot cell is drawn. - """ def __init__(self, draw_visual=True, draw_collision=False, scale=1.0, *args, **kwargs): @@ -47,16 +47,10 @@ def __init__(self, draw_visual=True, draw_collision=False, scale=1.0, *args, **k self._scale = scale # Native Geometry handles - - self.robot_scene_object = SceneObject(self.robot_cell.robot_model) # type: BaseRobotModelObject - + # robot_model_object = self._get_robot_model_object() + self.robot_model_scene_object = None # type: BaseRobotModelObject self.rigid_body_scene_objects = {} # type: dict[str, BaseRigidBodyObject] - for id, rigid_body in self.robot_cell.rigid_body_models.items(): - self.rigid_body_scene_objects[id] = SceneObject(rigid_body) - - self.tool_scene_objects = {} # type: dict[str, BaseToolObject] - for id, tool in self.robot_cell.tool_models.items(): - self.tool_scene_objects[id] = SceneObject(tool) + # self.tool_scene_objects = {} # type: dict[str, BaseToolObject] @property def robot_cell(self): @@ -70,32 +64,45 @@ def robot_cell(self): def draw(self, robot_cell_state=None): # type: (Optional[RobotCellState]) -> List[object] """Return all native geometry (in the CAD environment) belonging to the robot cell.""" - native_geometry = [] - if robot_cell_state: - self.update(robot_cell_state) - else: - self.robot_scene_object.draw() - for rigid_body_scene_object in self.rigid_body_scene_objects.values(): - rigid_body_scene_object.draw() - for tool_scene_object in self.tool_scene_objects.values(): - tool_scene_object.draw() - - def draw_rigid_bodies(self, robot_cell_state=None): - # type: (Optional[RobotCellState]) -> List[object] - """Return the native geometry (in the CAD environment) of the rigid bodies of the robot cell.""" - native_geometry = [] + native_geometries = [] + + # Default robot cell state if not provided + robot_cell_state = robot_cell_state if robot_cell_state else RobotCellState.from_robot_cell(self.robot_cell) + + # Draw the robot model + native_geometries.extend( + self.robot_model_scene_object.draw(robot_cell_state.robot_configuration, robot_cell_state.robot_base_frame) + ) + + # Draw the rigid bodies for id, rigid_body_scene_object in self.rigid_body_scene_objects.items(): rigid_body_state = robot_cell_state.rigid_body_states[id] if robot_cell_state else None - native_geometry += rigid_body_scene_object.draw(rigid_body_state) - return native_geometry - - def update(self, robot_cell_state): - # type: (RobotCellState) -> None - """Update the robot cell object with the given robot cell state.""" - # NOTE: All the constituent objects have an update method for transforming the native geometry - if robot_cell_state.robot_configuration: - self.robot_scene_object.update(robot_cell_state.robot_configuration) - for id, rigid_body_state in robot_cell_state.rigid_body_states.items(): - self.rigid_body_scene_objects[id].update(rigid_body_state) - for id, tool_state in robot_cell_state.tool_states.items(): - self.tool_scene_objects[id].update(tool_state) + native_geometries.extend(rigid_body_scene_object.draw(rigid_body_state)) + + # Draw the tools + # for tool_scene_object in self.tool_scene_objects.values(): + # native_geometries.extend(tool_scene_object.draw()) + + return native_geometries + + # -------------------------------------------------------------------------- + + # def draw_rigid_bodies(self, robot_cell_state=None): + # # type: (Optional[RobotCellState]) -> List[object] + # """Return the native geometry (in the CAD environment) of the rigid bodies of the robot cell.""" + # native_geometry = [] + # for id, rigid_body_scene_object in self.rigid_body_scene_objects.items(): + # rigid_body_state = robot_cell_state.rigid_body_states[id] if robot_cell_state else None + # native_geometry += rigid_body_scene_object.draw(rigid_body_state) + # return native_geometry + + # def update(self, robot_cell_state): + # # type: (RobotCellState) -> None + # """Update the robot cell object with the given robot cell state.""" + # # NOTE: All the constituent objects have an update method for transforming the native geometry + # if robot_cell_state.robot_configuration: + # self.robot_scene_object.update(robot_cell_state.robot_configuration) + # for id, rigid_body_state in robot_cell_state.rigid_body_states.items(): + # self.rigid_body_scene_objects[id].update(rigid_body_state) + # # for id, tool_state in robot_cell_state.tool_states.items(): + # # self.tool_scene_objects[id].update(tool_state) diff --git a/src/compas_fab/scene/base_robot_model_object.py b/src/compas_fab/scene/base_robot_model_object.py new file mode 100644 index 000000000..e52fd2df0 --- /dev/null +++ b/src/compas_fab/scene/base_robot_model_object.py @@ -0,0 +1,230 @@ +from compas import IPY +from compas.datastructures import Mesh +from compas.geometry import Frame +from compas.geometry import Transformation +from compas.scene import SceneObject +from compas_robots.model import MeshDescriptor + +if not IPY: + from typing import TYPE_CHECKING + + if TYPE_CHECKING: + from typing import List # noqa: F401 + from typing import Optional # noqa: F401 + + from compas_robots import Configuration # noqa: F401 + from compas_robots import RobotModel # noqa: F401 + from compas_robots.model import Collision # noqa: F401 + from compas_robots.model import Link # noqa: F401 + from compas_robots.model import Visual # noqa: F401 + + +class BaseRobotModelObject(SceneObject): + """Base class for compas_fab-special RobotModelObjects" """ + + MESH_JOIN_PRECISION = 12 + + def __init__(self, draw_visual=True, draw_collision=False, scale=1.0, *args, **kwargs): + # type: (Optional[bool], Optional[bool], Optional[float], *object, **object) -> None + super(BaseRobotModelObject, self).__init__(*args, **kwargs) + self._draw_visual = draw_visual + self._draw_collision = draw_collision + self._scale = scale + + # It will be filled when the `draw()` method is called for the first time. + self.links_visual_mesh_native_geometry = {} + self.links_collision_mesh_native_geometry = {} + # Dictionary to hold the current transformation of the robot links + self.links_visual_mesh_transformation = {} + self.links_collision_mesh_transformation = {} + + @property + def robot_model(self): + # type: () -> RobotModel + """The robot model this object is associated with.""" + return self.item + + def _extract_link_geometry(self, link_elements): + # type: (List[Visual] | List[Collision]) -> Mesh + """Extracts the geometry from a link's Visual or Collision elements and joins them into a single mesh. + + 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 + ------- + :class:`~compas.datastructures.Mesh` | None + The joined mesh of the link elements. + None if the link elements do not contain any mesh. + """ + if not link_elements: + return None + + joined_mesh = Mesh() + # Note: Each Link can have multiple visual nodes + for element in link_elements: + # Some elements may have a non-identity origin + origin = element.origin.to_compas_frame() + t_origin = Transformation.from_frame(origin) + shape = element.geometry.shape + # Note: the MeshDescriptor.meshes object supports a list of compas meshes. + if isinstance(shape, MeshDescriptor): + # Join multiple meshes + for mesh in shape.meshes: + transformed_mesh = mesh.transformed(t_origin) if t_origin != Transformation() else mesh + joined_mesh.join(transformed_mesh, False, precision=self.MESH_JOIN_PRECISION) + + return None if joined_mesh.is_empty() else joined_mesh + + def _initial_draw(self): + """Creating the native geometry when `draw()` method is called for the first time""" + # Reset the dictionaries + self.base_native_geometry = None + self.base_transformation = None + self.links_visual_mesh_native_geometry = {} # type: dict[str, Mesh] + self.links_collision_mesh_native_geometry = {} # type: dict[str, Mesh] + self.links_visual_mesh_transformation = {} # type: dict[str, Transformation] + self.links_collision_mesh_transformation = {} # type: dict[str, Transformation] + + # Iterate over the links and create the visual and collision meshes. + # Only create the geometry if it exists. + for link in self.robot_model.iter_links(): + link_name = link.name + # CREATE VISUAL MESHES + if self._draw_visual: + visual_mesh = self._extract_link_geometry(link.visual) + if visual_mesh: + self.links_visual_mesh_native_geometry[link_name] = self._create_geometry( + visual_mesh, name=link_name + "_visual" + ) + self.links_visual_mesh_transformation[link_name] = Transformation() + # CREATE COLLISION MESHES + if self._draw_collision: + collision_mesh = self._extract_link_geometry(link.collision) + if collision_mesh: + self.links_collision_mesh_native_geometry[link_name] = self._create_geometry( + collision_mesh, name=link_name + "_collision" + ) + self.links_collision_mesh_transformation[link_name] = Transformation() + + def draw(self, robot_configuration=None, base_frame=None): + # type: (Optional[Configuration], Optional[Frame]) -> List[object] + """Draw the robot model object in the respective CAD environment. + It will return the native geometry objects that were created. + """ + # Create the native geometry when the `draw()` method is called for the first time + if (not self.links_visual_mesh_native_geometry) and (not self.links_collision_mesh_native_geometry): + self._initial_draw() + + # Update the robot model if a configuration is provided + if robot_configuration: + self.update(robot_configuration, base_frame) + + # Return the native geometry + native_geometry = [] + if self._draw_visual: + native_geometry.extend(self.links_visual_mesh_native_geometry.values()) + if self._draw_collision: + native_geometry.extend(self.links_collision_mesh_native_geometry.values()) + return native_geometry + + def update(self, robot_configuration, base_frame=None): + # type: (Configuration, Optional[Frame]) -> None + print("Base Frame: ", base_frame) + if len(self.robot_model.get_configurable_joints()) == 0: + return + + def _update_link_meshes(link_name, new_transformation): + if link_name in self.links_visual_mesh_native_geometry: + # Compute the delta transformation + previous_transformation = self.links_visual_mesh_transformation[link_name] + delta_transformation = new_transformation * previous_transformation.inverse() # type: Transformation + # Transform the native geometry + native_geometry = self.links_visual_mesh_native_geometry[link_name] + new_native_geometry = self._transform(native_geometry, delta_transformation) + # Update the dictionaries + self.links_visual_mesh_native_geometry[link_name] = new_native_geometry + self.links_visual_mesh_transformation[link_name] = new_transformation + print( + "Updated visual link '{}' from previous '{}' with delta '{}' to new transformation '{}'.".format( + link_name, previous_transformation, delta_transformation, new_transformation + ) + ) + if link_name in self.links_collision_mesh_native_geometry: + # Compute the delta transformation + previous_transformation = self.links_collision_mesh_transformation[link_name] + delta_transformation = new_transformation * previous_transformation.inverse() + # Transform the native geometry + native_geometry = self.links_collision_mesh_native_geometry[link_name] + new_native_geometry = self._transform(native_geometry, delta_transformation) + # Update the dictionaries + self.links_collision_mesh_native_geometry[link_name] = new_native_geometry + self.links_collision_mesh_transformation[link_name] = new_transformation + + # Use the robot base frame if it is provided + t_wcf_rcf = Transformation.from_frame(base_frame) if base_frame else Transformation() + + # Update the base link + _update_link_meshes(self.robot_model.root.name, t_wcf_rcf) + + # Iterate over the joints (equivalent to all the child links) and update their pose + # This iteration order is the same as the result from transform_frames() + transformed_joint_frames = self.robot_model.transformed_frames(robot_configuration) + for joint_frame, joint in zip(transformed_joint_frames, list(self.robot_model.iter_joints())): + # From Robot Coordinate Frame to Link Coordinate Frame (Link Frame == Joint Frame) + t_rcf_lcf = Transformation.from_frame(joint_frame) + # From World Coordinate Frame to Link Coordinate Frame + t_wcf_lcf = t_wcf_rcf * t_rcf_lcf + _update_link_meshes(joint.child_link.name, t_wcf_lcf) + + # -------------------------------------------------------------------------- + # Private methods that need to be implemented by CAD specific classes + # -------------------------------------------------------------------------- + + def _transform(self, geometry, transformation): + """Transforms the given native CAD-specific geometry. + + Here we do not assume whether that the CAD-specific `._transform` method will operate on the native geometry + in place, or creating a new object. This is up to the CAD-specific implementation. + The `._transform` method should return the transformed geometry object, whether it is a new object or the same object. + + Parameters + ---------- + geometry : object + A CAD-specific (i.e. native) geometry object as returned by :meth:`_create_geometry`. + transformation : :class:`~compas.geometry.Transformation` + Transformation to update the geometry object. + + Returns + ------- + object + The transformed geometry object. + + """ + raise NotImplementedError + + def _create_geometry(self, geometry, name=None, color=None): + """Create new native geometry in the respective CAD environment. + + Parameters + ---------- + geometry : :class:`~compas.datastructures.Mesh` + Instance of a mesh data structure + name : str, optional + The name of the mesh to draw. + color : :class:`~compas.colors.Color` + The color of the object.` + + Returns + ------- + object + CAD-specific geometry + + Notes + ----- + This is an abstract method that needs to be implemented by derived classes. + + """ + raise NotImplementedError