Remove the high_accuracy mode from PyBullet IK

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

@@ -21,7 +21,7 @@
     start_configuration = robot.zero_configuration()
     start_state = RobotCellState.from_robot_configuration(robot, start_configuration)
 
-    options = dict(max_results=20, high_accuracy_threshold=1e-4, high_accuracy_max_iter=20)
+    options = {"max_results": 20}
     result_count = 0
     for config in planner.iter_inverse_kinematics(target, start_state, options=options):
         print("Found configuration", config)

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

@@ -54,6 +54,9 @@
 class PyBulletInverseKinematics(InverseKinematics):
     """Mix-in functions to calculate the robot's inverse kinematics for a given target."""
 
+    DEFAULT_TARGET_TOLERANCE_POSITION = 0.001
+    DEFAULT_TARGET_TOLERANCE_ORIENTATION = 0.001
+
     def inverse_kinematics(self, target, robot_cell_state, group=None, options=None):
         # type: (FrameTarget, Optional[RobotCellState], Optional[str], Optional[Dict]) -> Configuration
         """Calculate the robot's inverse kinematic for a given frame.
@@ -205,16 +208,12 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group,
             - ``"semi-constrained"``: (:obj:`bool`, optional) When ``True``, only the
               position of the target is considered. The orientation of frame will not be considered
               in the calculation.  Defaults to ``False``.
-            - ``"high_accuracy"``:  (:obj:`bool`, optional) When ``True``, the
-              solver will iteratively try to reach the ``high_accuracy_threshold``.
-              Failure to reach the threshold within ``high_accuracy_max_iter`` will raise an exception.
-              It is uncommon to use ``False``, because the solver will not
-              guarantee any accuracy.
-              Defaults to ``True``.
-            - ``"high_accuracy_threshold"``:  (:obj:`float`, optional) Defines the maximum
-              acceptable distance threshold for the high accuracy mode. Defaults to ``1e-4``.
-            - ``"high_accuracy_max_iter"``:  (:obj:`float`, optional) Defines the maximum
-              number of iterations to use for the high accuracy mode. Defaults to ``20``.
+            - ``"max_descend_iterations"``:  (:obj:`float`, optional) Defines the maximum
+              number of iterations to use during the gradient descend.
+              If this number of iterations are reached without convergence, the solver will consider
+              the initial guess as a bad starting point and will retry with a new random configuration.
+              Defaults to ``20``.
+              If the target tolerance is increased, this value should be increased as well.
             - ``"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.
@@ -263,9 +262,7 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group,
             raise ValueError("Planning group '{}' not found in the robot's semantics.".format(group))
 
         # Default options
-        options["high_accuracy"] = options.get("high_accuracy", True)
-        options["high_accuracy_threshold"] = options.get("high_accuracy_threshold", 1e-4)
-        options["high_accuracy_max_iter"] = options.get("high_accuracy_max_iter", 20)
+        options["max_descend_iterations"] = options.get("max_descend_iterations", 20)
         options["max_results"] = options.get("max_results", 100)
 
         options["check_collision"] = options.get("check_collision", True)
@@ -307,7 +304,6 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group,
         all_joint_names = robot.model.get_configurable_joint_names()
         assert set(all_joint_names) == set(start_configuration.keys()), "Robot configuration is missing some joints"
         rest_poses = client.build_pose_for_pybullet(start_configuration)
-        rest_poses_dict = dict(zip(joint_names_sorted, rest_poses))
 
         # Prepare `lower_limits`` and `upper_limits` input
         # Get joint limits in the same order as the joint_ids
@@ -318,42 +314,29 @@ def iter_inverse_kinematics_frame_target(self, target, robot_cell_state, group,
             lower_limits.append(joint.limit.lower if joint.type != Joint.CONTINUOUS else 0)
             upper_limits.append(joint.limit.upper if joint.type != Joint.CONTINUOUS else 2 * math.pi)
 
+        # Prepare `jointRanges` input
+        # I don't know what jointRanges needs to be.  Erwin Coumans knows, but he isn't telling.
+        # https://stackoverflow.com/questions/49674179/understanding-inverse-kinematics-pybullet
+        # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/preview?pru=AAABc7276PI*zazLer2rlZ8tAUI8lF98Kw#heading=h.9i02ojf4k3ve
+        joint_ranges = [u - l for u, l in zip(upper_limits, lower_limits)]
+
         # Prepare Parameters for calling pybullet.calculateInverseKinematics
         ik_options = dict(
             bodyUniqueId=body_id,
             endEffectorLinkIndex=link_id,
             targetPosition=point,
             targetOrientation=orientation,
             physicsClientId=client.client_id,
-        )
-
-        # I don't know what jointRanges needs to be.  Erwin Coumans knows, but he isn't telling.
-        # https://stackoverflow.com/questions/49674179/understanding-inverse-kinematics-pybullet
-        # https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/preview?pru=AAABc7276PI*zazLer2rlZ8tAUI8lF98Kw#heading=h.9i02ojf4k3ve
-        joint_ranges = [u - l for u, l in zip(upper_limits, lower_limits)]
-        ik_options.update(
-            dict(
-                lowerLimits=lower_limits,
-                upperLimits=upper_limits,
-                jointRanges=joint_ranges,
-                restPoses=rest_poses,
-            )
+            lowerLimits=lower_limits,
+            upperLimits=upper_limits,
+            jointRanges=joint_ranges,
+            restPoses=rest_poses,
         )
 
         # Options for semi-constrained mode, skipping the targetOrientation
         if options.get("semi-constrained"):
             ik_options.pop("targetOrientation")
 
-        # Options for high accuracy mode
-        if options.get("high_accuracy"):
-            ik_options.update(
-                dict(
-                    joint_ids_sorted=joint_ids_sorted,
-                    threshold=options.get("high_accuracy_threshold"),
-                    max_iter=options.get("high_accuracy_max_iter"),
-                )
-            )
-
         def set_random_config():
             # Function for setting random joint values for randomized search
             config = robot.random_configuration(group)
@@ -367,24 +350,27 @@ def set_random_config():
         # Loop to get multiple results
         for _ in range(options.get("max_results")):
 
-            # Calling the IK function (High accuracy or not)
-            if options.get("high_accuracy"):
-                joint_positions, close_enough = self._accurate_inverse_kinematics(
-                    verbose=options["verbose"], **ik_options
-                )
+            # Calling the IK function using the helper function that repeatedly
+            # calls the pybullet IK solver until convergence.
 
-                # NOTE: In principle, this accurate iter IK should work out of the
-                # pybullet box, but the results seem to be way off target. For now,
-                # I'm leaving the legacy iterative accurate ik in python as per
-                # older examples of pybullet, until we figure out why the builtin
-                # one is not cooperating.
-                if not close_enough:
-                    # If the solution is not close enough, we retry with a new randomized joint values
-                    set_random_config()
-                    continue
-            else:
-                joint_positions = pybullet.calculateInverseKinematics(**ik_options)
-                # Without the high accuracy mode, there is no guarantee of accuracy
+            joint_positions, close_enough = self._accurate_inverse_kinematics(
+                joint_ids_sorted=joint_ids_sorted,
+                tolerance_position=target.tolerance_position,
+                tolerance_orientation=target.tolerance_orientation,
+                max_iter=options.get("max_descend_iterations"),
+                verbose=options["verbose"],
+                **ik_options
+            )
+
+            # NOTE: In principle, this accurate iter IK should work out of the
+            # pybullet box, but the results seem to be way off target. For now,
+            # I'm leaving the legacy iterative accurate ik in python as per
+            # older examples of pybullet, until we figure out why the builtin
+            # one is not cooperating.
+            if not close_enough:
+                # If the solution is not close enough, we retry with a new randomized joint values
+                set_random_config()
+                continue
 
             assert len(joint_positions) == len(
                 joint_names_and_puids
@@ -503,9 +489,7 @@ def iter_inverse_kinematics_point_axis_target(self, target, robot_cell_state, gr
 
             The following key-value pairs have the same meaning as in the :meth:`iter_inverse_kinematics_frame_target` method:
 
-            - ``"high_accuracy"``:  (:obj:`bool`, optional)
-            - ``"high_accuracy_threshold"``:  (:obj:`float`, optional)
-            - ``"high_accuracy_max_iter"``:  (:obj:`float`, optional)
+            - ``"max_descend_iterations"``:  (:obj:`float`, optional)
             - ``"solution_uniqueness_threshold_prismatic"``: (:obj:`float`, optional)
             - ``"solution_uniqueness_threshold_revolute"``: (:obj:`float`, optional)
             - ``"check_collision"``: (:obj:`bool`, optional)
@@ -561,9 +545,7 @@ def iter_inverse_kinematics_point_axis_target(self, target, robot_cell_state, gr
         # Default options (for the FrameTarget function)
         # Later there is a frame_ik_options dict that will be passed to the pybullet IK solver
         # That one will set the max_results to 1 for the FrameTarget function
-        options["high_accuracy"] = options.get("high_accuracy", True)
-        options["high_accuracy_threshold"] = options.get("high_accuracy_threshold", 1e-4)
-        options["high_accuracy_max_iter"] = options.get("high_accuracy_max_iter", 20)
+        options["max_descend_iterations"] = options.get("max_descend_iterations", 20)
 
         options["solution_uniqueness_threshold_prismatic"] = options.get(
             "solution_uniqueness_threshold_prismatic", 3e-4
@@ -698,7 +680,9 @@ def set_random_config():
                 "No solution found after {} attempts (max_random_restart).".format(options.get("max_random_restart"))
             )
 
-    def _accurate_inverse_kinematics(self, joint_ids_sorted, threshold, max_iter, verbose=False, **kwargs):
+    def _accurate_inverse_kinematics(
+        self, joint_ids_sorted, tolerance_position, tolerance_orientation, max_iter, verbose=False, **kwargs
+    ):
         """Iterative inverse kinematics solver with a threshold for the distance to the target.
 
         This functions helps to get a more accurate solution by iterating over the IK solver
