PyBullet Check Collision Function - Not Tested

docs/examples/05_backends_pybullet/files/02_forward_kinematics.py

@@ -35,6 +35,7 @@
 
     print("Robot flange frame of the default planning group in the world coordinate system:")
     print(frame_WCF)
+    print(" ")
 
     # ---------------------------------
     # FK for all the links in the robot

docs/examples/05_backends_pybullet/files/03_check_collision.py

@@ -0,0 +1,42 @@
+import time
+
+import compas_fab
+from compas_robots import Configuration
+from compas_fab.backends import PyBulletClient
+from compas_fab.backends import PyBulletPlanner
+from compas_fab.robots import RobotCell
+from compas_fab.robots import RobotCellState
+from compas_fab.robots import RobotCellLibrary
+
+
+# #############################################
+# Pybullet Check Collision Example
+# #############################################
+
+
+with PyBulletClient("gui") as client:
+    planner = PyBulletPlanner(client)
+
+    # The robot cell is loaded from RobotCellLibrary
+    robot_cell, robot_cell_state = RobotCellLibrary.ur10e_gripper_one_beam()
+    planner.set_robot_cell(robot_cell)
+
+    # This configuration is not in collision
+    robot_cell_state.robot_configuration.joint_values = [0, -2.0, 2.0, 0, 0, 0]
+
+    # The following check_collision should pass without raising an exception
+    start = time.time()
+    planner.check_collision(robot_cell_state)
+    print(f"Time taken for collision check: {time.time() - start}")
+    input("Press Enter to continue...")
+
+    # This configuration is in collision
+    robot_cell_state.robot_configuration.joint_values = [0, 0.0, 2.0, 0, 0, 0]
+
+    # The following check_collision should raise an exception
+    # The verbose action will print all tested collision pairs
+    try:
+        planner.check_collision(robot_cell_state, options={"verbose": True})
+    except Exception as e:
+        print(f"Collision detected: {e}")
+    input("Press Enter to continue...")

src/compas_fab/backends/__init__.py

@@ -113,6 +113,7 @@
     InverseKinematicsError,
     KinematicsError,
     CollisionCheckInCollisionError,
+    CollisionCheckError,
 )
 
 from .tasks import (
@@ -168,6 +169,7 @@
     "InverseKinematicsError",
     "KinematicsError",
     "CollisionCheckInCollisionError",
+    "CollisionCheckError",
     "FutureResult",
     "CancellableFutureResult",
     # ROS

src/compas_fab/backends/exceptions.py

@@ -7,6 +7,8 @@
     "BackendFeatureNotSupportedError",
     "InverseKinematicsError",
     "KinematicsError",
+    "CollisionCheckInCollisionError",
+    "CollisionCheckError",
 ]
 
 
@@ -52,6 +54,13 @@ def __init__(self):
 # -------------------------
 
 
+class CollisionCheckError(BackendError):
+    """Indicates a collision check exception."""
+
+    def __init__(self, message):
+        super(CollisionCheckError, self).__init__(message)
+
+
 class CollisionCheckInCollisionError(BackendError):
     """Indicates a collision between two objects is detected during a collision check.
 
@@ -74,8 +83,8 @@ class CollisionCheckInCollisionError(BackendError):
         Name of the second object.
     """
 
-    def __init__(self, object1_type, object1_name, object2_type, object2_name):
-        # type(int, str, int, str) -> None
+    def __init__(self, object1_name, object2_name, object1_type=None, object2_type=None):
+        # type(str, str, Optional[int], Optional[int]) -> None
         message = "Collision between '{}' and '{}'".format(object1_name, object2_name)
         super(CollisionCheckInCollisionError, self).__init__(message)
         self.object1_type = object1_type

src/compas_fab/backends/kinematics/backend_features/analytical_pybullet_inverse_kinematics.py

