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

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]

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
 

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

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