@@ -717,6 +701,8 @@ def _accurate_inverse_kinematics(self, joint_ids_sorted, threshold, max_iter, ve
         body_id = kwargs["bodyUniqueId"]
         link_id = kwargs["endEffectorLinkIndex"]
         target_position = kwargs["targetPosition"]
+        tolerance_position = tolerance_position or self.DEFAULT_TARGET_TOLERANCE_POSITION
+        tolerance_orientation = tolerance_orientation or self.DEFAULT_TARGET_TOLERANCE_ORIENTATION
         # Note: the following two options that was present in the PyBullet IK Example does not seem to do anything
         # https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_kinematics.py
         # kwargs["maxNumIterations"] = max_iter
@@ -735,11 +721,15 @@ def _accurate_inverse_kinematics(self, joint_ids_sorted, threshold, max_iter, ve
                 target_position[1] - new_pose[1],
                 target_position[2] - new_pose[2],
             ]
+
             # The distance is squared to avoid a sqrt operation
             distance_squared = diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2]
             # Therefor, the threshold is squared as well
             # print("Iter: %d, Distance: %s" % (iter, distance_squared))
-            close_enough = distance_squared < threshold * threshold
+            close_enough = distance_squared < tolerance_position * tolerance_position
+
+            # TODO: Check orientation as well
+
             kwargs["restPoses"] = joint_poses
             iter += 1
         if verbose:

