GH SceneObject for RigidBody, RobotModel and RobotCell

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",
 ]

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

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

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)

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

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]

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",
 ]