65 lines
1.8 KiB
Python
65 lines
1.8 KiB
Python
import numpy as np
|
|
import pinocchio as pin
|
|
import pytest
|
|
|
|
from rm75_ik import (
|
|
RM75Kinematics,
|
|
physical_joint_limits,
|
|
pose_errors,
|
|
teleop_joint_limits,
|
|
)
|
|
|
|
|
|
def test_limit_profiles_match_stage_one_contract():
|
|
physical = physical_joint_limits()
|
|
teleop = teleop_joint_limits()
|
|
|
|
np.testing.assert_allclose(
|
|
np.rad2deg(physical.upper), [178, 130, 178, 135, 178, 128, 360]
|
|
)
|
|
np.testing.assert_allclose(
|
|
np.rad2deg(teleop.lower), [-150, -30, -170, -130, -175, -125, -179]
|
|
)
|
|
np.testing.assert_allclose(
|
|
np.rad2deg(teleop.upper), [150, 110, 170, 130, 175, 125, 179]
|
|
)
|
|
|
|
|
|
def test_zero_configuration_reaches_documented_flange_height():
|
|
kinematics = RM75Kinematics()
|
|
pose = kinematics.forward(np.zeros(7))
|
|
|
|
np.testing.assert_allclose(pose.translation, [0.0, 0.0, 0.8505], atol=3e-6)
|
|
np.testing.assert_allclose(pose.rotation, np.eye(3), atol=1e-7)
|
|
|
|
|
|
def test_tool_pose_is_composed_after_flange():
|
|
kinematics = RM75Kinematics()
|
|
q = np.deg2rad([30, -20, 40, 60, -50, 25, 90])
|
|
tool = pin.SE3(np.eye(3), np.array([0.0, 0.0, 0.16]))
|
|
|
|
expected = kinematics.forward(q) * tool
|
|
actual = kinematics.forward(q, tool)
|
|
|
|
assert pose_errors(expected, actual) == pytest.approx((0.0, 0.0), abs=1e-12)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
"configuration",
|
|
[
|
|
np.zeros(6),
|
|
np.full(7, np.nan),
|
|
np.deg2rad([179, 0, 0, 0, 0, 0, 0]),
|
|
],
|
|
)
|
|
def test_invalid_configuration_is_rejected(configuration):
|
|
with pytest.raises(ValueError):
|
|
RM75Kinematics().forward(configuration)
|
|
|
|
|
|
def test_jacobian_has_expected_shape_and_is_finite():
|
|
jacobian = RM75Kinematics().jacobian(np.deg2rad([10, 20, -30, 40, 50, -60, 70]))
|
|
assert jacobian.shape == (6, 7)
|
|
assert np.all(np.isfinite(jacobian))
|
|
|