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

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -100,6 +100,43 @@ def test_deployed_workspace_is_in_front_of_robot(config_name, node_names) -> Non
         assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
 
 
+@pytest.mark.parametrize(
+    "arm,single_config,dual_node,j3_reference_deg,j3_weight",
+    [
+        ("left", "left_arm_rm75.yaml", "left_arm_teleop", 67.96, 1e-5),
+        ("right", "right_arm_rm75.yaml", "right_arm_teleop", -89.57, 1e-4),
+    ],
+)
+def test_qp_optimization_parameters_match_single_and_dual_configs(
+    arm,
+    single_config,
+    dual_node,
+    j3_reference_deg,
+    j3_weight,
+) -> None:
+    del arm
+    with (CONFIG_DIR / single_config).open(encoding="utf-8") as stream:
+        single = yaml.safe_load(stream)["single_arm_velocity_teleop"][
+            "ros__parameters"
+        ]
+    with (CONFIG_DIR / "dual_arm_rm75.yaml").open(encoding="utf-8") as stream:
+        dual = yaml.safe_load(stream)[dual_node]["ros__parameters"]
+
+    expected = {
+        "qp_j3_reference_deg": j3_reference_deg,
+        "qp_j3_weight": j3_weight,
+        "qp_j4_min_deg": 10.0,
+        "qp_j4_warn_deg": 25.0,
+        "qp_j4_weight": 1e-4,
+        "qp_manipulability_sigma_stop": 0.01,
+        "qp_manipulability_sigma_warn": 0.04,
+        "qp_manipulability_weight": 1e-4,
+    }
+    for name, value in expected.items():
+        assert single[name] == pytest.approx(value)
+        assert dual[name] == pytest.approx(value)
+
+
 @pytest.mark.parametrize(
     ("existing", "expected_operation"),
     [(False, "create"), (True, "update")],

xr_rm_teleop/test/test_joint_control.py

@@ -11,6 +11,7 @@ from xr_rm_teleop.single_arm_velocity_teleop import (
     SingleArmVelocityTeleop,
     _make_transform,
     _so3_exp,
+    _so3_log,
 )
 
 
@@ -610,7 +611,7 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     assert teleop._ik_solver.solve_calls == 0
 
 
-def test_qp_failure_returns_last_known_good_target() -> None:
+def test_qp_failure_returns_none_and_keeps_last_known_good_target() -> None:
     class FailingSolver:
         def solve(self, target):
             del target
@@ -624,11 +625,11 @@ def test_qp_failure_returns_last_known_good_target() -> None:
 
     target = teleop._solve_joint_target(np.eye(4))
 
-    assert target == pytest.approx([0.1] * 7)
+    assert target is None
     assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
 
 
-def test_qp_success_updates_last_known_good_target() -> None:
+def test_qp_success_waits_for_send_before_updating_last_known_good_target() -> None:
     class SuccessfulSolver:
         def solve(self, target):
             del target
@@ -643,7 +644,54 @@ def test_qp_success_updates_last_known_good_target() -> None:
     target = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.2] * 7)
-    assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
+    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
+
+
+def test_target_filters_do_not_commit_candidate_state() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._filtered_target = [0.0, 0.0, 0.0]
+    teleop._filtered_orientation_target = np.eye(3)
+    teleop._target_filter_alpha = 0.5
+    teleop._target_filter_alpha_fast = 0.5
+    teleop._target_filter_fast_threshold_m = 1.0
+    teleop._orientation_filter_alpha = 0.5
+
+    position = teleop._filter_target([0.2, 0.0, 0.0])
+    orientation = teleop._filter_orientation_target(
+        _so3_exp(np.asarray([0.0, 0.0, 0.2]))
+    )
+
+    assert position == pytest.approx([0.1, 0.0, 0.0])
+    assert teleop._filtered_target == pytest.approx([0.0, 0.0, 0.0])
+    assert teleop._filtered_orientation_target == pytest.approx(np.eye(3))
+    assert np.linalg.norm(_so3_log(orientation)) == pytest.approx(0.1)
+
+
+def test_failed_send_does_not_commit_cartesian_reference_state() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._filtered_target = [0.2, 0.0, 0.0]
+    teleop._filtered_orientation_target = np.eye(3)
+    teleop._last_sent_target = [0.2, 0.0, 0.0]
+    teleop._last_sent_orientation = np.eye(3)
+    teleop._last_command_time = FakeTime()
+    teleop._send_joint_target = lambda joints: False
+
+    sent = teleop._send_and_commit_joint_target(
+        [0.3] * 7,
+        [0.3, 0.0, 0.0],
+        _so3_exp(np.asarray([0.0, 0.0, 0.1])),
+        [0.3, 0.0, 0.0],
+        _so3_exp(np.asarray([0.0, 0.0, 0.1])),
+        FakeTime(),
+    )
+
+    assert not sent
+    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
+    assert teleop._filtered_target == pytest.approx([0.2, 0.0, 0.0])
+    assert teleop._filtered_orientation_target == pytest.approx(np.eye(3))
+    assert teleop._last_sent_target == pytest.approx([0.2, 0.0, 0.0])
+    assert teleop._last_sent_orientation == pytest.approx(np.eye(3))
 
 
 def test_enter_active_control_initializes_se3_orientation_state() -> None:

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))
+    ]