Preparing for PybulletPlanCartesianMotion

docs/examples/05_backends_pybullet/files/04_plan_cartesian_motion.py

@@ -0,0 +1,42 @@
+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 RobotLibrary
+
+
+# #############################################
+# Headless (no-GUI) forwards kinematics example
+# #############################################
+
+# 'direct' mode
+with PyBulletClient("direct") as client:
+    # The robot in this example is loaded from a URDF file
+    robot = RobotLibrary.ur5()
+    planner = PyBulletPlanner(client)
+
+    # This is a simple robot cell with only the robot
+    robot_cell = RobotCell(robot)
+    planner.set_robot_cell(robot_cell)
+
+    # Create the starting configuration
+    configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.0])
+    # The `RobotCellState.from_robot_configuration` method can be used when the robot is the only element in the cell
+    robot_cell_state = RobotCellState.from_robot_configuration(robot, configuration)
+    # In this demo, the default planning group is used for the forward kinematics
+    frame_WCF = planner.forward_kinematics(robot_cell_state)
+
+# Set the start and goal frames
+start_frame = Frame([0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0])
+goal_frame = Frame([1.0, 1.0, 1.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0])
+
+# Create a planner
+planner = client.get_planner()
+
+# Plan a cartesian motion using the pybullet backend
+trajectory = planner.plan_cartesian_motion(robot, start_frame, goal_frame)
+
+# Execute the planned trajectory
+client.execute_trajectory(robot, trajectory)

src/compas_fab/backends/exceptions.py

@@ -57,6 +57,8 @@ def __init__(self):
 class CollisionCheckError(BackendError):
     """Indicates a collision check exception."""
 
+    # TODO: Rewrite this to be a collection of CC errors
+
     def __init__(self, message):
         super(CollisionCheckError, self).__init__(message)
 
@@ -73,16 +75,18 @@ class CollisionCheckInCollisionError(BackendError):
 
     Attributes
     ----------
-    object1_type : str
+    object1_type : int
         Type of the first object.
     object1_name : str
         Name of the first object.
-    object2_type : str
+    object2_type : int
         Type of the second object.
     object2_name : str
         Name of the second object.
     """
 
+    # TODO: Rename this class, add the type int as constants and add to a string mapping
+
     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)

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

@@ -1,27 +1,23 @@
-from compas_fab.backends.exceptions import BackendError
-from compas_fab.backends.exceptions import BackendTargetNotSupportedError
-from compas_fab.backends.exceptions import InverseKinematicsError
-from compas_fab.backends.interfaces import InverseKinematics
-from .analytical_inverse_kinematics import AnalyticalInverseKinematics
 import compas
 
+from .analytical_inverse_kinematics import AnalyticalInverseKinematics
 
 if not compas.IPY:
     from typing import TYPE_CHECKING
 
     if TYPE_CHECKING:
-        from compas_fab.backends import AnalyticalKinematicsPlanner  # noqa: F401
+        from compas_fab.backends import AnalyticalPyBulletPlanner  # noqa: F401
         from compas_fab.robots import RobotCellState  # noqa: F401
         from compas_fab.robots import Robot  # noqa: F401
         from compas_fab.robots import Target  # noqa: F401
         from typing import Generator  # noqa: F401
         from typing import Dict, List, Optional, Tuple  # noqa: F401
         from compas_fab.backends import PyBulletClient  # noqa: F401
 
-
-from compas_fab.robots import FrameTarget
 from compas_robots import Configuration
 
+from compas_fab.backends import CollisionCheckError
+from compas_fab.robots import FrameTarget
 
 from ..utils import joint_angles_to_configurations
 from ..utils import try_to_fit_configurations_between_bounds
@@ -46,19 +42,25 @@ class AnalyticalPybulletInverseKinematics(AnalyticalInverseKinematics):
     that supports ``"check_collision"``, so for now only the `PyBulletClient`.
     """
 
