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")],