Fix RCM margin, M3 joint limits, and add finite-difference tests

[mmmarinho]

Addresses code-analysis items 1, 2, and 4, and adds a finite-difference test suite.

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

Bug fixes

1. RCM constraint margin dropped (icra2019_controller.py). The safe-radius margin returned by get_rcm_constraint() was discarded when the RCM row was stacked into the full QP: the partial (1,) right-hand side was replaced by a zeroed (1,) vector, so the constraint degraded to J_rcm @ q_dot <= 0 and lost its safe-radius (VFI) bound. The margin is now carried into the full right-hand side (w_c_full), so the QP rhs matches get_rcm_constraint. Verified end-to-end: the stacked RCM rhs now equals the VFI margin (e.g. -1.55, -1.46) and the QP solves.

2. M3 joint limits uninitialized (src/M3_SerialManipulatorSimulatorFriendly.cpp). The base class only resizes lower_q_limit_/upper_q_limit_, so the constructor left them as uninitialized Eigen memory (garbage values, non-deterministic). They are now initialized to a wide default (±10 rad) so get_*_q_limit() never returns garbage and the joint-limit constraints stay feasible until the caller sets real limits. Verified by a clean C++ rebuild: defaults are finite and ordered.

3. example_plot KeyError (example_load_from_file.py). example_plot read the missing "diameter" key; the loader only produces "radius". It now reads "radius".

Also fixed (required for the example to run)

example_load_from_file.main() passed 2-tuples to rcm_constraints (the controller unpacks (position, radius, joint_index)) and never set joint limits, so it crashed / built an infeasible QP. It now applies the documented joint limits and uses the correct 3-tuples (frame index 6, matching example_create_needle_controller).

Tests (new)

Runs with pure-Python dqrobotics; no _core build is required. tests/conftest.py bridges the installed _core/marinholab.solvers into the repo package when running from the source checkout, and falls back to a mock _core when no compiled extension is available.

• tests/test_finite_difference.py — validates, by central finite differences, that each distance/plane task Jacobian differentiates the quantity its name and sign convention imply (squared distance for point/line constraints; signed plain distance for plane constraints), plus the haminus8 tool-frame Jacobian transform and an RCM-margin geometry cross-check.
• tests/test_regressions.py — guards the three fixes above (RCM margin in the QP rhs, M3 default limits finite/ordered, example_plot reading radius).

All 12 tests pass and pyright remains clean (0 errors).

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

[mmmarinho]

👍