clean up examples

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

@@ -13,7 +13,8 @@
 from compas_robots import Configuration
 from compas_robots import ToolModel
 
-with PyBulletClient() as client:
+# Starting the PyBulletClient with the "direct" mode means that the GUI is not shown
+with PyBulletClient("direct") as client:
 
     # Load a pre-made robot cell with one tool from the RobotCellLibrary
     robot_cell, robot_cell_state = RobotCellLibrary.ur5_cone_tool()
@@ -25,7 +26,6 @@
     # ---------------------
     # Compute FK with tools
     # ---------------------
-
     # The input configuration used for the forward kinematics is provided through the RobotCellState
     robot_cell_state.robot_configuration.joint_values = [-2.238, -1.153, -2.174, 0.185, 0.667, 0.0]
     # By default, if a tool is attached, the TCF is returned

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

@@ -6,13 +6,9 @@
 from compas_fab.robots import RobotCellState
 from compas_fab.robots import RobotLibrary
 
-
-# #############################################
-# Headless (no-GUI) forwards kinematics example
-# #############################################
-
-# 'direct' mode
+# Starting the PyBulletClient with the "direct" mode means that the GUI is not shown
 with PyBulletClient("direct") as client:
+
     # The robot in this example is loaded from a URDF file
     robot = RobotLibrary.ur5()
 
@@ -22,7 +18,6 @@
     # ----------------
     # FK without tools
     # ----------------
-
     # This is a simple robot cell with only the robot
     robot_cell = RobotCell(robot)
     planner.set_robot_cell(robot_cell)
@@ -40,7 +35,6 @@
     # ---------------------------------
     # FK for all the links in the robot
     # ---------------------------------
-
     for link_name in robot.get_link_names():
         frame_WCF = planner.forward_kinematics(robot_cell_state, options={"link": link_name})
         print(f"Frame of link '{link_name}' : {frame_WCF}")

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

@@ -8,18 +8,36 @@
 from compas_fab.robots import RobotLibrary
 
 with PyBulletClient() as client:
-    planner = PyBulletPlanner(client)
+    # Create a robot cell with a UR5 robot
     robot = RobotLibrary.ur5()
-    planner.set_robot_cell(RobotCell(robot))
+    robot_cell = RobotCell(robot)
 
-    frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
-    target = FrameTarget(frame_WCF)
+    planner = PyBulletPlanner(client)
+    planner.set_robot_cell(robot_cell)
 
+    # Create a RobotCellState from the robot's zero configuration
     start_configuration = robot.zero_configuration()
     robot_cell_state = RobotCellState.from_robot_configuration(robot, start_configuration)
 
+    # The FrameTarget represents the robot's planner coordinate frame (PCF)
+    # For the UR5 robot, the PCF is equal to the frame of the 'tool0' link
+    frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
+    target = FrameTarget(frame_WCF)
     joint_positions, joint_names = planner.inverse_kinematics(target, robot_cell_state)
 
     print("Inverse kinematics result: ", joint_positions, joint_names)
 
     input("Observe the IK result in PyBullet's GUI, Press Enter to continue...")
+
+    # To verify the IK result, we can compute the FK with the obtained joint positions
+    robot_cell_state.robot_configuration.joint_values = joint_positions
+    frame_WCF = planner.forward_kinematics(robot_cell_state)
+    print("Forward kinematics result (main group): \n ", frame_WCF)
+
+    # The result is the same as the 'tool0' link's frame
+    frame_WCF = planner.forward_kinematics(robot_cell_state, options={"link": "tool0"})
+    print("Forward kinematics result: (tool0 link): \n ", frame_WCF)
+
+    # However, note that the 'flange' link's frame has a different orientation
+    frame_WCF = planner.forward_kinematics(robot_cell_state, options={"link": "flange"})
+    print("Forward kinematics result: (flange link): \n ", frame_WCF)

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