@@ -79,7 +79,7 @@ def iter_inverse_kinematics_frame_target(self, target, start_state=None, group=N
             if options.get("check_collision", False):
                 try:
                     joint_values, joint_names = solution
-                    client.check_collisions(robot, Configuration.from_revolute_values(joint_values, joint_names))
+                    client.check_collisions(Configuration.from_revolute_values(joint_values, joint_names))
                 except BackendError as e:
                     # If keep order is true, yield (None, None) to keep the order of the solutions
                     if options.get("keep_order", False):

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

@@ -20,16 +20,18 @@
 
 from __future__ import absolute_import
 
-
+from compas_fab.backends.pybullet.backend_features.pybullet_check_collision import PyBulletCheckCollision
 from compas_fab.backends.pybullet.backend_features.pybullet_forward_kinematics import PyBulletForwardKinematics
 from compas_fab.backends.pybullet.backend_features.pybullet_inverse_kinematics import PyBulletInverseKinematics
 from compas_fab.backends.pybullet.backend_features.pybullet_set_robot_cell import PyBulletSetRobotCell
 from compas_fab.backends.pybullet.backend_features.pybullet_set_robot_cell_state import PyBulletSetRobotCellState
-
+from compas_fab.backends.pybullet.backend_features.pybullet_plan_cartesian_motion import PyBulletPlanCartesianMotion
 
 __all__ = [
+    "PyBulletCheckCollision",
     "PyBulletForwardKinematics",
     "PyBulletInverseKinematics",
+    "PyBulletPlanCartesianMotion",
     "PyBulletSetRobotCell",
     "PyBulletSetRobotCellState",
 ]

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

@@ -0,0 +1,90 @@
+from itertools import combinations
+
+import compas
+
+from compas.geometry import Frame
+from compas_fab.backends.interfaces import CheckCollision
+from compas_fab.backends import CollisionCheckInCollisionError
+
+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 import PyBulletPlanner  # noqa: F401
+
+    import pybullet
+from compas_fab.backends.pybullet.const import STATIC_MASS
+
+
+class PyBulletCheckCollision(CheckCollision):
+
+    # =======================================
+    def check_collision(self, state, group=None, options=None):
+        # type: (RobotCellState, Optional[str], Optional[Dict]) -> None
+        """Check if the robot is in collision.
+
+        The collision check involves the following steps:
+
+        1. Check for collisions between each robot link.
+        2. Check for collisions between each robot link and each tool.
+        3. Check for collisions between each robot link and each rigid body.
+        4. Check for collisions between each attached rigid body and all other rigid bodies.
+        5. Check for collisions between each tool and each rigid body.
+
+        In each of the above steps, the collision check is skipped if the collision is allowed
+        by the semantics of the robot, tool, or rigid body. For details, see in-line comments in code.
+
+
+        A collision report is returned with the CollisionCheckError exception message.
+        Typically, the exception is raised on the first collision detected and the rest of
+        the collision pairs are not checked.
+        If ``full_report`` is set to ``True``, the exception will be raised after all
+        collision pairs are checked, and the report will contain all failing collision pairs.
+        This can be useful for debugging and visualization.
+
+        Parameters
+        ----------
+        state : :class:`compas_fab.robots.RobotCellState`
+            The robot cell state describing the robot cell.
+            The attribute `robot_configuration`, must contain the full configuration of the robot corresponding to the planning group.
+        group : str, optional
+            The planning group used for collision checking. Defaults to the robot's main planning group.
+        options : dict, optional
+            Dictionary containing the following key-value pairs:
+
+            - ``"verbose"``: (:obj:`bool`, optional) When ``True``, additional information is printed.
+              Defaults to ``False``
+            - ``"full_report"``: (:obj:`bool`, optional) When ``True``, all collision pairs are checked
+              even when one encountered a collision. Defaults to ``False``
+
+        Raises
+        ------
+        CollisionCheckInCollisionError
+            If the robot is in collision.
+        """
+        options = options or {}
+
+        # Housekeeping for intellisense
+        planner = self  # type: PyBulletPlanner
+        client = planner.client  # type: PyBulletClient
+        robot = client.robot  # type: Robot
+
+        # Set the robot cell state
+        planner.set_robot_cell_state(state)
+
+        # Call the collision checking function from client
+        verbose = options.get("verbose", False)
+        full_report = options.get("full_report", False)
+        client.check_collisions(verbose=verbose, full_report=full_report)

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

@@ -20,7 +20,7 @@
 
 
 class PyBulletForwardKinematics(ForwardKinematics):
-    """Callable to calculate the robot's forward kinematic."""
+    """Mix-in function to calculate the robot's forward kinematic."""
 
     def forward_kinematics(self, robot_cell_state, group=None, options=None):
         # type: (RobotCellState, Optional[str], Optional[dict]) -> Frame
@@ -80,7 +80,7 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None):
 
         # Check for collisions if requested, it will throw an exception if the robot is in collision
         if options.get("check_collision"):
-            client.check_collisions(robot, configuration)
+            client.check_collisions(configuration)
 
         # If a link name provided, return the frame of that link
         link_name = options.get("link")

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

@@ -45,7 +45,7 @@
 
 
 class PyBulletInverseKinematics(InverseKinematics):
-    """Callable to calculate the robot's inverse kinematics for a given frame."""
+    """Mix-in functions to calculate the robot's inverse kinematics for a given target."""
 
     def iter_inverse_kinematics(self, target, start_state=None, group=None, options=None):
         # type: (Target, Optional[RobotCellState], Optional[str], Optional[Dict]) -> Generator[Tuple[List[float], List[str]], None, None]

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

@@ -112,6 +112,9 @@ def set_robot_cell_state(self, robot_cell_state):
                 rigid_body_base_frame = rigid_body_state.frame
             client.set_rigid_body_base_frame(rigid_body_name, rigid_body_base_frame)
 
+        # The client needs to keep track of the latest robot cell state
+        client._robot_cell_state = robot_cell_state
+
         # This function updates the position of all models in the robot cell, technically we could
         # keep track of the previous state and only update the models that have changed to improve performance.
         # We can improve when we have proper profiling and performance tests.