Re-add optimizer based on scipy.optimize.minimize (#135)

* Re-add scalar optimizer that can be selected via a configurable parameter

* Fix tests

Author: Phylliade

.github/workflows/ci.yml

@@ -26,8 +26,8 @@ jobs:
     strategy:
       max-parallel: 4
       matrix:
-        # Note: We need to quote '3.10' otherwise it's interpreted as as number, e.g. 3.1
-        python-version: [3.6, 3.7, 3.8, 3.9, '3.10']
+        # Note: We need to quote '3.10' and '3.11' otherwise it's interpreted as as number, e.g. 3.1
+        python-version: [3.7, 3.8, 3.9, '3.10', "3.11"]
     steps:
       - uses: actions/checkout@v1
         name: "Checkout branch"

src/ikpy/inverse_kinematics.py

@@ -7,7 +7,7 @@
 ORIENTATION_COEFF = 1.
 
 
-def inverse_kinematic_optimization(chain, target_frame, starting_nodes_angles, regularization_parameter=None, max_iter=None, orientation_mode=None, no_position=False):
+def inverse_kinematic_optimization(chain, target_frame, starting_nodes_angles, regularization_parameter=None, max_iter=None, orientation_mode=None, no_position=False, optimizer="least_squares"):
     """
     Computes the inverse kinematic on the specified target
 
@@ -32,7 +32,14 @@ def inverse_kinematic_optimization(chain, target_frame, starting_nodes_angles, r
         * "all": Target the three axes
     no_position: bool
         Do not optimize against position
+    optimizer: str
+        The solver to use. Choices:
+        * "least_squares": Use scipy.optimize.least_squares
+        * "scalar": Use scipy.optimize.minimize
     """
+    if optimizer not in ["least_squares", "scalar"]:
+        raise ValueError("Unknown solver: {}".format(optimizer))
+
     # Begin with the position
     target = target_frame[:3, -1]
 
@@ -124,15 +131,22 @@ def optimize_total(x):
     # Compute bounds
     real_bounds = [link.bounds for link in chain.links]
     # real_bounds = real_bounds[chain.first_active_joint:]
-    real_bounds = np.moveaxis(chain.active_from_full(real_bounds), -1, 0)
+    real_bounds = chain.active_from_full(real_bounds)
 
     logs.logger.info("Bounds: {}".format(real_bounds))
 
     if max_iter is not None:
         logs.logger.info("max_iter is not used anymore in the IK, using it as a parameter will raise an exception in the future")
 
     # least squares optimization
-    res = scipy.optimize.least_squares(optimize_total, chain.active_from_full(starting_nodes_angles), bounds=real_bounds)
+    if optimizer == "scalar":
+        def optimize_scalar(x):
+            return np.linalg.norm(optimize_total(x))
+        res = scipy.optimize.minimize(optimize_scalar, chain.active_from_full(starting_nodes_angles), bounds=real_bounds)
+    elif optimizer == "least_squares":
+        # We need to unzip the bounds
+        real_bounds = np.moveaxis(real_bounds, -1, 0)
+        res = scipy.optimize.least_squares(optimize_total, chain.active_from_full(starting_nodes_angles), bounds=real_bounds)
 
     if res.status != -1:
         logs.logger.info("Inverse kinematic optimisation OK, termination status: {}".format(res.status))

tests/test_chain.py

@@ -76,6 +76,13 @@ def test_ik_optimization(torso_right_arm):
     np.testing.assert_almost_equal(
         torso_right_arm.forward_kinematics(ik)[:3, 3], target, decimal=3)
 
+    # Check using the scalar optimizer
+    ik = torso_right_arm.inverse_kinematics_frame(
+    frame_target, initial_position=joints, optimizer="scalar")
+    # Check whether the results are almost equal
+    np.testing.assert_almost_equal(
+        torso_right_arm.forward_kinematics(ik)[:3, 3], target, decimal=3)
+
 
 def test_chain_serialization(torso_right_arm):