@@ -0,0 +1,39 @@
+from compas.geometry import Frame
+from compas_fab.backends import PyBulletClient
+from compas_fab.backends import PyBulletPlanner
+
+from compas_fab.robots import FrameTarget
+from compas_fab.robots import RobotCellLibrary
+
+from compas_fab.backends.exceptions import InverseKinematicsError
+
+with PyBulletClient() as client:
+
+    # Load a pre-made robot cell with one tool from the RobotCellLibrary
+    robot_cell, robot_cell_state = RobotCellLibrary.ur10e_gripper_one_beam()
+    planner = PyBulletPlanner(client)
+    planner.set_robot_cell(robot_cell)
+
+    # The FrameTarget represents the tool's coordinate frame (TCF) when a tool is attached
+    target_center_point = [0.0, 0.5, 0.15]
+    frame_WCF = Frame(target_center_point, [1, 0, 0], [0, 1, 0])
+    target = FrameTarget(frame_WCF)
+
+    # Example 1: Default IK without collision checking
+    # The target is causing the attached beam to collide with the floor.
+    # However, the planner does not check for collisions and returns a solution.
+    joint_positions, joint_names = planner.inverse_kinematics(target, robot_cell_state)
+    input("Observe the IK result in PyBullet's GUI, Press Enter to continue...")
+
+    # Example 2: Enable collision checking in the IK
+    try:
+        # Enable the check_collision mode via options
+        options = {"check_collision": True}
+        joint_positions, joint_names = planner.inverse_kinematics(target, robot_cell_state, options=options)
+    except InverseKinematicsError as e:
+        # The planner will try many times but still unable to find a solution
+        # after "max_results", it will return InverseKinematicsError.
+        print(e)
+        print(e.message)
+        print(e.target_pcf)
+    input("Observe the IK result in PyBullet's GUI, Press Enter to continue...")

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

@@ -6,16 +6,20 @@
 from compas_fab.robots import RobotCellLibrary
 
 with PyBulletClient() as client:
-    planner = PyBulletPlanner(client)
+
+    # Load a pre-made robot cell with one tool from the RobotCellLibrary
     robot_cell, robot_cell_state = RobotCellLibrary.ur5_cone_tool()
+    planner = PyBulletPlanner(client)
     planner.set_robot_cell(robot_cell)
 
+    # The FrameTarget represents the tool's coordinate frame (TCF) when a tool is attached
     target_center_point = [0.0, 0.5, 0.5]
     frame_WCF = Frame(target_center_point, [1, 0, 0], [0, 0, -1])
     target = FrameTarget(frame_WCF)
 
-    # The robot_cell_state from the RobotCellLibrary contains a zero configuration for the robot
-    options = {"check_collision": True}  # check_collision is turned off by default
+    # Enable the check_collision mode in the inverse_kinematics function.
+    # This will force the PyBullet planner to check for collisions during the IK computation
+    options = {"check_collision": True}  #
     joint_positions, joint_names = planner.inverse_kinematics(target, robot_cell_state, options=options)
 
     print("Inverse kinematics result: ", joint_positions, joint_names)

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

@@ -9,12 +9,6 @@
 from compas_fab.robots import RobotCellLibrary
 from compas_fab.backends import CollisionCheckError
 
-
-# #############################################
-# Pybullet Check Collision Example
-# #############################################
-
-
 with PyBulletClient("gui") as client:
     planner = PyBulletPlanner(client)
     print(f"Observe the Pybullet GUI window to see the robot cell state being checked")
@@ -24,7 +18,7 @@
     planner.set_robot_cell(robot_cell)
 
     # ---------------------------------------------
-    # Trial 1 - No collision
+    # Example 1 - No collision
     # ---------------------------------------------
 
     # This configuration is not in collision
@@ -37,7 +31,7 @@
     input("Press Enter to continue...\n")
 
     # ---------------------------------------------
-    # Trial 2 - Collision between beam and floor
+    # Example 2 - Collision between beam and floor
     # ---------------------------------------------
 
     robot_cell_state.robot_configuration.joint_values = [0, -1.0, 2.0, 0, 0, 0]
@@ -50,7 +44,7 @@
     input("Press Enter to continue...\n")
 
     # ---------------------------------------------
-    # Trial 3 - Multiple Collisions
+    # Example 3 - Multiple Collisions
     # ---------------------------------------------
 
     robot_cell_state.robot_configuration.joint_values = [0, -0.8, 2.0, 0, 0, 0]
@@ -64,7 +58,7 @@
     input("Press Enter to continue...\n")
 
     # ---------------------------------------------
-    # Trial 4 - Verbose Mode
+    # Example 4 - Verbose Mode
     # ---------------------------------------------
 
     robot_cell_state.robot_configuration.joint_values = [0, -0.7, 2.0, 0, 0, 0]

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

@@ -8,22 +8,16 @@
 from compas_fab.robots import RobotCellLibrary
 from compas_fab.robots import RobotCellState
 
-
-# #############################################
-# Headless (no-GUI) forwards kinematics example
-# #############################################
-
-# 'direct' mode
 with PyBulletClient("gui") as client:
     planner = PyBulletPlanner(client)
 
     # The robot cell in this example is loaded from RobotCellLibrary
     robot_cell, robot_cell_state = RobotCellLibrary.ur5_cone_tool()
     planner.set_robot_cell(robot_cell)
 
