YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
YikaiFu-cart
2026-08-03 16:00:33 +08:00
parent a9c605c804
commit 99cb45ef6d
1 changed files with 26 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/test_placo_transforms.py
+26 -11
View File
@@ -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)
world_tcp = solver._robot.get_T_world_frame(solver._tcp_frame)
assert solver._base_frame == expected_base_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)