Add finite-difference tests for the _impl VFI constraint Jacobians

[mmmarinho]

Adds tests/test_impl_finite_difference.py, which validates — by central finite differences — the wrapper Jacobians defined in marinholab/working/needlemanipulation/_impl.py that build the NeedleController VFI constraint rows. This complements the existing tests/test_finite_difference.py, which checks the raw dqrobotics distance/plane Jacobians.

This PR was created by an AI agent (OpenHands) on behalf of the user.

What's validated

Each _impl Jacobian is checked against the scalar (or vector) quantity its docstring/name says it differentiates:

Function	FD reference
rotation_axis_jacobian	d/dq of Ad(r(q), a) (vector-valued, 4×4)
J_phi_z	d/dq of ⟨n, Ad(r(q), k_)⟩
normal_dot_product_jacobian	d/dq of ⟨n, Ad(r(q), a)⟩
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²(angle)·d_plane(q)²
insertion_W	d/dq of [D(q) − D_safe(q)] (squared point-to-line minus the safe band)
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

All errors are at FD precision (≤ ~5e-9).

Note on the insertion geometry

_impl places the depth-dependent phi_z (angular) insertion constraint in the linear, non-saturated region only for h_min < h < h_max, where h is the needle tip's signed distance to the vessel plane. To keep that row differentiable, the tests build the vessel-0 geometry at module scope so the tip (the frame position at the base config) sits exactly at h_mid = (h_min + h_max)/2, plus a small in-plane lateral offset so the tip is not on the vessel line (keeping both the point-to-line and plane gradients non-degenerate). The phi_*/h_* constants mirror the values hard-coded in needle_jacobian/needle_w.

Scope

• Pure-Python dqrobotics only — no compiled _core required (works under the mock fallback too).
• Test-only change: adds one file, tests/test_impl_finite_difference.py (11 tests).
• pytest: 23 passed (12 existing + 11 new). pyright: 0 errors.

Co-authored-by: openhands <openhands@all-hands.dev>

[mmmarinho]

👍