From ebb745b10e2b8dd41e3b6c19a397b6f68e9da8c6 Mon Sep 17 00:00:00 2001 From: openhands Date: Fri, 28 Aug 2026 01:42:02 +0000 Subject: [PATCH] Add finite-difference tests for the _impl VFI constraint Jacobians Complements tests/test_finite_difference.py (which checks the raw dqrobotics distance/plane Jacobians) by validating, by central finite differences, the wrapper Jacobians defined in _impl.py that build the NeedleController VFI rows: - rotation_axis_jacobian == d/dq of Ad(r(q), a) (vector-valued) - J_phi_z == d/dq of - normal_dot_product_jacobian == d/dq of - J_dot_product_safe == d/dh[cos(phi_safe(h))] * Jh (and exactly zero in the saturated regions h <= h_min / h >= h_max) - phi_z_constraint_W == d/dq of [dot_product_safe(h(q)) - phi_z(q)] - insertion_J_D_safe == d/dq of tan^2(angle) * d_plane(q)^2 - insertion_W == d/dq of [D(q) - D_safe(q)] - needle_jacobian == every stacked row (radius / plane / normal / insertion / angular) is the FD gradient of its matching constraint scalar, with the correct sign and row order. The insertion tests place the needle tip at h_mid = (h_min + h_max)/2 above the vessel plane so the depth-dependent phi_z row sits in the linear (non-saturated) region and is differentiable. Pure-Python dqrobotics only (no compiled _core required). Co-authored-by: openhands --- tests/test_impl_finite_difference.py | 387 +++++++++++++++++++++++++++ 1 file changed, 387 insertions(+) create mode 100644 tests/test_impl_finite_difference.py diff --git a/tests/test_impl_finite_difference.py b/tests/test_impl_finite_difference.py new file mode 100644 index 0000000..cc00b73 --- /dev/null +++ b/tests/test_impl_finite_difference.py @@ -0,0 +1,387 @@ +"""Finite-difference validation of the ``_impl`` VFI constraint Jacobians. + +Complements :mod:`tests.test_finite_difference` (which checks the raw +``dqrobotics`` distance/plane Jacobians) by checking the **wrapper** Jacobians +defined in :mod:`marinholab.working.needlemanipulation._impl`, i.e. the +functions that build the ``NeedleController`` VFI rows. Each one is validated +by central finite differences against the scalar (or vector) quantity its +docstring/name says it differentiates: + +* :func:`_impl.rotation_axis_jacobian` -- ``d/dq Ad(r(q), a)`` (vector valued); +* :func:`_impl.J_phi_z` -- ``d/dq ``; +* :func:`_impl.normal_dot_product_jacobian` -- ``d/dq ``; +* :func:`_impl.J_dot_product_safe` -- ``d/dh cos(phi_safe(h)) * Jh`` + (zero in the saturated regions); +* :func:`_impl.phi_z_constraint_W` -- ``d/dq [dot_product_safe(h(q)) - phi_z(q)]``; +* :func:`_impl.insertion_J_D_safe` -- ``d/dq tan^2(angle) * d_plane(q)^2``; +* :func:`_impl.insertion_W` -- ``d/dq [D(q) - D_safe(q)]`` (point-to-line + squared minus the safe radius band); +* :func:`_impl.needle_jacobian` -- every stacked row (radius / plane / + normal / insertion / angular) is the FD gradient of its matching + constraint scalar, with the correct sign and row order. + +Pure-Python ``dqrobotics`` only (no compiled ``_core`` required). A fixed, +well-conditioned 4-DOF arm and off-manifold vessel primitives keep every row +non-degenerate. +""" +from __future__ import annotations + +import math + +import numpy as np +import pytest + +from dqrobotics import DQ, Ad, E_, cross, dot, k_, rotation, translation +from dqrobotics.robot_modeling import DQ_Kinematics, DQ_SerialManipulatorDH +from dqrobotics.utils import DQ_Geometry + +from marinholab.working.needlemanipulation import _impl + +# 5x4 DH parameter matrix (a, alpha, d, theta, joint_type=1) for a 4-DOF arm. +_DH_PARAMS = np.array( + [ + [0.0, 0.3, 0.0, 0.35], + [0.0, 0.0, 1.5707963267948966, 1.5707963267948966], + [0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0], + [1, 1, 1, 1], + ] +) +_Q0 = np.array([0.5, -0.3, 0.9, 0.2]) +_DOF = _Q0.size +_FD_TOL = 2e-6 +_EPS = 1e-6 + +# Depth-dependent angular (phi_z) insertion constraint constants. These mirror +# the values hard-coded in ``_impl.needle_jacobian`` / ``needle_w``; the +# reference finite difference below depends on them, so they must stay in sync. +_INSERTION_ANGLE = 2.0 * math.pi / 3.0 +_PHI_MIN = math.pi / 6.0 +_PHI_MAX = math.pi / 2.0 +_H_MIN = 0.0 +_H_MAX = 0.05 + +# Vessel-0 geometry, built at module scope. ``_impl`` places the angular +# insertion constraint in the linear (non-saturated) region only for +# ``h_min < h < h_max`` where ``h`` is the tip's signed distance to the vessel +# plane. We therefore put the needle tip (the frame position at ``_Q0``) +# exactly at ``h_mid = (h_min + h_max) / 2`` above the plane, plus a lateral +# (in-plane) offset so the tip is NOT on the vessel line. This keeps both the +# point-to-line and plane gradients non-degenerate and, crucially, keeps the +# depth-dependent ``phi_z`` row differentiable. +_ROBOT = DQ_SerialManipulatorDH(_DH_PARAMS) +_TIP0 = translation(_ROBOT.fkm(_Q0)) +_N1 = DQ([0.3, 0.7, 0.6]).normalize() +# Unit vector perpendicular to _N1 (Gram-Schmidt of a fixed reference axis). +_M0 = DQ([1.0, 0.0, 0.0]) +_M = (_M0 - _N1 * dot(_M0, _N1)).normalize() +_H_MID = (_H_MIN + _H_MAX) / 2.0 +_LAT_OFFSET = 0.03 +_P1 = _TIP0 - _H_MID * _N1 + _LAT_OFFSET * _M +_N2 = DQ([-0.4, 0.2, 0.9]).normalize() +_P2 = DQ([-0.06, 0.09, 0.04]) + +# A fixed unit axis for the rotated-axis Jacobian. +_A = DQ([0.3, 0.6, 0.7]).normalize() + +# Vessel-0 primitives span the insertion line/plane (same construction as +# ``_impl.needle_jacobian``: ``n + E_ * cross(p, n)`` / ``n + E_ * dot(p, n)``). +_INS_LINE = _N1 + E_ * cross(_P1, _N1) +_INS_PLANE = _N1 + E_ * dot(_P1, _N1) + + +def _pose(q: np.ndarray, robot) -> tuple[DQ, np.ndarray, np.ndarray, np.ndarray]: + """Return (x, Jx, Jr, Jt) at joint configuration ``q``.""" + x = robot.fkm(q) + Jx = robot.pose_jacobian(q) + Jr = np.asarray(DQ_Kinematics.rotation_jacobian(Jx), dtype=np.float64) + Jt = np.asarray( + DQ_Kinematics.translation_jacobian(Jx, x), dtype=np.float64 + ) + return x, Jx, Jr, Jt + + +def _fd_scalar(fn, q: np.ndarray) -> np.ndarray: + """Central finite-difference Jacobian of ``fn(q) -> float``.""" + jac = np.zeros(q.size) + for i in range(q.size): + qp = q.copy() + qm = q.copy() + qp[i] += _EPS + qm[i] -= _EPS + jac[i] = (fn(qp) - fn(qm)) / (2.0 * _EPS) + return jac + + +def _fd_vector(fn, q: np.ndarray) -> np.ndarray: + """Central finite-difference Jacobian of ``fn(q) -> (m,)``; returns (m, n).""" + cols = [] + for i in range(q.size): + qp = q.copy() + qm = q.copy() + qp[i] += _EPS + qm[i] -= _EPS + cols.append((np.asarray(fn(qp)) - np.asarray(fn(qm))) / (2.0 * _EPS)) + return np.column_stack(cols) + + +def _row(jac) -> np.ndarray: + """Flatten a (possibly (1, n)) Jacobian to a 1-D row.""" + return np.asarray(jac, dtype=float).ravel() + + +def _h(q: np.ndarray, robot) -> float: + """Signed distance from the needle tip (== frame, identity offset) to the + insertion plane -- the depth ``h`` of the angular insertion constraint.""" + return float( + DQ_Geometry.point_to_plane_distance(translation(robot.fkm(q)), _INS_PLANE) + ) + + +@pytest.fixture(scope="module") +def robot() -> DQ_SerialManipulatorDH: + r = DQ_SerialManipulatorDH(_DH_PARAMS) + r.set_lower_q_limit(np.full(_DOF, -2.0)) + r.set_upper_q_limit(np.full(_DOF, 2.0)) + return r + + +@pytest.fixture(scope="module") +def pose0(robot): + x, Jx, Jr, Jt = _pose(_Q0, robot) + return {"x": x, "Jx": Jx, "Jr": Jr, "Jt": Jt, "r": rotation(x)} + + +# --- rotated / orientation Jacobians --------------------------------------- + + +def test_rotation_axis_jacobian(pose0, robot): + # rotation_axis_jacobian(a, r, Jr) must equal d/dq of Ad(r(q), a) + # (vector-valued, 4x4). ``a`` is a fixed unit axis. + x, Jr, r = pose0["x"], pose0["Jr"], pose0["r"] + closed = _impl.rotation_axis_jacobian(_A, r, Jr) + fd = _fd_vector(lambda qq: Ad(rotation(robot.fkm(qq)), _A).vec4(), _Q0) + assert np.asarray(closed).shape == fd.shape == (_DOF, _DOF) + assert np.max(np.abs(np.asarray(closed) - fd)) < _FD_TOL + + +def test_J_phi_z(pose0, robot): + # J_phi_z(n, r, Jr) must equal d/dq of phi_z = . + Jr, r = pose0["Jr"], pose0["r"] + closed = _row(_impl.J_phi_z(_N1, r, Jr)) + fd = _fd_scalar( + lambda qq: _impl.phi_z(_N1, rotation(robot.fkm(qq))), _Q0 + ) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + + +def test_normal_dot_product_jacobian(pose0, robot): + # Must equal d/dq of for a fixed unit axis a. + Jr, r = pose0["Jr"], pose0["r"] + closed = _row(_impl.normal_dot_product_jacobian(_N2, _A, r, Jr)) + fd = _fd_scalar( + lambda qq: float(dot(_N2, Ad(rotation(robot.fkm(qq)), _A)).q[0]), _Q0 + ) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + + +# --- safe-angle (phi_safe) chain rule -------------------------------------- + + +def test_J_dot_product_safe_chain_rule(): + # Interior: J_dot_product_safe == d/dh[dot_product_safe(h)] * Jh. + h = 0.025 + Jh = np.arange(4.0) + dh = _fd_scalar( + lambda qq: _impl.dot_product_safe(qq[0], _PHI_MIN, _PHI_MAX, _H_MIN, _H_MAX), + np.array([h]), + )[0] + closed = _row(_impl.J_dot_product_safe(h, Jh, _PHI_MIN, _PHI_MAX, _H_MIN, _H_MAX)) + assert closed.shape == Jh.shape == (4,) + assert np.max(np.abs(closed - dh * Jh)) < 1e-7 + + +def test_J_dot_product_safe_saturated_regions(): + # Outside [h_min, h_max] the safe angle is constant, so the Jacobian is 0. + Jh = np.arange(4.0) + assert np.allclose( + _impl.J_dot_product_safe(_H_MAX + 1e-9, Jh, _PHI_MIN, _PHI_MAX, _H_MIN, _H_MAX), + 0.0, + ) + assert np.allclose( + _impl.J_dot_product_safe(_H_MIN - 1e-9, Jh, _PHI_MIN, _PHI_MAX, _H_MIN, _H_MAX), + 0.0, + ) + + +# --- angular insertion constraint ------------------------------------------ + + +def test_phi_z_constraint_W(pose0, robot): + # phi_z_constraint_W == d/dq [dot_product_safe(h(q)) - phi_z(q)], where + # h(q) is the tip's signed distance to the insertion plane. + Jr, r = pose0["Jr"], pose0["r"] + Jx, Jt = pose0["Jx"], pose0["Jt"] + h = _h(_Q0, robot) + Jh = np.asarray( + DQ_Kinematics.point_to_plane_distance_jacobian(Jt, translation(pose0["x"]), _INS_PLANE), + dtype=np.float64, + ) + closed = _row( + _impl.phi_z_constraint_W( + _N1, r, Jr, h, Jh, _PHI_MIN, _PHI_MAX, _H_MIN, _H_MAX + ) + ) + fd = _fd_scalar( + lambda qq: ( + _impl.dot_product_safe(_h(qq, robot), _PHI_MIN, _PHI_MAX, _H_MIN, _H_MAX) + - _impl.phi_z(_N1, rotation(robot.fkm(qq))) + ), + _Q0, + ) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + # h must lie in the linear (non-saturated) region for the FD to match. + assert _H_MIN < h < _H_MAX + + +# --- point-to-line insertion constraint ------------------------------------ + + +def test_insertion_J_D_safe(pose0, robot): + # insertion_J_D_safe == d/dq of tan^2(angle) * d_plane(q)^2. + Jt, x = pose0["Jt"], pose0["x"] + closed = _row(_impl.insertion_J_D_safe(Jt, _INSERTION_ANGLE, _INS_PLANE, x)) + fd = _fd_scalar( + lambda qq: _impl.insertion_D_safe( + _INSERTION_ANGLE, _INS_PLANE, robot.fkm(qq) + ), + _Q0, + ) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + + +def test_insertion_W(pose0, robot): + # insertion_W == d/dq of [D(q) - D_safe(q)] (squared point-to-line + # distance minus the safe radius band). + Jt, x = pose0["Jt"], pose0["x"] + closed = _row(_impl.insertion_W(Jt, _INS_LINE, _INSERTION_ANGLE, _INS_PLANE, x)) + fd = _fd_scalar( + lambda qq: ( + _impl.insertion_D(_INS_LINE, robot.fkm(qq)) + - _impl.insertion_D_safe(_INSERTION_ANGLE, _INS_PLANE, robot.fkm(qq)) + ), + _Q0, + ) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + + +# --- composite needle_jacobian --------------------------------------------- + + +def _plane_of_needle(q: np.ndarray, robot) -> DQ: + """The needle's own plane (unit k_ through the tip), as in ``needle_w``.""" + x = robot.fkm(q) + n = Ad(rotation(x), k_) + t = translation(x) + return n + E_ * dot(t, n) + + +def test_needle_jacobian_radius_plane_rows(pose0, robot): + # With two vessels and no normals (``ns_vessel=None``), ``W`` has exactly + # the 8 base rows -- per vessel v: [+J(radius^2), -J(radius^2), + # +J(plane d), -J(plane d)] -- matching the row order in ``needle_w``. + ps = [_P1, _P2] + W = _impl.needle_jacobian(pose0["Jx"], pose0["x"], ps, None) + assert W.shape == (8, _DOF) + + ref = [] + for p_v in ps: + j_rad = _fd_scalar( + lambda qq, p=p_v: float( + DQ_Geometry.point_to_point_squared_distance( + translation(robot.fkm(qq)), p + ) + ), + _Q0, + ) + j_pl = _fd_scalar( + lambda qq, p=p_v: float( + DQ_Geometry.point_to_plane_distance(p, _plane_of_needle(qq, robot)) + ), + _Q0, + ) + ref += [j_rad, -j_rad, j_pl, -j_pl] + ref = np.vstack(ref) + assert np.max(np.abs(W - ref)) < _FD_TOL + + +def test_needle_jacobian_normal_rows(pose0, robot): + # With one normal (no insertion), the orientation rows are + # [+J(), -J()] for that normal. + ps = [_P1] + ns = [_N2] + W = _impl.needle_jacobian(pose0["Jx"], pose0["x"], ps, ns) + assert W.shape == (6, _DOF) + j_norm = _fd_scalar( + lambda qq: float(dot(_N2, Ad(rotation(robot.fkm(qq)), k_)).q[0]), _Q0 + ) + assert np.max(np.abs(W[4] - j_norm)) < _FD_TOL + assert np.max(np.abs(W[5] - (-j_norm))) < _FD_TOL + + +def test_needle_jacobian_insertion_rows(pose0, robot): + # With insertion enabled, the first normal's row is replaced by the + # point-to-line and angular (phi_z) insertion rows. Identity offset means + # the tip equals the frame, so these rows match ``insertion_W`` / + # ``phi_z_constraint_W`` evaluated at the frame pose. + ps = [_P1] + ns = [_N1] + W = _impl.needle_jacobian( + pose0["Jx"], pose0["x"], ps, ns, insertion_constraints=True + ) + assert W.shape == (6, _DOF) + + # Row 4: d/dq [D(q) - D_safe(q)]. + fd_ins = _fd_scalar( + lambda qq: ( + _impl.insertion_D(_INS_LINE, robot.fkm(qq)) + - _impl.insertion_D_safe(_INSERTION_ANGLE, _INS_PLANE, robot.fkm(qq)) + ), + _Q0, + ) + assert np.max(np.abs(W[4] - fd_ins)) < _FD_TOL + + # Row 5: d/dq [dot_product_safe(h(q)) - phi_z(q)]. + fd_phi = _fd_scalar( + lambda qq: ( + _impl.dot_product_safe(_h(qq, robot), _PHI_MIN, _PHI_MAX, _H_MIN, _H_MAX) + - _impl.phi_z(_N1, rotation(robot.fkm(qq))) + ), + _Q0, + ) + assert np.max(np.abs(W[5] - fd_phi)) < _FD_TOL + + # The radius/plane rows (0..3) still match the (unsigned) base rows. + j_rad = _fd_scalar( + lambda qq: float( + DQ_Geometry.point_to_point_squared_distance( + translation(robot.fkm(qq)), _P1 + ) + ), + _Q0, + ) + j_pl = _fd_scalar( + lambda qq: float( + DQ_Geometry.point_to_plane_distance(_P1, _plane_of_needle(qq, robot)) + ), + _Q0, + ) + assert np.max(np.abs(W[0] - j_rad)) < _FD_TOL + assert np.max(np.abs(W[1] - (-j_rad))) < _FD_TOL + assert np.max(np.abs(W[2] - j_pl)) < _FD_TOL + assert np.max(np.abs(W[3] - (-j_pl))) < _FD_TOL