"""Tests for fk.py. Run with: python -m pytest -q""" from __future__ import annotations import numpy as np import pytest from scipy.linalg import expm from fk import (SO101_ARM_NAMES, SO101_JOINTS, Joint, chain_fk, exp_twist, fit_poly_fk, fk_dh, joint_transform, parse_urdf_joints, planar_2link_fk, poe_fk, poly_fk, screw_axes, skew, so101_fk) DEG = np.pi / 180.0 RNG = np.random.default_rng(11) def is_rotation(R: np.ndarray) -> bool: return bool(np.allclose(R.T @ R, np.eye(3)) and np.isclose(np.linalg.det(R), 1.0)) # --- planar 2-link ----------------------------------------------------------- def test_2link_hand_example() -> None: elbow, tip = planar_2link_fk(30 * DEG, 45 * DEG, 0.116, 0.135) assert np.allclose(elbow, [0.116 * np.cos(30 * DEG), 0.058]) assert np.allclose(tip, [0.13540, 0.18840], atol=5e-5) def test_2link_special_poses() -> None: _, tip = planar_2link_fk(0.0, 0.0, 0.3, 0.2) assert np.allclose(tip, [0.5, 0.0]) # stretched along x _, tip = planar_2link_fk(90 * DEG, -90 * DEG, 0.3, 0.2) assert np.allclose(tip, [0.2, 0.3]) # link 1 up, link 2 along x _, tip = planar_2link_fk(0.0, np.pi, 0.3, 0.2) assert np.allclose(tip, [0.1, 0.0]) # folded back def test_2link_reach_bounds() -> None: l1, l2 = 0.116, 0.135 q = RNG.uniform(-np.pi, np.pi, size=(2000, 2)) r = np.array([np.linalg.norm(planar_2link_fk(a, b, l1, l2)[1]) for a, b in q]) assert r.max() <= l1 + l2 + 1e-12 assert r.min() >= abs(l1 - l2) - 1e-12 # --- DH ---------------------------------------------------------------------- def test_standard_dh_matches_2link_formula() -> None: l1, l2 = 0.116, 0.135 for a, b in RNG.uniform(-np.pi, np.pi, size=(50, 2)): T = fk_dh([(0.0, l1, 0.0), (0.0, l2, 0.0)], [a, b]) assert np.allclose(T[:2, 3], planar_2link_fk(a, b, l1, l2)[1]) assert np.isclose(np.arctan2(T[1, 0], T[0, 0]), np.arctan2(np.sin(a + b), np.cos(a + b))) def test_modified_dh_matches_2link_with_tool_offset() -> None: # Modified DH puts a_{i-1} before joint i, so the last link length becomes a tool offset. l1, l2 = 0.116, 0.135 for a, b in RNG.uniform(-np.pi, np.pi, size=(50, 2)): T = fk_dh([(0.0, 0.0, 0.0), (0.0, l1, 0.0)], [a, b], modified=True) T_tool = T @ np.array([[1, 0, 0, l2], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1.0]]) assert np.allclose(T_tool[:2, 3], planar_2link_fk(a, b, l1, l2)[1]) def test_dh_3link_planar() -> None: L = [0.3, 0.2, 0.1] q = np.array([20, 30, -40]) * DEG T = fk_dh([(0.0, li, 0.0) for li in L], q) angles = np.cumsum(q) expected = [sum(li * np.cos(t) for li, t in zip(L, angles)), sum(li * np.sin(t) for li, t in zip(L, angles))] assert np.allclose(T[:2, 3], expected) # --- URDF chains ------------------------------------------------------------- def test_identity_chain() -> None: j = Joint("j", (0.0, 0.0, 0.0), (0.0, 0.0, 0.0)) assert np.allclose(chain_fk([j, j], [0.0, 0.0])[-1], np.eye(4)) def test_joint_transform_rotates_after_origin() -> None: j = Joint("j", (1.0, 0.0, 0.0), (0.0, 0.0, 0.0), axis=(0.0, 0.0, 1.0)) T = joint_transform(j, 90 * DEG) assert np.allclose(T[:3, 3], [1.0, 0.0, 0.0]) # origin is not moved by q assert np.allclose(T[:3, :3] @ [1, 0, 0], [0, 1, 0]) def test_parse_urdf_joints() -> None: urdf = """ """ j1, j2 = parse_urdf_joints(urdf, ["shoulder_pan", "j2"]) assert j1 == SO101_JOINTS[0] assert j2.kind == "fixed" and j2.xyz == SO101_JOINTS[-1].xyz and j2.rpy == SO101_JOINTS[-1].rpy def test_so101_chain_layout() -> None: assert [j.name for j in SO101_JOINTS if j.kind == "revolute"] == SO101_ARM_NAMES assert len(chain_fk(SO101_JOINTS, np.zeros(5))) == 7 def test_so101_frames_are_rigid_transforms() -> None: for _ in range(50): q = np.array([RNG.uniform(j.lower, j.upper) for j in SO101_JOINTS[:5]]) for T in chain_fk(SO101_JOINTS, q): assert is_rotation(T[:3, :3]) assert np.allclose(T[3], [0, 0, 0, 1]) def test_so101_link_lengths_do_not_change_with_q() -> None: def elbow_to_wrist(q: np.ndarray) -> float: f = chain_fk(SO101_JOINTS, q) return float(np.linalg.norm(f[4][:3, 3] - f[3][:3, 3])) # wrist_flex origin - elbow origin d0 = elbow_to_wrist(np.zeros(5)) assert d0 == pytest.approx(np.hypot(0.1349, 0.0052), abs=1e-9) for _ in range(20): assert elbow_to_wrist(RNG.uniform(-1.5, 1.5, size=5)) == pytest.approx(d0) def test_so101_pan_axis_is_vertical() -> None: # Rotating shoulder_pan changes x and y of the gripper frame but not its height. q = np.array([0.0, -0.3, 0.6, 0.2, 0.4]) z0 = so101_fk(q)[2, 3] r0 = np.hypot(*so101_fk(q)[:2, 3] - SO101_JOINTS[0].xyz[:2]) for pan in np.linspace(-1.5, 1.5, 7): q[0] = pan p = so101_fk(q)[:3, 3] assert p[2] == pytest.approx(z0, abs=1e-5) # rpy in the URDF uses 3.14159, not pi assert np.hypot(*(p[:2] - SO101_JOINTS[0].xyz[:2])) == pytest.approx(r0, abs=1e-5) def test_so101_wrist_roll_does_not_move_its_own_axis_point() -> None: q = np.array([0.2, -0.4, 0.5, 0.3, 0.0]) p0 = chain_fk(SO101_JOINTS, q)[5][:3, 3] # gripper_link origin q[4] = 1.2 assert np.allclose(chain_fk(SO101_JOINTS, q)[5][:3, 3], p0) def test_so101_home_pose_values() -> None: p = so101_fk(np.zeros(5))[:3, 3] assert np.allclose(p, [0.3914, 0.0, 0.2265], atol=5e-4) # --- product of exponentials ------------------------------------------------- def test_exp_twist_matches_matrix_exponential() -> None: for _ in range(20): w = RNG.normal(size=3) w /= np.linalg.norm(w) v = RNG.normal(size=3) th = RNG.uniform(-3, 3) se3 = np.zeros((4, 4)) se3[:3, :3], se3[:3, 3] = skew(w), v assert np.allclose(exp_twist(np.concatenate((w, v)), th), expm(se3 * th)) def test_poe_equals_urdf_chain_for_so101() -> None: S_list, M = screw_axes(SO101_JOINTS) assert np.allclose(M, so101_fk(np.zeros(5))) for _ in range(50): q = RNG.uniform(-1.6, 1.6, size=5) assert np.allclose(poe_fk(S_list, M, q), so101_fk(q)) # --- learned model ----------------------------------------------------------- def test_polynomial_fk_fits_inside_but_not_outside_training_range() -> None: rng = np.random.default_rng(3) l1, l2 = 0.116, 0.135 q_train = rng.uniform(0, np.pi / 2, size=(500, 2)) xy = np.array([planar_2link_fk(a, b, l1, l2)[1] for a, b in q_train]) xy += rng.normal(0.0, 0.001, size=xy.shape) coef = fit_poly_fk(q_train, xy, degree=4) q_in = rng.uniform(0, np.pi / 2, size=(500, 2)) q_out = rng.uniform(np.pi / 2, np.pi, size=(500, 2)) err = lambda qs: np.mean(np.linalg.norm( # noqa: E731 poly_fk(coef, qs, 4) - np.array([planar_2link_fk(a, b, l1, l2)[1] for a, b in qs]), axis=1)) assert err(q_in) < 0.001 # under 1 mm inside the training range assert err(q_out) > 0.05 # more than 5 cm outside it