tests/backends/pybullet/test_pybullet_planner.py

@@ -1,13 +1,14 @@
 import compas
 
+from copy import deepcopy
+
 if not compas.IPY:
     from compas_fab.backends import PyBulletClient
     from compas_fab.backends import PyBulletPlanner
 
 from compas.geometry import Point
 from compas.geometry import Vector
 from compas.geometry import Frame
-
 from compas.tolerance import Tolerance
 from compas_fab.robots import RobotLibrary
 from compas_fab.robots import FrameTarget
@@ -138,11 +139,11 @@ def _test_pybullet_ik_fk_agreement(robot, ik_target_frames):
 
     """
     # These options are set to ensure that the IK solver converges to a high accuracy
-    # Threshold is set to 1e-5 meters to be larger than the tolerance used for comparison (1e-4 meters)
+    # Target.tolerance_orientation is set to 1e-5 meters to be larger than the tolerance used for comparison (1e-4 meters)
+    tolerance_position = 1e-5
+    tolerance_orientation = 1e-2
     ik_options = {
-        "high_accuracy_max_iter": 50,
-        "high_accuracy": True,
-        "high_accuracy_threshold": 1e-5,
+        "max_descend_iterations": 50,
         "return_full_configuration": True,
     }
 
@@ -153,11 +154,17 @@ def _test_pybullet_ik_fk_agreement(robot, ik_target_frames):
 
         for ik_target_frame in ik_target_frames:
             # IK Query to the planner (Frame to Configuration)
+            ik_target_frame = deepcopy(ik_target_frame)  # type: FrameTarget
             try:
 
                 ik_result = next(
                     planner.iter_inverse_kinematics(
-                        FrameTarget(ik_target_frame, target_mode=TargetMode.ROBOT),
+                        FrameTarget(
+                            ik_target_frame,
+                            target_mode=TargetMode.ROBOT,
+                            tolerance_position=tolerance_position,
+                            tolerance_orientation=tolerance_orientation,
+                        ),
                         RobotCellState.from_robot_configuration(robot),
                         group=planning_group,
                         options=ik_options,
@@ -280,8 +287,10 @@ def test_pybullet_ik_out_of_reach_ur5():
         )
     )
 
-    # high_accuracy_max_iter is set to 20 to reduce the number of iterations, faster testing time.
-    ik_options = {"high_accuracy_max_iter": 20, "high_accuracy": True, "high_accuracy_threshold": 1e-5}
+    # max_descend_iterations is set to 20 to reduce the number of iterations, faster testing time.
+    ik_options = {
+        "max_descend_iterations": 20,
+    }
 
     with PyBulletClient(connection_type="direct") as client:
         planner = PyBulletPlanner(client)

tests/backends/pybullet/test_pybullet_planner_inverse_kinematics.py

@@ -147,7 +147,6 @@ def test_iter_inverse_kinematics_frame_target(planner_with_test_cell):
 
     options = {
         "max_results": 1,
-        "high_accuracy": True,
         "check_collision": True,
         "verbose": False,
     }