test: 强化双臂逆解测试边界

xr_rm_teleop/test/test_placo_transforms.py

@@ -26,6 +26,8 @@ ARM_CASES = (
         list(range(14, 21)),
         list(range(13, 20)),
         "omnipic",
+        "scissor_base_link",
+        "scissor_scissor_tcp",
     ),
     (
         "right",
@@ -33,6 +35,8 @@ ARM_CASES = (
         list(range(7, 14)),
         list(range(6, 13)),
         "scissor",
+        "omnipic_base_link",
+        "omnipic_OmniPic_tcp",
     ),
 )
 
@@ -101,7 +105,8 @@ def _dual_placo_solver(
 
 
 @pytest.mark.parametrize(
-    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix",
+    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
+    "expected_base_frame,expected_tcp_frame",
     ARM_CASES,
 )
 def test_solver_uses_arm_specific_offsets(
@@ -110,6 +115,8 @@ def test_solver_uses_arm_specific_offsets(
     q_offsets: list[int],
     v_offsets: list[int],
     inactive_prefix: str,
+    expected_base_frame: str,
+    expected_tcp_frame: str,
 ) -> None:
     solver, _ = _dual_placo_solver(arm, joint_degrees)
 
@@ -118,7 +125,8 @@ def test_solver_uses_arm_specific_offsets(
 
 
 @pytest.mark.parametrize(
-    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix",
+    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
+    "expected_base_frame,expected_tcp_frame",
     ARM_CASES,
 )
 def test_joint_state_pose_is_relative_to_selected_arm_base(
@@ -127,18 +135,23 @@ def test_joint_state_pose_is_relative_to_selected_arm_base(
     q_offsets: list[int],
     v_offsets: list[int],
     inactive_prefix: str,
+    expected_base_frame: str,
+    expected_tcp_frame: str,
 ) -> None:
     solver, joints = _dual_placo_solver(arm, joint_degrees)
 
     actual_pose = solver.update_joint_state(joints)
-    world_base = solver._robot.get_T_world_frame(solver._base_frame)
+    assert solver._base_frame == expected_base_frame
-    world_tcp = solver._robot.get_T_world_frame(solver._tcp_frame)
+    assert solver._tcp_frame == expected_tcp_frame
+    world_base = solver._robot.get_T_world_frame(expected_base_frame)
+    world_tcp = solver._robot.get_T_world_frame(expected_tcp_frame)
 
     assert actual_pose == pytest.approx(np.linalg.inv(world_base) @ world_tcp)
 
 
 @pytest.mark.parametrize(
-    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix",
+    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
+    "expected_base_frame,expected_tcp_frame",
     ARM_CASES,
 )
 def test_qp_solve_converges_without_moving_inactive_arm(
@@ -147,6 +160,8 @@ def test_qp_solve_converges_without_moving_inactive_arm(
     q_offsets: list[int],
     v_offsets: list[int],
     inactive_prefix: str,
+    expected_base_frame: str,
+    expected_tcp_frame: str,
 ) -> None:
     solver, joints = _dual_placo_solver(arm, joint_degrees)
     start_pose = solver.update_joint_state(joints)
@@ -186,8 +201,6 @@ def test_qp_solve_converges_without_moving_inactive_arm(
 
 
 def test_solver_rejects_unknown_arm() -> None:
-    pytest.importorskip("placo")
-
     with pytest.raises(ValueError, match="arm must be left or right"):
         PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, "middle")
 
@@ -195,10 +208,11 @@ def test_solver_rejects_unknown_arm() -> None:
 def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
     solver = object.__new__(PlacoIkSolver)
     solver._actual_joints = np.zeros(7)
+    solver._q_offsets = np.arange(7, 14)
     solver._robot = SimpleNamespace(
-        state=SimpleNamespace(q=np.zeros(14))
+        state=SimpleNamespace(q=np.zeros(21))
     )
-    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._frame_task = SimpleNamespace(T_a_b=None)
     solver._target_errors = lambda: (1.5e-3, 0.0)
 
     result = solver.solve(np.eye(4))
@@ -209,11 +223,12 @@ def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
 def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
     solver = object.__new__(PlacoIkSolver)
     solver._actual_joints = np.zeros(7)
+    solver._q_offsets = np.arange(7, 14)
     solver._robot = SimpleNamespace(
-        state=SimpleNamespace(q=np.zeros(14)),
+        state=SimpleNamespace(q=np.zeros(21)),
         update_kinematics=lambda: None,
     )
-    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._frame_task = SimpleNamespace(T_a_b=None)
     solver._solver = SimpleNamespace(solve=lambda update: None)
     solver._validate_result = lambda result, previous: None
     solver._target_errors = lambda: (2.1e-3, 0.0)