WIP for setting robot cell and cell state

docs/examples/05_backends_pybullet/files/01_set_robot_cell.py

@@ -0,0 +1,21 @@
+from compas.datastructures import Mesh
+
+import compas_fab
+from compas_fab.backends import PyBulletClient
+from compas_fab.backends import PyBulletPlanner
+from compas_fab.robots import RobotCell
+from compas_fab.robots import RigidBody
+
+with PyBulletClient() as client:
+    urdf_filepath = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
+    robot = client.load_robot(urdf_filepath)
+
+    robot_cell = RobotCell(robot)
+    floor_mesh = Mesh.from_stl(compas_fab.get("planning_scene/floor.stl"))
+    robot_cell.rigid_body_models["floor"] = RigidBody(floor_mesh)
+
+    planner = PyBulletPlanner(client)
+    planner.set_robot_cell(robot_cell)
+    # The floor should appear in the PyBullet's GUI
+
+    input("Press Enter to continue...")

src/compas_fab/backends/interfaces/__init__.py

@@ -56,6 +56,8 @@
 from .backend_features import RemoveAttachedCollisionMesh
 from .backend_features import RemoveCollisionMesh
 from .backend_features import ResetPlanningScene
+from .backend_features import SetRobotCell
+from .backend_features import SetRobotCellState
 from .client import ClientInterface
 from .client import PlannerInterface
 
@@ -74,4 +76,6 @@
     "RemoveCollisionMesh",
     "RemoveAttachedCollisionMesh",
     "ResetPlanningScene",
+    "SetRobotCell",
+    "SetRobotCellState",
 ]

src/compas_fab/backends/interfaces/backend_features.py

@@ -17,6 +17,7 @@
         from compas_robots import Configuration  # noqa: F401
         from compas.geometry import Frame  # noqa: F401
         from compas_fab.backends.interfaces import ClientInterface  # noqa: F401
+        from compas_fab.robots import RobotCell  # noqa: F401
         from compas_fab.robots import RobotCellState  # noqa: F401
         from compas_fab.robots import FrameTarget  # noqa: F401
 
@@ -35,6 +36,13 @@ def __init__(self, client=None):
         # All backend features are assumed to be associated with a backend client.
         if client:
             self.client = client  # type: ClientInterface
+
+        # The current robot cell last set by user calling Planner.set_robot_cell()
+        self.robot_cell = None  # type: RobotCell
+
+        # The current robot cell state last set by user calling Planner.set_robot_cell_state()
+        self.robot_cell_state = None  # type: RobotCellState
+
         super(BackendFeature, self).__init__()
 
 
@@ -45,6 +53,39 @@ def __init__(self, client=None):
 #   does not support that feature.
 
 
+class SetRobotCell(BackendFeature):
+    """Mix-in interface for implementing a planner's set robot cell feature."""
+
+    def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None):
+        # type: (RobotCell, Optional[RobotCellState], Optional[Dict]) -> None
+        """Pass the models in the robot cell to the planning client.
+
+        The client keeps the robot cell models in memory and uses them for planning.
+        Calling this method will override the previous robot cell in the client.
+        It should be called only if the robot cell models have changed.
+
+        """
+        pass
+
+
+class SetRobotCellState(BackendFeature):
+    """Mix-in interface for implementing a planner's set robot cell state feature."""
+
+    def set_robot_cell_state(self, robot_cell_state):
+        # type: (RobotCellState) -> None
+        """Set the robot cell state to the client.
+
+        The client requires a robot cell state at the beginning of each planning request.
+        This cell state must correspond to the robot cell set earlier by :meth:`set_robot_cell`.
+
+        This function is called automatically by planning functions that takes a start_state as input.
+        Therefore it is typically not necessary for the user to call this function,
+        except when used to trigger visualization using native backend canvas.
+
+        """
+        pass
+
+
 class ForwardKinematics(BackendFeature):
     """Mix-in interface for implementing a planner's forward kinematics feature."""
 

src/compas_fab/backends/interfaces/client.py

