From 0ec19aa97206946d87f66158ae5bf56b5329cf8b Mon Sep 17 00:00:00 2001 From: Sutou Kouhei Date: Wed, 4 Nov 2026 14:05:53 +0900 Subject: [PATCH] Support Mink 1.4 Mink 1.4.0 removed the private Task._assemble_qp (kevinzakka/mink#189), which BoundedFrameTask and NullspacePostureTask used in their compute_qp_objective() overrides. Since Mink 1.2.0, solve_ik() builds the objective from compute_qp_residual() and doesn't call compute_qp_objective() for tasks that provide a residual. Both tasks already implement compute_qp_residual(), so drop the compute_qp_objective() overrides. Since Mink 1.4.0, Task.compute_qp_objective() is derived from compute_qp_residual(). Tests now compare residuals instead of objectives. They build the expected error with the same _error_and_jacobian() path as the implementation, because Mink 1.4.0's native compute_error() and _error_and_jacobian() can differ in the last few bits. Also add Mink 1.3.0 to the CI matrix. Co-Authored-By: Claude Opus 5.5 --- .github/workflows/test.yaml | 5 +- src/openarm_control/qp/bounded_frame_task.py | 19 ------- .../qp/nullspace_posture_task.py | 6 -- tests/test_qp_tasks.py | 57 ++++++++++++------- 4 files changed, 42 insertions(+), 45 deletions(-) diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index d4a000f3..817d08b3 100644 --- a/.github/workflows/test.yaml +++ b/.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: diff --git a/src/openarm_control/qp/bounded_frame_task.py b/src/openarm_control/qp/bounded_frame_task.py index 8a6cf1f9..c0eb0f1f 100644 --- a/src/openarm_control/qp/bounded_frame_task.py +++ b/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, - ) diff --git a/src/openarm_control/qp/nullspace_posture_task.py b/src/openarm_control/qp/nullspace_posture_task.py index 50ac1086..864091b0 100644 --- a/src/openarm_control/qp/nullspace_posture_task.py +++ b/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, diff --git a/tests/test_qp_tasks.py b/tests/test_qp_tasks.py index 425e8fe2..6f5ef96b 100644 --- a/tests/test_qp_tasks.py +++ b/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 (