-    def iter_inverse_kinematics_frame_target(self, target, start_state=None, group=None, options=None):
-        # type: (FrameTarget, Optional[RobotCellState], Optional[str], Optional[Dict]) -> Generator[Tuple[List[float], List[str]], None, None]
+    def iter_inverse_kinematics_frame_target(self, target, start_state, group=None, options=None):
+        # type: (FrameTarget, RobotCellState, Optional[str], Optional[Dict]) -> Generator[Tuple[List[float], List[str]], None, None]
         """This function overrides the iter_inverse_kinematics_frame_target function from AnalyticalInverseKinematics
         to include the PyBulletClient for collision checking.
 
         It depends on the PyBulletClient to be able to check for collisions.
 
         """
+        # TODO: This function is migrated but not finished with new CC functions.
+
         options = options or {}
-        planner = self  # type: AnalyticalKinematicsPlanner
+        planner = self  # type: AnalyticalPyBulletPlanner
         robot = planner.robot_cell.robot  # type: Robot
         client = self.client  # type: PyBulletClient
 
+        # Set robot cell state to start state if provided
+        if start_state:
+            planner.set_robot_cell_state(start_state)
+
         # Scale Target and get target frame
         target = self._scale_input_target(target)
         target_frame = target.target_frame
@@ -79,8 +81,11 @@ 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(Configuration.from_revolute_values(joint_values, joint_names))
-                except BackendError as e:
+                    # To save some time, we only set the robot configuration but avoid calling set_robot_cell_states
+                    client.set_robot_configuration(Configuration(joint_names=joint_names, joint_values=joint_values))
+                    # Passing the `_skip_set_robot_cell_state` option to the collision check function
+                    planner.check_collision(None, options={"_skip_set_robot_cell_state": True})
+                except CollisionCheckError as e:
                     # If keep order is true, yield (None, None) to keep the order of the solutions
                     if options.get("keep_order", False):
                         yield (None, None)

src/compas_fab/backends/kinematics/planner.py

@@ -47,13 +47,15 @@ def __init__(self, kinematics_solver):
 
 
 class AnalyticalPyBulletPlanner(
-    PyBulletForwardKinematics,
-    AnalyticalPybulletInverseKinematics,
     AnalyticalPlanCartesianMotion,
     PyBulletSetRobotCell,
     PyBulletSetRobotCellState,
+    PyBulletCheckCollision,
+    PyBulletForwardKinematics,
+    AnalyticalPybulletInverseKinematics,
     PlannerInterface,
 ):
+    # TODO: This class need rework
     """Combination of PyBullet as the client for Collision Detection and Analytical Inverse Kinematics.
 
     This planner is based on the the PyBullet client for collision detection and use

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

@@ -33,7 +33,7 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None):
         Collision checking can be enabled by setting ``"check_collision"`` to ``True`` in the options.
         This will cause the backend to perform collision checking on the robot's configuration and raise
         a :class:`CollisionCheckError` if the robot is in collision. This is equivalent to calling
-        :meth:`compas_fab.backends.PyBulletClient.check_collisions`.
+        :meth:`compas_fab.backends.PyBulletCheckCollision.check_collision`.
 
         Parameters
         ----------
@@ -52,7 +52,7 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None):
             - ``"link"``: (:obj:`str`, optional) The name of the link to
               calculate the forward kinematics for. Defaults to the end effector.
             - ``"check_collision"``: (:obj:`str`, optional) When ``True``,
-              :meth:`compas_fab.backends.PyBulletClient.check_collisions` will be called.
+              :meth:`compas_fab.backends.PyBulletCheckCollision.check_collision` will be called.
               Defaults to ``False``.
 
         Returns
@@ -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(configuration)
+            planner.check_collision(None, options={"_skip_set_robot_cell_state": True})
 
         # If a link name provided, return the frame of that link
         link_name = options.get("link")