@@ -12,6 +12,7 @@
     if TYPE_CHECKING:
         from compas_fab.robots import Robot
         from compas_fab.robots import RobotCell
+        from compas_fab.robots import RobotCellState
 
 
 class ClientInterface(object):
@@ -29,19 +30,13 @@ class ClientInterface(object):
 
     def __init__(self):
         self._robot = None  # type: Robot
-        self._robot_cell = None  # type: RobotCell
         print("ClientInterface init")
 
     @property
     def robot(self):
         # type: () -> Robot
         return self._robot
 
-    @property
-    def robot_cell(self):
-        # type: () -> RobotCell
-        return self._robot_cell
-
 
 class PlannerInterface(object):
     """Interface for all backend planners.
@@ -62,18 +57,72 @@ class PlannerInterface(object):
     client : :class:`compas_fab.backends.interfaces.ClientInterface`, read-only
         The client instance associated with the planner.
         It cannot be changed after initialization.
+
+    robot_cell : :class:`compas_fab.robots.RobotCell`, read-only
+        The robot cell instance previously passed to the client.
+
+    robot_cell_state : :class:`compas_fab.robots.RobotCellState`, read-only
+        The last robot cell state instance passed to the client.
+
+
     """
 
     def __init__(self, client=None):
         if client:
             self._client = client
-
+        self._robot_cell = None  # type: RobotCell
+        self._robot_cell_state = None  # type: RobotCellState
         super(PlannerInterface, self).__init__()
 
     @property
     def client(self):
         return self._client
 
+    @property
+    def robot_cell(self):
+        # type: () -> RobotCell
+        return self._robot_cell
+
+    @property
+    def robot_cell_state(self):
+        # type: () -> RobotCellState
+        return self._robot_cell_state
+
+    # ==========================================================================
+    # collision objects robot cell and cell state
+    # ==========================================================================
+
+    def set_robot_cell(self, *args, **kwargs):
+        """Pass the models in the robot cell to the planning client.
+
+        The client keeps the robot cell models in memory and uses them for planning.
+        Calling this method will override the previous robot cell in the client.
+        It should be called only if the robot cell models have changed.
+
+        Raises
+        ------
+        BackendFeatureNotSupportedError
+            Planner does not have this feature.
+        """
+        raise BackendFeatureNotSupportedError("Assigned planner does not have this feature.")
+
+    def set_robot_cell_state(self, *args, **kwargs):
+        """Set the robot cell state to the client.
+
+        The client requires a robot cell state at the beginning of each planning request.
+        This cell state must correspond to the robot cell set earlier by :meth:`set_robot_cell`.
+
+        This function is called automatically by planning functions that takes a start_state as input.
+        Therefore it is typically not necessary for the user to call this function,
+        except when used to trigger visualization using native backend canvas.
+
+        Raises
+        ------
+        BackendFeatureNotSupportedError
+            Planner does not have this feature.
+        """
+        raise BackendFeatureNotSupportedError("Assigned planner does not have this feature.")
+
     # ==========================================================================
     # planning services
     # ==========================================================================

src/compas_fab/backends/pybullet/backend_features/__init__.py

@@ -32,6 +32,7 @@
     PyBulletRemoveAttachedCollisionMesh,
 )
 from compas_fab.backends.pybullet.backend_features.pybullet_remove_collision_mesh import PyBulletRemoveCollisionMesh
+from compas_fab.backends.pybullet.backend_features.pybullet_set_robot_cell import PyBulletSetRobotCell
 
 
 __all__ = [
@@ -42,4 +43,5 @@
     "PyBulletInverseKinematics",
     "PyBulletRemoveAttachedCollisionMesh",
     "PyBulletRemoveCollisionMesh",
+    "PyBulletSetRobotCell",
 ]

src/compas_fab/backends/pybullet/backend_features/pybullet_set_robot_cell.py

