50 lines
1.7 KiB
Python
50 lines
1.7 KiB
Python
import math
|
|
|
|
import numpy as np
|
|
import pytest
|
|
|
|
from xr_rm_teleop.placo_ik_solver import (
|
|
PlacoIkSolver,
|
|
_arm_pose_to_transform,
|
|
_tool_pose_to_transform,
|
|
_transform_to_arm_pose,
|
|
)
|
|
from xr_rm_teleop.realman_adapter import ArmPose
|
|
|
|
|
|
def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
|
|
flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
|
|
tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
|
|
|
|
base_to_flange = _arm_pose_to_transform(flange_pose)
|
|
flange_to_tool = _tool_pose_to_transform(tool_pose)
|
|
base_to_tool = base_to_flange @ flange_to_tool
|
|
recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)
|
|
|
|
assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
|
|
assert recovered_flange == pytest.approx(base_to_flange)
|
|
|
|
|
|
def test_transform_to_arm_pose_roundtrip() -> None:
|
|
expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
|
|
|
|
actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))
|
|
|
|
assert actual.xyz() == pytest.approx(expected.xyz())
|
|
assert actual.rpy() == pytest.approx(expected.rpy())
|
|
|
|
|
|
def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
|
|
solver = object.__new__(PlacoIkSolver)
|
|
solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
|
|
solver._velocity_limits = np.ones(7)
|
|
solver._dt = 0.1
|
|
solver._actual_joints = np.zeros(7)
|
|
|
|
with pytest.raises(ValueError, match="finite"):
|
|
solver._validate_result(np.full(7, np.nan))
|
|
with pytest.raises(ValueError, match="position"):
|
|
solver._validate_result(np.full(7, 2.0))
|
|
with pytest.raises(ValueError, match="velocity"):
|
|
solver._validate_result(np.full(7, 0.2))
|