Support Mink 1.4

.github/workflows/test.yaml

@@ -29,7 +29,10 @@ jobs:
     name: pytest (${{ matrix.mink }})
     strategy:
       matrix:
-        mink: ["mink==1.2.0", "mink"]
+        mink:
+          - "mink==1.2.0"
+          - "mink==1.3.0"
+          - "mink"
     runs-on: ubuntu-latest
     timeout-minutes: 10
     permissions:

src/openarm_control/qp/bounded_frame_task.py

@@ -86,7 +86,6 @@ def __init__(
         self._target_linear_speed_fast = float(target_linear_speed_fast)
         self._position_error_latch_threshold = float(position_error_latch_threshold)
         self._previous_target_position: np.ndarray | None = None
-        self._identity: np.ndarray | None = None
         self.limit_activation = 1.0
 
     def set_target(self, transform: mink.SE3) -> None:
@@ -199,21 +198,3 @@ def _bounded_terms(
                 error[:3] - full_error[:3]
             )
         return error, jacobian
-
-    def compute_qp_objective(
-        self,
-        configuration: mink.Configuration,
-    ) -> mink.Objective:
-        """Modulate position by its schedule and always bound orientation."""
-        terms = self._bounded_terms(configuration)
-        if terms is None:
-            return self.frame_task.compute_qp_objective(configuration)
-        error, jacobian = terms
-        nv = configuration.model.nv
-        if self._identity is None or self._identity.shape != (nv, nv):
-            self._identity = np.eye(nv, dtype=np.float64)
-        return self._assemble_qp(
-            error,
-            jacobian,
-            self._identity,
-        )

src/openarm_control/qp/nullspace_posture_task.py

@@ -128,7 +128,6 @@ def __init__(
             model,
             indices,
         )
-        self._identity = np.eye(model.nv, dtype=np.float64)
         self._configuration_error = np.empty(model.nv, dtype=np.float64)
         self._previous_direction: np.ndarray | None = None
         self._svd_partner: NullspacePostureTask | None = None
@@ -235,11 +234,6 @@ def compute_jacobian(self, configuration: mink.Configuration) -> np.ndarray:
         _, jacobian = self._compute_terms(configuration)
         return jacobian
 
-    def compute_qp_objective(self, configuration: mink.Configuration) -> mink.Objective:
-        """Assemble both terms from one SVD so they use the same direction."""
-        error, jacobian = self._compute_terms(configuration)
-        return self._assemble_qp(error, jacobian, self._identity)
-
     def compute_qp_residual(
         self,
         configuration: mink.Configuration,

tests/test_qp_tasks.py

@@ -34,6 +34,23 @@
 from _support import make_setup
 
 
+def _assert_residual_equal(
+    actual: tuple[np.ndarray, np.ndarray, float],
+    expected: tuple[np.ndarray, np.ndarray, float],
+) -> None:
+    for actual_part, expected_part in zip(actual, expected, strict=True):
+        np.testing.assert_array_equal(actual_part, expected_part)
+
+
+def _objective_from_residual(
+    residual: tuple[np.ndarray, np.ndarray, float], nv: int
+) -> mink.Objective:
+    weighted_jacobian, weighted_error, mu = residual
+    H = weighted_jacobian.T @ weighted_jacobian + mu * np.eye(nv)
+    c = -weighted_error @ weighted_jacobian
+    return mink.Objective(H, c)
+
+
 def _make_task(
     *,
     orientation_error_limit: float = 0.0,
@@ -80,17 +97,16 @@ def test_limits_request_but_preserves_full_error_and_target() -> None:
     assert native.transform_target_to_world is not None
 
 
-def test_zero_activation_is_exactly_the_native_objective() -> None:
+def test_zero_activation_is_exactly_the_native_residual() -> None:
     configuration, native, task, target = _make_task()
     target[:3] += [0.03, -0.02, 0.01]
     task.set_target(pose_to_se3(target))
     task.set_limit_activation(0.0)
 
-    native_objective = native.compute_qp_objective(configuration)
-    wrapped_objective = task.compute_qp_objective(configuration)
-
-    np.testing.assert_array_equal(wrapped_objective.H, native_objective.H)
-    np.testing.assert_array_equal(wrapped_objective.c, native_objective.c)
+    _assert_residual_equal(
+        task.compute_qp_residual(configuration),
+        native.compute_qp_residual(configuration),
+    )
 
 
 def test_orientation_limit_is_independent_of_position_activation() -> None:
@@ -109,16 +125,14 @@ def test_orientation_limit_is_independent_of_position_activation() -> None:
     assert float(np.linalg.norm(full_error[3:])) > 0.9
     assert float(np.linalg.norm(limited_error[3:])) == pytest.approx(0.20 / 5)
 
-    expected_error = full_error.copy()
-    expected_error[3:] = limited_error[3:]
-    expected = task._assemble_qp(
-        expected_error,
-        task.compute_jacobian(configuration),
-        np.eye(configuration.model.nv),
+    # Use the same native error path as the task: Mink 1.4.0's compute_error()
+    # and _error_and_jacobian() can differ in the last few bits.
+    expected_error, jacobian = task.frame_task._error_and_jacobian(configuration)
+    expected_error[3:] = task._clip_error(expected_error)[3:]
+    _assert_residual_equal(
+        task.compute_qp_residual(configuration),
+        task._weighted_residual(expected_error, jacobian),
     )
-    actual = task.compute_qp_objective(configuration)
-    np.testing.assert_allclose(actual.H, expected.H)
-    np.testing.assert_allclose(actual.c, expected.c)
 
 
 def test_clipping_preserves_input_and_disabled_orientation() -> None:
@@ -156,16 +170,21 @@ def test_fused_residuals_preserve_objective_without_repeated_terms(
     frame.set_target(pose_to_se3(target))
     nullspace = solver._nullspace_tasks["right"]
     tasks = [frame, nullspace, solver._kinetic_energy_task]
-    error = frame.compute_full_error(configuration)
+    # Use the same native error path as the task: Mink 1.4.0's compute_error()
+    # and _error_and_jacobian() can differ in the last few bits.
+    error, jacobian = frame.frame_task._error_and_jacobian(configuration)
     limited = frame._clip_error(error)
     error[:3] += activation * (limited[:3] - error[:3])
     if orientation_limit > 0.0:
         error[3:] = limited[3:]
     expected = [
-        frame._assemble_qp(
-            error, frame.compute_jacobian(configuration), np.eye(configuration.nv)
+        _objective_from_residual(
+            frame._weighted_residual(error, jacobian), configuration.nv
+        ),
+        _objective_from_residual(
+            nullspace.compute_qp_residual(configuration), configuration.nv
         ),
-        *[task.compute_qp_objective(configuration) for task in tasks[1:]],
+        solver._kinetic_energy_task.compute_qp_objective(configuration),
     ]
 
     with (