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

test: 添加双 RM75 逆解回归

Browse Source
This commit is contained in:
YikaiFu-cart
2026-08-03 15:49:58 +08:00
parent 7edc28b44a
commit 2d197a8928
1 changed files with 142 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/test_placo_transforms.py
+137 -45
View File
@@ -7,76 +7,156 @@ import numpy as np
import pytest
from xr_rm_teleop.placo_ik_solver import (
QP_ORIENTATION_TOLERANCE_RAD,
QP_POSITION_TOLERANCE_M,
PlacoIkSolver,
_validated_transform,
)
def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
urdf_path = (
DUAL_URDF_PATH = (
Path(__file__).resolve().parents[1]
/ "models"
/ "rm75_omnipicker"
/ "urdf"
/ "RM75-B_OmniPicker_fixed.urdf"
/ "dual_rm75"
/ "Dual_arm.urdf"
)
root = ElementTree.parse(urdf_path).getroot()
ARM_CASES = (
(
"left",
[-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
list(range(14, 21)),
list(range(13, 20)),
"omnipic",
),
(
"right",
[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
list(range(7, 14)),
list(range(6, 13)),
"scissor",
),
)
def test_dual_urdf_has_expected_joints_meshes_and_tcp_frames() -> None:
root = ElementTree.parse(DUAL_URDF_PATH).getroot()
moving_joint_names = [
joint.attrib["name"]
for joint in root.findall("joint")
if joint.attrib["type"] != "fixed"
]
tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']")
mesh_filenames = [
mesh.attrib["filename"]
for mesh in root.findall(".//mesh")
]
fixed_joints = {
"omnipic_base_mount_joint": (
"dual_arm_base_link",
"omnipic_base_link",
None,
),
"scissor_base_mount_joint": (
"dual_arm_base_link",
"scissor_base_link",
None,
),
"omnipic_OmniPic_tcp_fixed": (
"omnipic_gripper_link",
"omnipic_OmniPic_tcp",
"0 0 0.14",
),
"scissor_scissor_tcp_fixed": (
"scissor_scissor_link",
"scissor_scissor_tcp",
"0 0 0",
),
"scissor_scissor_fixed_joint": (
"scissor_link_7",
"scissor_scissor_link",
"0 0 0.165",
),
}
assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
assert all(
filename.startswith(
"package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
)
for filename in mesh_filenames
)
assert tcp_joint is not None
assert tcp_joint.attrib["type"] == "fixed"
assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
assert moving_joint_names == [
*[f"omnipic_joint_{index}" for index in range(1, 8)],
*[f"scissor_joint_{index}" for index in range(1, 8)],
]
assert all(filename.startswith("meshes/") for filename in mesh_filenames)
for name, (parent, child, xyz) in fixed_joints.items():
joint = root.find(f"joint[@name='{name}']")
assert joint is not None
assert joint.attrib["type"] == "fixed"
assert joint.find("parent").attrib["link"] == parent
assert joint.find("child").attrib["link"] == child
if xyz is not None:
assert joint.find("origin").attrib["xyz"] == xyz
def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
def _dual_placo_solver(
arm: str,
joint_degrees: list[float],
) -> tuple[PlacoIkSolver, list[float]]:
pytest.importorskip("placo")
urdf_path = (
Path(__file__).resolve().parents[1]
/ "models"
/ "rm75_omnipicker"
/ "urdf"
/ "RM75-B_OmniPicker_fixed.urdf"
joints = [math.radians(value) for value in joint_degrees]
return PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, arm), joints
@pytest.mark.parametrize(
"arm,joint_degrees,q_offsets,v_offsets,inactive_prefix",
ARM_CASES,
)
joints = [
math.radians(value)
for value in [
-90.14,
3.76,
-86.89,
87.89,
-96.53,
-79.62,
-90.04,
]
]
return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints
def test_solver_uses_arm_specific_offsets(
arm: str,
joint_degrees: list[float],
q_offsets: list[int],
v_offsets: list[int],
inactive_prefix: str,
) -> None:
solver, _ = _dual_placo_solver(arm, joint_degrees)
assert solver._q_offsets == q_offsets
assert solver._v_offsets == v_offsets
def test_qp_solve_converges_to_reachable_tcp_target() -> None:
solver, joints = _rm75_placo_solver()
@pytest.mark.parametrize(
"arm,joint_degrees,q_offsets,v_offsets,inactive_prefix",
ARM_CASES,
)
def test_joint_state_pose_is_relative_to_selected_arm_base(
arm: str,
joint_degrees: list[float],
q_offsets: list[int],
v_offsets: list[int],
inactive_prefix: 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 actual_pose == pytest.approx(np.linalg.inv(world_base) @ world_tcp)
@pytest.mark.parametrize(
"arm,joint_degrees,q_offsets,v_offsets,inactive_prefix",
ARM_CASES,
)
def test_qp_solve_converges_without_moving_inactive_arm(
arm: str,
joint_degrees: list[float],
q_offsets: list[int],
v_offsets: list[int],
inactive_prefix: str,
) -> None:
solver, joints = _dual_placo_solver(arm, joint_degrees)
start_pose = solver.update_joint_state(joints)
inactive_q_offsets = [
solver._robot.get_joint_offset(f"{inactive_prefix}_joint_{index}")
for index in range(1, 8)
]
inactive_before = solver._robot.state.q[inactive_q_offsets].copy()
target_pose = start_pose.copy()
target_pose[0, 3] += 0.07
target_pose[0, 3] += 0.01
result = solver.solve(target_pose)
reached_pose = solver.update_joint_state(result)
@@ -96,8 +176,20 @@ def test_qp_solve_converges_to_reachable_tcp_target() -> None:
)
)
assert np.asarray(result).shape == (7,)
assert np.isfinite(result).all()
assert position_error <= QP_POSITION_TOLERANCE_M
assert orientation_error <= 5e-3
assert orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
assert solver._robot.state.q[inactive_q_offsets] == pytest.approx(
inactive_before
)
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")
def test_qp_solve_accepts_position_error_within_two_millimeters() -> None: