Polishing the CC backend

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

@@ -7,6 +7,7 @@
 from compas_fab.robots import RobotCell
 from compas_fab.robots import RobotCellState
 from compas_fab.robots import RobotCellLibrary
+from compas_fab.backends import CollisionCheckError
 
 
 # #############################################
@@ -16,27 +17,64 @@
 
 with PyBulletClient("gui") as client:
     planner = PyBulletPlanner(client)
+    print(f"Observe the Pybullet GUI window to see the robot cell state being checked")
 
     # The robot cell is loaded from RobotCellLibrary
     robot_cell, robot_cell_state = RobotCellLibrary.ur10e_gripper_one_beam()
     planner.set_robot_cell(robot_cell)
 
+    # ---------------------------------------------
+    # Trial 1 - No collision
+    # ---------------------------------------------
+
     # This configuration is not in collision
-    robot_cell_state.robot_configuration.joint_values = [0, -2.0, 2.0, 0, 0, 0]
+    robot_cell_state.robot_configuration.joint_values = [0, -1.5, 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...")
+    input("Press Enter to continue...\n")
+
+    # ---------------------------------------------
+    # Trial 2 - Collision between beam and floor
+    # ---------------------------------------------
 
-    # This configuration is in collision
-    robot_cell_state.robot_configuration.joint_values = [0, 0.0, 2.0, 0, 0, 0]
+    robot_cell_state.robot_configuration.joint_values = [0, -1.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...")
+        planner.check_collision(robot_cell_state)
+    except CollisionCheckError as e:
+        print(f"\nCollision detected: \n{e}")
+    input("Press Enter to continue...\n")
+
+    # ---------------------------------------------
+    # Trial 3 - Multiple Collisions
+    # ---------------------------------------------
+
+    robot_cell_state.robot_configuration.joint_values = [0, -0.8, 2.0, 0, 0, 0]
+
+    # The following check_collision should raise an exception
+    # The `full_report` option will return all failed collision pairs in the exception message
+    try:
+        planner.check_collision(robot_cell_state, options={"full_report": True})
+    except CollisionCheckError as e:
+        print(f"\nCollision detected: \n{e}")
+    input("Press Enter to continue...\n")
+
+    # ---------------------------------------------
+    # Trial 4 - Verbose Mode
+    # ---------------------------------------------
+
+    robot_cell_state.robot_configuration.joint_values = [0, -0.7, 2.0, 0, 0, 0]
+
+    # The following check_collision should raise an exception
+    # The `full_report` option will print all failed collision pairs`
+    try:
+        planner.check_collision(robot_cell_state, options={"verbose": True, "full_report": True})
+    except CollisionCheckError as e:
+        print(f"\nCollision detected: \n{e}")
+    input("Press Enter to continue...\n")
+
+    # The verbose action will print all tested collision pairs

src/compas_fab/backends/interfaces/backend_features.py

@@ -21,6 +21,7 @@
         from compas_fab.robots import RobotCellState  # noqa: F401
         from compas_fab.robots import FrameTarget  # noqa: F401
         from compas_fab.robots import Target  # noqa: F401
+        from compas_fab.robots import Waypoint  # noqa: F401
 
 
 class BackendFeature(object):
@@ -48,6 +49,18 @@ def _scale_input_target(self, target):
 
         return target
 
+    def _scale_input_waypoint(self, waypoint):
+        # type: (Waypoint) -> Waypoint
+        """Scale the waypoint if the robot has user defined scale."""
+        robot = self.robot_cell.robot  # type: Robot
+
+        # Check `robot.need_scaling` to avoid unnecessary scaling
+        if robot.need_scaling:
+            # Scale input target back to meter scale
+            return waypoint.scaled(1.0 / robot.scale_factor)
+
+        return waypoint
+
     def _scale_output_frame(self, frame):
         # type: (Frame) -> Frame
         """Scale the frame if the robot has user defined scale."""
@@ -120,31 +133,19 @@ def set_robot_cell_state(self, robot_cell_state):
 class CheckCollision(BackendFeature):
     """Mix-in interface for implementing a planner's collision check feature."""
 
-    def check_collision(self, robot_cell_state, allowed_collision_pairs=[]):
-        # type: (RobotCellState, List[Tuple[str,str]]) -> None
+    def check_collision(self, robot_cell_state, options=None):
+        # type: (RobotCellState, Optional[dict]) -> None
         """Check if the robot cell is in collision at the specified state.
 
-
-        Collision checking typically involves checking for collisions between
-        (1) the robot's links, a.k.a. self-collision;
-        (2) the robot's links and the attached objects;
-        (3) the robot's links and stationary objects;
-        (4) the attached objects and the stationary objects.
-        (5) the attached objects and other attached objects.
-        Where the attached and stationary objects refers to environment rigid bodies, tools and workpieces.
-
         Different planners may have different criteria for collision checking, check the planner's documentation for details.
 
         Parameters
         ----------
         robot_cell_state : :class:`compas_fab.robots.RobotCellState`
             The robot cell state to check for collision.
-        allowed_collision_pairs : :obj:`list` of :obj:`tuple` of (str, str)
-            Collision pairs that are allowed to collide.
-            The pairs are defined by the names of the collision objects.
-            - For robot links, the names are the link names of the robot prefixed by the robot's name.
-              For example, a robot named "ur5" with a link named "link" will have the name "robot/link".
-
+        options : dict, optional
+            Dictionary containing kwargs for arguments specific to
+            the client being queried.
 
         Returns
         -------