-    # ==========================================
+    # ---------------------------------------------
     # Plan Cartesian Motion with FrameWaypoints
-    # ==========================================
+    # ---------------------------------------------
 
     # The starting robot configuration is set in the robot cell state
     robot_cell_state.robot_configuration.joint_values = [-2.238, -1.153, -2.174, 0.185, 0.667, 0.0]
@@ -47,9 +41,10 @@
     for i, point in enumerate(trajectory.points):
         print("- JointTrajectoryPoint {}, joint_values: {}".format(i, point.joint_values))
 
-    # #############################################
+    # ------------------------------------------------
     # Replay the trajectory in the PyBullet simulation
-    # #############################################
+    # ------------------------------------------------
+    # The following code only serves demonstration purposes
     # User can step through the trajectory points by pressing 'Enter' key
     step = 0
     intermediate_robot_cell_state = robot_cell_state.copy()  # type: RobotCellState

src/compas_fab/backends/exceptions.py

@@ -52,10 +52,10 @@ def __init__(self, message):
 class InverseKinematicsError(KinematicsError):
     """Indicates that no IK solution could be found by the kinematic solver."""
 
-    def __init__(self, message, target_t0cf=None):
+    def __init__(self, message, target_pcf=None):
         super(InverseKinematicsError, self).__init__("No inverse kinematics solution found.")
         self.message = message
-        self.target_t0cf = target_t0cf
+        self.target_pcf = target_pcf
 
 
 # -------------------------

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

@@ -101,13 +101,15 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state=None, gr
             - ``"high_accuracy_max_iter"``:  (:obj:`float`, optional) Defines the maximum
               number of iterations to use for the high accuracy mode. Defaults to ``20``.
             - ``"max_results"``: (:obj:`int`) Maximum number of results to return.
+              If set to 1, the solver will be deterministic, descending from the initial
+              robot configuration.
               Defaults to ``100``.
-            - ``solution_uniqueness_threshold_prismatic``: (:obj:`float`, optional) The minimum
-                distance between two solutions in the prismatic joint space to consider them unique.
-                Units are in meters. Defaults to ``3e-4``.
-            - ``solution_uniqueness_threshold_revolute``: (:obj:`float`, optional) The minimum
-                distance between two solutions in the revolute joint space to consider them unique.
-                Units are in radians. Defaults to ``1e-3``.
+            - ``"solution_uniqueness_threshold_prismatic"``: (:obj:`float`, optional) The minimum
+              distance between two solutions in the prismatic joint space to consider them unique.
+              Units are in meters. Defaults to ``3e-4``.
+            - ``"solution_uniqueness_threshold_revolute"``: (:obj:`float`, optional) The minimum
+              distance between two solutions in the revolute joint space to consider them unique.
+              Units are in radians. Defaults to ``1e-3``.
             - ``"check_collision"``: (:obj:`bool`, optional)
               Whether or not to check for collision. Defaults to ``False``.
 
@@ -149,18 +151,20 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state=None, gr
         robot_cell_state = robot_cell_state.copy()  # Make a copy to avoid modifying the original
         planner.set_robot_cell_state(robot_cell_state)
 
-        body_id = client.robot_puid
-        link_id = client.robot_link_puids[link_name]
-
         # Target frame
         target = self._scale_input_target(target)
         target_frame = target.target_frame
 
+        # TODO: Implement a fail fast mechanism to check if the attached tool and objects are in collision
+
         # Tool Coordinate Frame if there are tools attached
         attached_tool_id = robot_cell_state.get_attached_tool_id(group)
         if attached_tool_id:
             target_frame = planner.from_tcf_to_pcf([target_frame], attached_tool_id)[0]
 
+        # Formatting input for PyBullet
+        body_id = client.robot_puid
+        link_id = client.robot_link_puids[link_name]
         point, orientation = pose_from_frame(target_frame)
 
         # Get list of keys (joint_name) from the joint_ids dict in the order of its values (puid)
@@ -275,7 +279,9 @@ def set_random_config():
 
         # If no solution was found after max_results, raise an error
         if len(solutions) == 0:
-            raise InverseKinematicsError("No solution found after {} attempts (max_results).".format(max_results))
+            raise InverseKinematicsError(
+                "No solution found after {} attempts (max_results).".format(max_results), target_pcf=target_frame
+            )
 
     def _accurate_inverse_kinematics(self, joint_ids_sorted, threshold, max_iter, **kwargs):
         # Based on these examples