feat: 优化双臂采摘QP稳健性

xr_rm_teleop/test/test_placo_transforms.py

@@ -6,6 +6,7 @@ from xml.etree import ElementTree
 import numpy as np
 import pytest
 
+from xr_rm_teleop import placo_ik_solver
 from xr_rm_teleop.placo_ik_solver import (
     QP_ORIENTATION_TOLERANCE_RAD,
     QP_POSITION_TOLERANCE_M,
@@ -242,6 +243,7 @@ def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
     solver._frame_task = SimpleNamespace(T_a_b=None)
     solver._solver = SimpleNamespace(solve=lambda update: None)
     solver._validate_result = lambda result, previous: None
+    solver._update_auxiliary_task_weights = lambda: None
     solver._target_errors = lambda: (2.1e-3, 0.0)
 
     with pytest.raises(RuntimeError, match="QP did not converge after 30"):
@@ -285,3 +287,130 @@ def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
         solver._validate_result(np.full(7, 2.0))
     with pytest.raises(ValueError, match="velocity"):
         solver._validate_result(np.full(7, 0.2))
+
+
+def test_lower_margin_activation_is_clamped_and_linear() -> None:
+    activation = placo_ik_solver._lower_margin_activation
+
+    assert activation(0.05, 0.01, 0.04) == 0.0
+    assert activation(0.025, 0.01, 0.04) == pytest.approx(0.5)
+    assert activation(0.005, 0.01, 0.04) == 1.0
+
+
+@pytest.mark.parametrize(
+    "arm,joint_degrees,j3_reference_deg",
+    [
+        ("left", ARM_CASES[0][1], 67.96),
+        ("right", ARM_CASES[1][1], -89.57),
+    ],
+)
+def test_solver_configures_auxiliary_qp_tasks(
+    arm: str,
+    joint_degrees: list[float],
+    j3_reference_deg: float,
+) -> None:
+    pytest.importorskip("placo")
+    solver = PlacoIkSolver(
+        str(DUAL_URDF_PATH),
+        1.0 / 90.0,
+        arm,
+        j3_reference_deg=j3_reference_deg,
+        j3_weight=1e-5,
+        j4_min_deg=10.0,
+        j4_warn_deg=25.0,
+        j4_weight=1e-4,
+        manipulability_sigma_stop=0.01,
+        manipulability_sigma_warn=0.04,
+        manipulability_weight=1e-4,
+    )
+    joints = np.radians(joint_degrees).tolist()
+    solver.update_joint_state(joints)
+
+    assert solver._j3_task.get_joint(
+        solver._joint_names[2]
+    ) == pytest.approx(math.radians(j3_reference_deg))
+    assert np.asarray(solver._j4_constraint.A)[
+        solver._q_offsets[3]
+    ] == pytest.approx(-1.0)
+    assert np.asarray(solver._j4_constraint.b) == pytest.approx(
+        [-math.radians(10.0)]
+    )
+    assert solver._j4_constraint.priority == "hard"
+    jacobian = solver._active_tcp_jacobian()
+    assert jacobian.shape == (6, 7)
+    assert np.isfinite(jacobian).all()
+    assert np.linalg.svd(jacobian, compute_uv=False)[-1] > 0.0
+
+
+def test_failed_qp_restores_internal_state_to_actual_feedback() -> None:
+    solver, joints = _dual_placo_solver("left", ARM_CASES[0][1])
+    current_pose = solver.update_joint_state(joints)
+    unreachable = current_pose.copy()
+    unreachable[2, 3] += 10.0
+
+    with pytest.raises((RuntimeError, ValueError)):
+        solver.solve(unreachable)
+
+    assert solver._robot.state.q[solver._q_offsets] == pytest.approx(joints)
+
+
+def test_solver_rejects_non_positive_manipulability_threshold() -> None:
+    pytest.importorskip("placo")
+
+    with pytest.raises(ValueError, match="manipulability thresholds"):
+        PlacoIkSolver(
+            str(DUAL_URDF_PATH),
+            1.0 / 90.0,
+            "left",
+            manipulability_sigma_stop=0.0,
+            manipulability_sigma_warn=0.04,
+        )
+
+
+@pytest.mark.parametrize(
+    "q4_deg,sigma_min,expected_activation",
+    [
+        (25.0, 0.04, 0.0),
+        (17.5, 0.025, 0.5),
+        (10.0, 0.01, 1.0),
+    ],
+)
+def test_auxiliary_weights_activate_only_inside_warning_margins(
+    q4_deg: float,
+    sigma_min: float,
+    expected_activation: float,
+) -> None:
+    class TaskSpy:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def configure(self, name, priority, weight) -> None:
+            self.calls.append((name, priority, weight))
+
+    solver = object.__new__(PlacoIkSolver)
+    solver._q_offsets = np.arange(7, 14)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(21))
+    )
+    solver._robot.state.q[solver._q_offsets[3]] = math.radians(q4_deg)
+    solver._j4_task = TaskSpy()
+    solver._j4_min = math.radians(10.0)
+    solver._j4_warn = math.radians(25.0)
+    solver._j4_weight = 1e-4
+    solver._manipulability_task = TaskSpy()
+    solver._manipulability_sigma_stop = 0.01
+    solver._manipulability_sigma_warn = 0.04
+    solver._manipulability_weight = 1e-4
+    jacobian = np.zeros((6, 7))
+    jacobian[:, :6] = np.diag([1.0] * 5 + [sigma_min])
+    solver._active_tcp_jacobian = lambda: jacobian
+
+    solver._update_auxiliary_task_weights()
+
+    expected_weight = 1e-4 * expected_activation
+    assert solver._j4_task.calls == [
+        ("j4_soft_buffer", "soft", pytest.approx(expected_weight))
+    ]
+    assert solver._manipulability_task.calls == [
+        ("tcp_6d_manipulability", "soft", pytest.approx(expected_weight))
+    ]