diff --git a/marinholab/working/needlemanipulation/example_load_from_file.py b/marinholab/working/needlemanipulation/example_load_from_file.py index cb55ed2..21af194 100644 --- a/marinholab/working/needlemanipulation/example_load_from_file.py +++ b/marinholab/working/needlemanipulation/example_load_from_file.py @@ -152,8 +152,8 @@ def example_plot( ax.set_zlabel('$z$') dqp.plot(robot, q=q) - dqp.plot(rcm1["position"], sphere=True, radius=rcm1["diameter"], color="red", alpha=0.5) - dqp.plot(rcm2["position"], sphere=True, radius=rcm2["diameter"], color="blue", alpha=0.5) + dqp.plot(rcm1["position"], sphere=True, radius=rcm1["radius"], color="red", alpha=0.5) + dqp.plot(rcm2["position"], sphere=True, radius=rcm2["radius"], color="blue", alpha=0.5) plt.show(block=True) @@ -174,14 +174,21 @@ def main() -> None: try: lrobot, lrcm1, lrcm2 = get_information_from_file(files('marinholab.working.needlemanipulation').joinpath('left_robot.yaml').read_text()) + # Apply the robot's joint limits (the same values used by + # example_create_needle_controller). The controller's joint-limit + # constraints are built from these, so they must be set before the + # controller is constructed or the QP is infeasible from the start. + lrobot.set_lower_q_limit([-85, -85, 5, -265, -85, -355, -170, -30, -30]) + lrobot.set_upper_q_limit([85, 85, 120, 0, 85, 355, 170, 30, 30]) + controller = ICRA19TaskSpaceController( kinematics=lrobot, gain=10.0, damping=0.01, alpha=0.999, rcm_constraints=[ - (lrcm1["position"], lrcm1["radius"]), - (lrcm2["position"], lrcm2["radius"])] + (lrcm1["position"], lrcm1["radius"], 6), + (lrcm2["position"], lrcm2["radius"], 6)] ) q_init = [0, 0, 0, 0, 0, 0, 0, 0, 0] diff --git a/marinholab/working/needlemanipulation/icra2019_controller.py b/marinholab/working/needlemanipulation/icra2019_controller.py index 230bc2c..8f928b6 100644 --- a/marinholab/working/needlemanipulation/icra2019_controller.py +++ b/marinholab/working/needlemanipulation/icra2019_controller.py @@ -237,16 +237,22 @@ def _get_optimization_parameters( # Full matrix and vector W_c = np.zeros((1,DOF)) - w_c = np.zeros(1) - # Add the current partial results + w_c_full = np.zeros(1) + # Add the current partial results. The Jacobian row ``W_c_idx`` + # is (1, idx+1) (only the first ``idx+1`` joints move the + # constrained frame); the RCM margin returned by + # ``get_rcm_constraint`` must be carried into the full + # right-hand side, otherwise the constraint degrades to + # ``J_rcm @ q_dot <= 0`` and loses its safe-radius bound. W_c[0, 0:idx+1] = W_c_idx + w_c_full[0] = w_c[0] if W is None: W = W_c - w = w_c + w = w_c_full else: W = np.vstack((W, W_c)) - w = np.hstack((w, w_c)) + w = np.hstack((w, w_c_full)) return H, f, W, w diff --git a/src/M3_SerialManipulatorSimulatorFriendly.cpp b/src/M3_SerialManipulatorSimulatorFriendly.cpp index 2585827..b86aaad 100644 --- a/src/M3_SerialManipulatorSimulatorFriendly.cpp +++ b/src/M3_SerialManipulatorSimulatorFriendly.cpp @@ -28,6 +28,16 @@ M3_SerialManipulatorSimulatorFriendly::M3_SerialManipulatorSimulatorFriendly(con offset_after_.size() != actuation_types_.size() ) throw std::runtime_error("Size issue"); + // The base class only resizes the limit vectors, leaving them + // uninitialized. Initialize them to a wide range so that + // get_lower_q_limit()/get_upper_q_limit() never return garbage and the + // joint-limit constraints stay feasible until the caller sets real + // limits with set_lower_q_limit()/set_upper_q_limit(). + // ``kDefaultJointLimit`` is a large (practically unbounded) value in + // radians, the unit used for joint positions throughout dqrobotics. + static constexpr double kDefaultJointLimit = 10.0; + lower_q_limit_ = VectorXd::Constant(actuation_types_.size(), -kDefaultJointLimit); + upper_q_limit_ = VectorXd::Constant(actuation_types_.size(), kDefaultJointLimit); } DQ M3_SerialManipulatorSimulatorFriendly::_joint_transformation(const double &q, const int &ith) const diff --git a/tests/conftest.py b/tests/conftest.py new file mode 100644 index 0000000..41d5e86 --- /dev/null +++ b/tests/conftest.py @@ -0,0 +1,129 @@ +"""Shared pytest configuration for the needlemanipulation test suite. + +The package ``__init__`` imports the compiled ``_core`` extension (and the +``marinholab.solvers.qpoases`` dependency). When tests run from the repository +root the repository's ``marinholab`` package shadows the installed one, so we: + +1. import ``dqrobotics`` first (the ``_core`` extension subclasses its + pybind11 types, so the base types must be registered); +2. merge the installed ``marinholab`` directory into the top-level package's + search path so sub-packages such as ``marinholab.solvers`` resolve; +3. import the compiled ``_core`` extension (loading the installed ``.so`` + directly when the repository checkout has no build of its own) and register + it as ``marinholab.working.needlemanipulation._core`` before the package + ``__init__`` runs; +4. only as a last resort (no compiled extension available at all) register a + mock ``_core`` so the pure-Python controller / Jacobian logic stays + importable and testable. + +``CORE_AVAILABLE`` records whether the real compiled extension was usable. +""" +from __future__ import annotations + +import glob +import importlib.util +import os +import site +import sys +from typing import Optional +from unittest.mock import MagicMock + + +def _site_dirs() -> list[str]: + """System site-packages plus the user site-packages.""" + dirs = list(site.getsitepackages()) + try: + usr = site.getusersitepackages() + if usr: + dirs.append(usr) + except Exception: + pass + return dirs + + +def _merge_marinholab_site_packages() -> None: + """Expose installed ``marinholab.*`` sub-packages under the repo package.""" + try: + import marinholab + except Exception: + return + try: + pkg_dir = os.path.abspath(os.path.dirname(marinholab.__file__)) + except Exception: + return + for sp in _site_dirs(): + d = os.path.abspath(os.path.join(sp, "marinholab")) + if os.path.isdir(d) and d != pkg_dir and d not in marinholab.__path__: + marinholab.__path__.append(d) + + +def _installed_core_so() -> Optional[str]: + """Path to an installed ``_core`` extension, if any.""" + for sp in _site_dirs(): + base = os.path.abspath( + os.path.join(sp, "marinholab", "working", "needlemanipulation") + ) + for pat in ("_core*.so", "_core*.pyd", "_core*.dll"): + hits = sorted(glob.glob(os.path.join(base, pat))) + if hits: + return hits[0] + return None + + +def _load_real_core() -> bool: + """Load the compiled ``_core`` extension and register it under the repo + submodule name. Returns ``True`` on success.""" + so = _installed_core_so() + if so is None: + return False + try: + import dqrobotics # noqa: F401 register base pybind11 types + except Exception: + return False + _merge_marinholab_site_packages() + try: + import marinholab.working.needlemanipulation._core # noqa: F401 + return True + except Exception: + pass + name = "marinholab.working.needlemanipulation._core" + try: + spec = importlib.util.spec_from_file_location(name, so) + if spec is None or spec.loader is None: + return False + mod = importlib.util.module_from_spec(spec) + sys.modules[name] = mod + spec.loader.exec_module(mod) + return True + except Exception: + return False + + +def _install_core_mock() -> None: + mock_core = MagicMock() + mock_core.M3_SerialManipulatorSimulatorFriendly = MagicMock() + sys.modules["marinholab.working.needlemanipulation._core"] = mock_core + + +def _ensure_core_available() -> bool: + try: + import dqrobotics # noqa: F401 + except Exception: + pass + _merge_marinholab_site_packages() + if _load_real_core(): + return True + _install_core_mock() + return False + + +CORE_AVAILABLE: bool = _ensure_core_available() + + +import pytest # noqa: E402 (imported after sys.modules patching) + + +@pytest.fixture(scope="session") +def core_available() -> bool: + """Whether the compiled ``_core`` extension is importable.""" + return CORE_AVAILABLE diff --git a/tests/test_finite_difference.py b/tests/test_finite_difference.py new file mode 100644 index 0000000..9910bdc --- /dev/null +++ b/tests/test_finite_difference.py @@ -0,0 +1,222 @@ +"""Finite-difference validation of the dual-quaternion task Jacobians. + +These tests exercise the pure-Python ``dqrobotics`` kinematics (no compiled +``_core`` required) and confirm, by central finite differences, that each +distance / plane Jacobian used by the needlemanipulation controllers +differentiates the quantity its name and sign convention imply: + +* point-to-point and line-based distances use the **squared** distance + (matching the RCM / vessel geometry built from ``*_squared_distance``); +* plane-based constraints use the **signed plain** distance. + +A fixed, well-conditioned 4-DOF arm and a handful of off-manifold fixed +primitives are used so every Jacobian row is non-degenerate. +""" +from __future__ import annotations + +import numpy as np +import pytest + +from dqrobotics import DQ, Ad, E_, cross, dot, haminus8, k_, rotation, translation +from dqrobotics.robot_modeling import DQ_Kinematics, DQ_SerialManipulatorDH +from dqrobotics.utils import DQ_Geometry + +# 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.6, -0.4, 1.1, 0.25]) +_DOF = _Q0.size +_FD_TOL = 2e-6 +_EPS = 1e-6 + +# Fixed primitives, placed off the arm's reach so the distances are +# non-degenerate. A point is a pure quaternion; a line is a unit axis plus a +# perpendicular moment; a plane is a unit normal plus a signed offset. +_P_POINT = DQ([1.0, 0.15, -0.2]) +_LINE_AXIS = DQ([0.2, 0.1, 0.0]).normalize() +_LINE_T0 = DQ([0.0, 0.05, 0.06]) +_P_LINE = _LINE_AXIS + E_ * cross(_LINE_T0, _LINE_AXIS) +_P_PLANE_N = DQ([0.3, 0.4, 0.3]).normalize() +_P_PLANE_P = DQ([0.1, -0.05, 0.15]) +_P_PLANE = _P_PLANE_N + E_ * dot(_P_PLANE_P, _P_PLANE_N) + + +def _finite_difference(scalar_fn, q: np.ndarray, eps: float = _EPS) -> np.ndarray: + """Central finite-difference Jacobian of ``scalar_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] = (scalar_fn(qp) - scalar_fn(qm)) / (2.0 * eps) + return jac + + +def _row(jac) -> np.ndarray: + """Flatten a Jacobian returned by ``DQ_Kinematics`` to a 1-D row.""" + return np.asarray(jac, dtype=float).ravel() + + +def _frame_plane(q: np.ndarray, robot, idx=None) -> DQ: + """Plane attached to a moving frame: unit normal + signed offset.""" + xx = robot.fkm(q) if idx is None else robot.fkm(q, idx) + n = Ad(rotation(xx), k_) + t = translation(xx) + return n + E_ * dot(t, n) + + +def _frame_line(q: np.ndarray, robot, idx=None) -> DQ: + """Line attached to a moving frame: the frame's z-axis through its origin.""" + xx = robot.fkm(q) if idx is None else robot.fkm(q, idx) + axis = Ad(rotation(xx), k_) + t = translation(xx) + return axis + E_ * cross(t, axis) + + +@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 base(robot) -> dict: + x = robot.fkm(_Q0) + Jx = robot.pose_jacobian(_Q0) + return { + "x": x, + "Jx": Jx, + "Jt": DQ_Kinematics.translation_jacobian(Jx, x), + } + + +def test_point_to_point_squared_distance_jacobian(base, robot): + x, Jt = base["x"], base["Jt"] + closed = _row( + DQ_Kinematics.point_to_point_distance_jacobian(Jt, translation(x), _P_POINT) + ) + fd = _finite_difference( + lambda qq: DQ_Geometry.point_to_point_squared_distance( + translation(robot.fkm(qq)), _P_POINT + ) + , _Q0) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + + +def test_point_to_line_squared_distance_jacobian(base, robot): + x, Jt = base["x"], base["Jt"] + closed = _row( + DQ_Kinematics.point_to_line_distance_jacobian(Jt, translation(x), _P_LINE) + ) + fd = _finite_difference( + lambda qq: DQ_Geometry.point_to_line_squared_distance( + translation(robot.fkm(qq)), _P_LINE + ) + , _Q0) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + + +def test_line_to_point_squared_distance_jacobian_moving_line(base, robot): + # The RCM configuration: the line is attached to the frame and moves with + # the robot, while the point is fixed. + x, Jx = base["x"], base["Jx"] + Jl = DQ_Kinematics.line_jacobian(Jx, x, k_) + line0 = _frame_line(_Q0, robot) + closed = _row( + DQ_Kinematics.line_to_point_distance_jacobian(Jl, line0, _P_POINT) + ) + fd = _finite_difference( + lambda qq: DQ_Geometry.point_to_line_squared_distance( + _P_POINT, _frame_line(qq, robot) + ) + , _Q0) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + + +def test_point_to_plane_signed_distance_jacobian(base, robot): + x, Jt = base["x"], base["Jt"] + closed = _row( + DQ_Kinematics.point_to_plane_distance_jacobian(Jt, translation(x), _P_PLANE) + ) + fd = _finite_difference( + lambda qq: DQ_Geometry.point_to_plane_distance( + translation(robot.fkm(qq)), _P_PLANE + ) + , _Q0) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + + +def test_plane_to_point_signed_distance_jacobian_moving_plane(base, robot): + # The plane is attached to the frame (moves with the robot); the point is + # fixed. This mirrors the vessel-plane constraint in _impl.py. + x, Jx = base["x"], base["Jx"] + Jpi = DQ_Kinematics.plane_jacobian(Jx, x, k_) + closed = _row( + DQ_Kinematics.plane_to_point_distance_jacobian(Jpi, _P_POINT) + ) + fd = _finite_difference( + lambda qq: DQ_Geometry.point_to_plane_distance(_P_POINT, _frame_plane(qq, robot)) + , _Q0) + assert closed.shape == fd.shape == (_DOF,) + assert np.max(np.abs(closed - fd)) < _FD_TOL + + +def test_haminus8_tool_frame_jacobian_transform(robot): + # With a fixed tool offset A, the pose Jacobian of the offset frame must + # equal haminus8(A) @ J of the base frame (the transform used to offset + # the needle tip). Verified to machine precision. + robot.set_effector(DQ([1.0])) + x_id = robot.fkm(_Q0) + J_id = robot.pose_jacobian(_Q0) + A = DQ([1.0]) + E_ * DQ([0.2, 0.1, 0.05, 0.0]) + robot.set_effector(A) + x_off = robot.fkm(_Q0) + J_off = robot.pose_jacobian(_Q0) + robot.set_effector(DQ([1.0])) + + # Pose consistency: x_off == x_id * A. + assert np.allclose( + np.asarray(x_off.vec4()), np.asarray((x_id * A).vec4()), atol=1e-12 + ) + # Jacobian transform consistency. + err = np.max(np.abs(np.asarray(J_off) - np.asarray(haminus8(A) @ J_id))) + assert err < 1e-9 + + +def test_get_rcm_constraint_matches_geometry(robot): + # Cross-check the controller's RCM margin against the closed-form geometry + # so a future change to get_rcm_constraint cannot silently desync the QP + # right-hand side from the VFI margin. + from marinholab.working.needlemanipulation.icra2019_controller import ( + ICRA19TaskSpaceController, + ) + + idx = 2 + Jx_idx = robot.pose_jacobian(_Q0, idx) + x_idx = robot.fkm(_Q0, idx) + r_safe = 0.5 + eta = 2.0 + _, w = ICRA19TaskSpaceController.get_rcm_constraint( + Jx_idx, x_idx, k_, _P_POINT, r_safe, eta + ) + # get_rcm_constraint spans the line along the primitive (k_) through the + # frame origin, which is exactly _frame_line(..., idx) with the z-axis. + line = _frame_line(_Q0, robot, idx) + Dl_p = DQ_Geometry.point_to_line_squared_distance(_P_POINT, line) + assert np.isclose(w[0], eta * (r_safe ** 2 - Dl_p)) + # With this geometry the margin is non-zero, i.e. a real (non-void) bound. + assert abs(w[0]) > 1e-6 diff --git a/tests/test_regressions.py b/tests/test_regressions.py new file mode 100644 index 0000000..bfcd189 --- /dev/null +++ b/tests/test_regressions.py @@ -0,0 +1,124 @@ +"""Regression tests for the bug fixes in this change. + +These target the three concrete bugs: + +1. The RCM constraint margin (right-hand side) being dropped when the RCM row + is stacked into the full QP — it must equal the margin from + ``get_rcm_constraint``. +2. ``M3_SerialManipulatorSimulatorFriendly`` default joint limits being + uninitialized (garbage) memory — they must be finite and ordered. +3. ``example_plot`` reading a ``"diameter"`` key that the loader never + produces — it must read ``"radius"``. + +The ``_core``-dependent tests are skipped when the compiled extension is not +available (see :mod:`tests.conftest`). +""" +from __future__ import annotations + +import numpy as np +import pytest + +from dqrobotics import DQ, k_ +from marinholab.working.needlemanipulation import ( + M3_SerialManipulatorSimulatorFriendly, + ICRA19TaskSpaceController, +) +from marinholab.working.needlemanipulation.example_load_from_file import ( + get_information_from_file, + example_plot, +) + +# The RCM joint index and the example's documented joint limits (radians). +_RC_IDX = 6 +_LOWER = [-85, -85, 5, -265, -85, -355, -170, -30, -30] +_UPPER = [85, 85, 120, 0, 85, 355, 170, 30, 30] + + +@pytest.fixture(scope="module") +def loaded(): + """(robot, rcm1, rcm2) built from the bundled left_robot.yaml.""" + from importlib.resources import files + + return get_information_from_file( + files("marinholab.working.needlemanipulation") + .joinpath("left_robot.yaml") + .read_text() + ) + + +def test_rcm_margin_is_stacked_into_qp_rhs(loaded): + robot, r1, _ = loaded + robot.set_lower_q_limit(list(_LOWER)) + robot.set_upper_q_limit(list(_UPPER)) + + rcm = [(r1["position"], r1["radius"], _RC_IDX)] + ctrl = ICRA19TaskSpaceController( + robot, gain=10.0, damping=0.01, alpha=0.999, rcm_constraints=rcm + ) + + q = np.zeros(9) + xd = robot.fkm(q) + _, _, W, w = ctrl._get_optimization_parameters(q, xd) + + # The RCM row is the row after the 2*DOF joint-limit rows. + dof = len(q) + rcm_rhs = w[2 * dof] + Jx_idx = robot.pose_jacobian(q, _RC_IDX) + x_idx = robot.fkm(q, _RC_IDX) + _, w_ref = ctrl.get_rcm_constraint( + Jx_idx, x_idx, k_, r1["position"], r1["radius"], ctrl.vfi_gain + ) + + # The stacked right-hand side must carry the VFI margin (item 1). + assert np.isclose(rcm_rhs, w_ref[0]) + assert abs(w_ref[0]) > 1e-9 # a real (non-void) bound, not a dropped zero + + +def test_m3_default_limits_are_finite_and_ordered(loaded): + robot, _, _ = loaded + lo = np.asarray(robot.get_lower_q_limit(), dtype=float) + up = np.asarray(robot.get_upper_q_limit(), dtype=float) + + assert lo.shape == up.shape == (9,) + assert np.all(np.isfinite(lo)) and np.all(np.isfinite(up)), ( + "default joint limits are not finite (uninitialized memory?)" + ) + assert np.all(lo < up), "default joint limits are not ordered" + + +def test_m3_default_limits_via_direct_construction(core_available): + # Also exercise the C++ constructor directly (bypasses the YAML loader). + pytest.importorskip("marinholab.working.needlemanipulation._core") + if not core_available: + pytest.skip("compiled _core extension not available") + + n = 5 + rx = M3_SerialManipulatorSimulatorFriendly.ActuationType.RX + ob = [DQ([1.0, 0.0, 0.0, 0.0]) for _ in range(n)] + oa = [DQ([1.0, 0.0, 0.0, 0.0]) for _ in range(n)] + robot = M3_SerialManipulatorSimulatorFriendly(ob, oa, [rx] * n) + + lo = np.asarray(robot.get_lower_q_limit(), dtype=float) + up = np.asarray(robot.get_upper_q_limit(), dtype=float) + assert lo.shape == up.shape == (n,) + assert np.all(np.isfinite(lo)) and np.all(np.isfinite(up)) + assert np.all(lo < up) + + +def test_example_plot_reads_radius_key(loaded): + robot, r1, r2 = loaded + # The loader only produces "position" and "radius"; example_plot must not + # reference a missing "diameter" key (item 4). + assert set(r1.keys()) == {"position", "radius"} + assert set(r2.keys()) == {"position", "radius"} + assert "diameter" not in r1 and "diameter" not in r2 + # The attribute is reachable on the module (no NameError / ImportError). + assert callable(example_plot) + + +def test_example_plot_source_uses_radius_key(): + import inspect + + src = inspect.getsource(example_plot) + assert "radius" in src + assert "diameter" not in src