@@ -0,0 +1,59 @@
+from compas_fab.backends.interfaces import SetRobotCell
+
+import compas
+
+if not compas.IPY:
+    from typing import TYPE_CHECKING
+
+    if TYPE_CHECKING:
+        from typing import Optional  # noqa: F401
+        from typing import Dict  # noqa: F401
+        from typing import List  # noqa: F401
+        from typing import Tuple  # noqa: F401
+
+        from compas_fab.robots import Robot  # noqa: F401
+        from compas_robots import Configuration  # noqa: F401
+        from compas.geometry import Frame  # noqa: F401
+        from compas_fab.backends.interfaces import ClientInterface  # noqa: F401
+        from compas_fab.robots import RobotCell  # noqa: F401
+        from compas_fab.robots import RobotCellState  # noqa: F401
+        from compas_fab.backends import PyBulletClient  # noqa: F401
+
+from compas_fab.backends.pybullet.const import STATIC_MASS
+
+
+class PyBulletSetRobotCell(SetRobotCell):
+
+    def set_robot_cell(self, robot_cell, robot_cell_state=None, options=None):
+        # type: (RobotCell, Optional[RobotCellState], Optional[Dict]) -> None
+        """Pass the models in the robot cell to the Pybullet client.
+
+        The client keeps the robot cell models in memory and uses them for planning.
+        Calling this method will override the previous robot cell in the client.
+        It should be called only if the robot cell models have changed.
+
+        """
+        client = self.client  # type: PyBulletClient
+
+        # TODO: Check for new, modified and removed objects compared to the
+        # TODO: previous robot cell state and update the PyBullet world accordingly
+
+        # At the moment, all previous object in the PyBullet world are removed
+        # and the new robot cell is added to the PyBullet world
+
+        # Remove all objects from the PyBullet world
+        client.remove_all_tools()
+        client.remove_all_rigid_bodies()
+
+        # Add the robot cell to the PyBullet world
+        for name, tool in robot_cell.tool_models.items():
+            client.add_tool(name, tool.mesh, tool.mass)
+        for name, rigid_body in robot_cell.rigid_body_models.items():
+            client.add_rigid_body(name, rigid_body.mesh, rigid_body.mass)
+
+        # Update the robot cell in the client
+        self._robot_cell = robot_cell
+
+        # If a robot cell state is provided, update the client's robot cell state
+        if robot_cell_state:
+            self.set_robot_cell_state(robot_cell_state, options)

src/compas_fab/backends/ros/backend_features/move_it_inverse_kinematics.py

@@ -142,6 +142,10 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options=
         ------
         :obj:`tuple` of (:obj:`list` of :obj:`float`, :obj:`list` of :obj:`str`)
             A tuple of 2 elements containing a list of joint positions and a list of matching joint names.
+
+        Examples
+        --------
+
         """
         options = options or {}
         robot = self.client.robot
@@ -151,7 +155,7 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options=
 
         # Set scene
         # TODO: Implement start_state
-        self.client.robot_cell.assert_cell_state_match(start_state)
+        self.robot_cell.assert_cell_state_match(start_state)
         start_state = start_state or RobotCellState.from_robot_configuration(robot)
 
         # Start configuration
@@ -164,15 +168,17 @@ def iter_inverse_kinematics(self, target, start_state=None, group=None, options=
         if robot.need_scaling:
             target_frame = target_frame.scaled(1.0 / robot.scale_factor)
             # Now target_frame is back in meter scale
-        attached_tool = self.client.robot_cell.get_attached_tool(start_state, group)
+        attached_tool = self.robot_cell.get_attached_tool(start_state, group)
         if attached_tool:
             target_frame = from_tcf_to_t0cf(target_frame, attached_tool.frame)
             # Attached tool frames does not need scaling because Tools are modelled in meter scale
 
         # Scale Attached Collision Meshes
+        attached_collision_meshes = self.robot_cell.get_attached_rigid_bodies_as_attached_collision_meshes(
+            start_state, group
+        )
         if robot.need_scaling:
-            acms = options.get("attached_collision_meshes", [])  # type: List[AttachedCollisionMesh]
-            for acm in acms:
+            for acm in attached_collision_meshes:
                 acm.collision_mesh.scale(1.0 / robot.scale_factor)
 
         # Compose the kwargs for the async function