"""Tests for ik.py. Run: python -m pytest -q""" from __future__ import annotations import numpy as np import pytest from ik import (fk_planar, ik_2link, ik_solve, jacobian_planar, manipulability, numerical_jacobian, resolved_rate_step) L1, L2 = 0.30, 0.20 LENGTHS = np.array([L1, L2]) def fk2(q: np.ndarray) -> np.ndarray: return fk_planar(q, LENGTHS) def jac2(q: np.ndarray) -> np.ndarray: return jacobian_planar(q, LENGTHS) def fk_3d(q: np.ndarray) -> np.ndarray: """Base yaw + shoulder pitch + elbow pitch arm (example lengths, metres).""" h, a, b = 0.05, 0.12, 0.12 radial = a * np.cos(q[1]) + b * np.cos(q[1] + q[2]) height = h + a * np.sin(q[1]) + b * np.sin(q[1] + q[2]) return np.array([radial * np.cos(q[0]), radial * np.sin(q[0]), height]) def test_worked_example_numbers() -> None: q1, q2 = ik_2link(0.35, 0.20, L1, L2, "down") assert np.degrees(q1) == pytest.approx(1.22, abs=0.01) assert np.degrees(q2) == pytest.approx(74.29, abs=0.01) @pytest.mark.parametrize("elbow", ["up", "down"]) def test_fk_of_analytic_ik_returns_target(elbow: str) -> None: rng = np.random.default_rng(1) for _ in range(200): r = rng.uniform(abs(L1 - L2) + 1e-3, L1 + L2 - 1e-3) phi = rng.uniform(-np.pi, np.pi) target = np.array([r * np.cos(phi), r * np.sin(phi)]) q = np.array(ik_2link(*target, L1, L2, elbow=elbow)) np.testing.assert_allclose(fk2(q), target, atol=1e-12) assert (q[1] >= 0) if elbow == "down" else (q[1] <= 0) def test_unreachable_target_raises() -> None: with pytest.raises(ValueError): ik_2link(0.6, 0.0, L1, L2) # beyond l1 + l2 with pytest.raises(ValueError): ik_2link(0.05, 0.0, L1, L2) # inside |l1 - l2| def test_boundary_of_workspace_gives_straight_arm() -> None: q1, q2 = ik_2link(L1 + L2, 0.0, L1, L2) assert q1 == pytest.approx(0.0, abs=1e-9) assert q2 == pytest.approx(0.0, abs=1e-9) def test_analytic_jacobian_matches_finite_difference() -> None: rng = np.random.default_rng(2) lengths = np.array([0.2, 0.15, 0.1, 0.05]) for _ in range(20): q = rng.uniform(-np.pi, np.pi, size=4) j_num = numerical_jacobian(lambda qq: fk_planar(qq, lengths), q) np.testing.assert_allclose(jacobian_planar(q, lengths), j_num, atol=1e-8) def test_det_and_manipulability_of_2link() -> None: q = np.array([0.4, 1.1]) jmat = jac2(q) assert np.linalg.det(jmat) == pytest.approx(L1 * L2 * np.sin(q[1])) assert manipulability(jmat) == pytest.approx(L1 * L2 * abs(np.sin(q[1]))) assert manipulability(jac2(np.array([0.4, 0.0]))) == pytest.approx(0.0, abs=1e-8) @pytest.mark.parametrize("method,kw", [("pinv", {}), ("dls", {"lam": 0.01}), ("transpose", {"alpha": 10.0, "max_iter": 500})]) def test_iterative_ik_converges_on_reachable_target(method: str, kw: dict) -> None: target = np.array([0.35, 0.20]) res = ik_solve(fk2, target, np.array([0.3, 1.2]), method=method, jac=jac2, **kw) assert res.converged np.testing.assert_allclose(fk2(res.q), target, atol=1e-6) def test_dls_with_numerical_jacobian_on_3d_arm() -> None: rng = np.random.default_rng(3) for _ in range(20): q_true = rng.uniform([-1.5, 0.2, -2.0], [1.5, 1.2, -0.3]) target = fk_3d(q_true) res = ik_solve(fk_3d, target, np.array([0.0, 0.7, -1.0]), method="dls", lam=0.01, max_step=0.3, max_iter=300) assert res.converged np.testing.assert_allclose(fk_3d(res.q), target, atol=1e-6) def test_redundant_chain_converges() -> None: lengths = np.array([0.20, 0.15, 0.10]) target = np.array([0.25, 0.15]) res = ik_solve(lambda q: fk_planar(q, lengths), target, np.array([0.1, 0.1, 0.1]), method="dls", lam=0.01, max_step=0.3) assert res.converged assert res.iterations < 20 def test_singular_target_pinv_blows_up_dls_stays_bounded() -> None: target = np.array([0.52, 0.10]) # 0.5295 m away, reach is 0.50 m q0 = np.array([0.3, 1.2]) pinv = ik_solve(fk2, target, q0, method="pinv", jac=jac2, max_iter=100) dls = ik_solve(fk2, target, q0, method="dls", lam=0.05, jac=jac2, max_iter=100) assert not pinv.converged and not dls.converged assert max(pinv.step_norms) > 10.0 # huge joint jumps near q2 = 0 assert max(dls.step_norms) < 1.0 # DLS settles at the closest reachable point: arm straight, pointing at target closest = target / np.linalg.norm(target) * (L1 + L2) np.testing.assert_allclose(fk2(dls.q), closest, atol=1e-3) assert dls.errors[-1] == pytest.approx(np.linalg.norm(target) - (L1 + L2), abs=1e-3) def test_dls_step_bounded_at_exact_singularity() -> None: q = np.array([0.0, 0.0]) # straight arm, det J = 0 jmat = jac2(q) err = np.array([0.01, 0.0]) # along the arm: impossible direction lam = 0.05 dq = jmat.T @ np.linalg.solve(jmat @ jmat.T + lam**2 * np.eye(2), err) assert np.all(np.isfinite(dq)) # Largest DLS gain is 1 / (2 lam) whatever the singular values are assert np.linalg.norm(dq) <= np.linalg.norm(err) / (2 * lam) + 1e-12 def test_joint_limits_respected() -> None: lengths = np.array([0.20, 0.15, 0.10]) q_min, q_max = np.radians([-90, 0, 0]), np.radians([90, 90, 45]) res = ik_solve(lambda q: fk_planar(q, lengths), np.array([0.25, 0.15]), np.array([0.3, 0.6, 0.6]), method="dls", lam=0.01, q_min=q_min, q_max=q_max) assert res.converged assert np.all(res.q >= q_min - 1e-12) and np.all(res.q <= q_max + 1e-12) def test_resolved_rate_tracks_straight_line() -> None: dt, steps = 0.002, 500 q = np.array(ik_2link(0.35, 0.10, L1, L2)) v = np.array([0.0, 0.10]) # 0.1 m/s upwards for 1 s start = fk2(q) for _ in range(steps): q = resolved_rate_step(q, v, jac2(q), dt) np.testing.assert_allclose(fk2(q), start + v * dt * steps, atol=2e-3)