Full type annotations + Doxygen docs; pyright clean

AGENTS.md

@@ -22,17 +22,31 @@ only the abstract `DQ_QuadraticProgrammingSolver` (pybind11, "pure virtual
 function") is available.
 
 `quadprog` is a **declared runtime dependency** of this package
-(`pyproject.toml`). The CI test job must install it; the hand-picked test
-dependency list in `.github/workflows/python-publish.yml` includes it on
-purpose.
+(`pyproject.toml`), and the `build` job in
+`.github/workflows/python-publish.yml` installs it on purpose. If the
+controller raises "pure virtual function" at `DQ_QuadprogSolver()`,
+`quadprog` is missing in the environment.
 
 ## Tests
 
-`tests/conftest.py` mocks `marinholab.working.needlemanipulation._core`, so the
-full suite runs without building the C++ extension. Run from the repo root:
+There is **no repository test suite** — `tests/` was removed together with
+the `simulator_tests` merge (the old `conftest.py` mocked `_core` and is no
+longer present). There is also no dedicated CI `test` job; the workflow only
+runs `build` (matrix wheel builds) and `publish`. To smoke-test a change
+locally, build the package in a venv and exercise the controller API:
 
 ```
-/tmp/venv-test/bin/python -m pytest tests/ -v
+python -c "
+from importlib.resources import files
+from dqrobotics import DQ
+from marinholab.working.needlemanipulation import NeedleController
+from marinholab.working.needlemanipulation.example_load_from_file import get_information_from_file
+r, r1, r2 = get_information_from_file(files('marinholab.working.needlemanipulation').joinpath('left_robot.yaml').read_text())
+c = NeedleController(r, 10.0, 0.01, 0.999, [(r1['position'], r1['radius'], 6)],
+    DQ([1]), [DQ([1,2,3])], 0.003, insertion_constraints=True)
+H, f, W, w = c._get_optimization_parameters([0.0]*9, r.fkm([0.0]*9))
+print(W.shape)
+"
 ```
 
 ## CI build job: do not cache `build/`
@@ -60,3 +74,58 @@ uploading the artifact. Do not drop that step, and do not hand a pinned
 `manylinux_2_X` tag: `--plat auto` derives the tag from the wheel's actual
 symbols (e.g. `manylinux_2_24`), while a pinned glibc-based tag that is
 stricter than the symbols allow makes `repair` fail the build.
+
+## Type checking with pyright (and the `stubs/` package)
+
+The `marinholab` package is checked with **pyright** in `standard` mode
+(`[tool.pyright]` in `pyproject.toml`). It runs clean: `0 errors, 0 warnings`.
+
+The package depends on the third-party **`dqrobotics`** library, which is a
+compiled pybind11 extension that ships **no `py.typed` marker and no `.pyi`
+stubs**. As a result every name it exposes (e.g. `DQ`, `haminus4`,
+`DQ_Kinematics`) is `Unknown`/undefined to a checker, and star imports from
+it fail `reportUndefinedVariable`.
+
+To keep the rest of the codebase fully checkable **without weakening the
+checks globally**, we maintain a small, closed-set stub package under
+`stubs/dqrobotics/`:
+
+```
+stubs/dqrobotics/
+    __init__.pyi            # DQ + math helpers (i_, j_, k_, E_, conj, dot, ...)
+    robot_modeling/__init__.pyi   # DQ_SerialManipulator, DQ_Kinematics
+    utils/__init__.pyi      # DQ_Geometry
+    solvers/__init__.pyi    # DQ_QuadraticProgrammingSolver, DQ_QuadprogSolver
+```
+
+pyright is pointed at it via `stubPath = "stubs"` in `pyproject.toml`, so it
+resolves `dqrobotics` from the stubs rather than the installed (untyped)
+package. The stubs declare **only the symbols this project actually
+imports** — not a full mirror of `dqrobotics`' API. If a new `dqrobotics`
+symbol is needed, add it to the matching stub file.
+
+- **When `dqrobotics` ships its own type information**, delete `stubs/` and
+  the `stubPath` entry — the real types will take over.
+- Run the check from the repo root (with a venv on the `venvPath`/`venv`
+  set in `pyproject.toml`): `pyright`.
+
+## Annotation + Doxygen conventions
+
+All shipped Python is fully annotated and documented:
+
+- **Functions/methods** carry full parameter and return annotations
+  (`np.ndarray`, `DQ`, `Optional[...]`, `Tuple[...]`, ...). Use `Optional`
+  (or `X | None`) where a value may be `None`; never annotate a mutable
+  default with a type that `None` can't satisfy.
+- **Docstrings** follow the Doxygen-style form used across the repo: a short
+  one-line summary, then `Args:`, `Returns:`/`Return:`, and `Raises:`
+  blocks as applicable. Module docstrings open with the copyright header and
+  a one-paragraph description of what the module provides.
+- **C++** sources use Doxygen `@file` / `@brief` / `@param` / `@return` /
+  `@throws` comments on the public API and the non-trivial protected
+  helpers (see `include/M3_SerialManipulatorSimulatorFriendly.h` and
+  `src/M3_SerialManipulatorSimulatorFriendly.cpp`).
+- The C++ `_core` extension's Python-visible surface is documented via the
+  `m.doc()` Sphinx text in `src/core.cpp` and the `_core.pyi` type stub;
+  keep the two consistent when the API changes.
+

include/M3_SerialManipulatorSimulatorFriendly.h

@@ -1,28 +1,42 @@
 #pragma once
 /**
-(C) Copyright 2020-2022
-MIT Lience
-
-Contributors:
-- Murilo M. Marinho (murilomarinho@ieee.org)
-*/
+ * @file M3_SerialManipulatorSimulatorFriendly.h
+ * @brief A serial-manipulator kinematics model with configurable per-joint
+ *        offsets and actuation types, exposed to Python via pybind11.
+ *
+ * (C) Copyright 2020-2022
+ * MIT License
+ *
+ * Contributors:
+ * - Murilo M. Marinho (murilomarinho@ieee.org)
+ */
 
 
 #include <dqrobotics/robot_modeling/DQ_SerialManipulator.h>
 
 namespace DQ_robotics
 {
 
+/**
+ * @brief A serial manipulator whose joints carry explicit pre/post offsets.
+ *
+ * Each joint contributes a dual-quaternion transformation of the form
+ * @c offset_before_ * actuation(q) * offset_after_, which lets the model
+ * represent sensor frames and joint offsets that a plain
+ * @ref DQ_SerialManipulator would not. Supports both revolute (R) and
+ * prismatic (T) joints about any principal axis.
+ */
 class M3_SerialManipulatorSimulatorFriendly: public DQ_SerialManipulator
 {
 public:
+    /** @brief The actuation type and axis of a single joint. */
     enum class ActuationType{
-        RZ,
-        RY,
-        RX,
-        TZ,
-        TY,
-        TX
+        RZ, ///< Revolution about the z-axis.
+        RY, ///< Revolution about the y-axis.
+        RX, ///< Revolution about the x-axis.
+        TZ, ///< Translation along the z-axis.
+        TY, ///< Translation along the y-axis.
+        TX  ///< Translation along the x-axis.
     };
 protected:
     std::vector<DQ> offset_before_;
@@ -35,6 +49,15 @@ class M3_SerialManipulatorSimulatorFriendly: public DQ_SerialManipulator
 
 
     M3_SerialManipulatorSimulatorFriendly()=delete;
+    /**
+     * @brief Construct the manipulator.
+     *
+     * @param offset_before  Per-joint dual-quaternion offset applied before actuation.
+     * @param offset_after   Per-joint dual-quaternion offset applied after actuation.
+     * @param actuation_types Per-joint actuation type and axis.
+     *
+     * @throws std::runtime_error if the three vectors do not have equal size.
+     */
     M3_SerialManipulatorSimulatorFriendly(const std::vector<DQ>& offset_before,
                                           const std::vector<DQ>& offset_after,
                                           const std::vector<ActuationType>& actuation_types);
@@ -43,10 +66,33 @@ class M3_SerialManipulatorSimulatorFriendly: public DQ_SerialManipulator
     using DQ_SerialManipulator::raw_pose_jacobian_derivative;
     using DQ_SerialManipulator::raw_fkm;
 
+    /**
+     * @brief Joint types this model can represent.
+     * @return A vector containing @c DQ_JointType::REVOLUTE.
+     */
     std::vector<DQ_JointType> get_supported_joint_types() const override;
 
+    /**
+     * @brief Raw pose Jacobian of the chain up to a given link.
+     * @param q_vec         Joint configuration vector.
+     * @param to_ith_link   Index of the terminal link.
+     * @return An 8 x (to_ith_link+1) dual-quaternion pose Jacobian.
+     */
     MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override;
+    /**
+     * @brief Time derivative of the raw pose Jacobian.
+     * @param q             Joint configuration vector.
+     * @param q_dot         Joint velocity vector.
+     * @param to_ith_link   Index of the terminal link.
+     * @return An 8 x (to_ith_link+1) Jacobian-derivative matrix.
+     */
     MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override;
+    /**
+     * @brief Raw forward kinematics of the chain up to a given link.
+     * @param q_vec         Joint configuration vector.
+     * @param to_ith_link   Index of the terminal link.
+     * @return The dual-quaternion pose of the terminal link.
+     */
     DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override;
 };
 

marinholab/working/needlemanipulation/_core.pyi

@@ -3,7 +3,7 @@
 from __future__ import annotations
 
 import sys
-from typing import TYPE_CHECKING
+from typing import TYPE_CHECKING, ClassVar
 
 import numpy as np
 
@@ -46,7 +46,10 @@ class M3_SerialManipulatorSimulatorFriendly(DQ_SerialManipulator):
     Inherits from ``DQ_SerialManipulator`` and overrides the kinematic methods.
     """
 
-    ActuationType = ActuationType
+    # ``ActuationType`` is a pybind11 enum registered as a nested attribute of
+    # the class; expose it so ``M3_...ActuationType.RX`` resolves to an
+    # ``ActuationType`` instance.
+    ActuationType: ClassVar[type["ActuationType"]]
 
     def __init__(
         self,

marinholab/working/needlemanipulation/_impl.py

@@ -3,6 +3,19 @@
 (www.murilomarinho.info)
 
 LGPLv3 License
+
+Needle manipulation constraint implementation.
+
+This module implements the Violation Field Indicator (VFI) constraint
+machinery used by :class:`NeedleController`. It computes, for a needle pose
+relative to one or more vessel primitives, the inequality matrix ``W``
+(:func:`needle_jacobian`) and the right-hand-side vector ``w``
+(:func:`needle_w`) that bound the joint velocities in the quadratic program
+solved at each control step.
+
+When ``insertion_constraints`` is enabled an additional angular insertion
+constraint (the needle must approach the vessel within a depth-dependent
+angle band) and a tip point-to-line insertion constraint are appended.
 """
 
 import math
@@ -228,11 +241,44 @@ def needle_jacobian(
     Jx_needle: np.ndarray,
     x_needle: DQ,
     ps_vessel: list[DQ],
-    ns_vessel: list[DQ],
+    ns_vessel: list[DQ] | None,
     insertion_constraints: bool = False,
     needle_offset: DQ = DQ([1.0]),
 ) -> np.ndarray:
-    """Construct the needle constraint matrix."""
+    """Construct the needle inequality constraint matrix ``W``.
+
+    Builds one row of ``W`` (in the ``W @ q_dot <= w`` formulation) per
+    constraint enforced on the needle, where the needle pose is the end
+    effector pose transformed by the (fixed) relative needle pose. The
+    returned matrix has shape ``(n_constraints, DOF)``.
+
+    Per vessel in ``ps_vessel`` a point-to-point radius constraint (inside/
+    outside the needle radius band) and a plane constraint (the needle's own
+    plane vs. the vessel point) are added. When ``ns_vessel`` is provided, an
+    orientation constraint (needle axis vs. the vessel normal) is added for
+    every vessel normal — except, when ``insertion_constraints`` is set, the
+    first normal is instead handled by the depth-dependent angular insertion
+    constraint below.
+
+    Args:
+        Jx_needle: Pose Jacobian of the needle frame, ``8 x DOF``.
+        x_needle: Dual-quaternion pose of the needle frame.
+        ps_vessel: Vessel positions as pure dual quaternions.
+        ns_vessel: Vessel normals as pure dual quaternions, or ``None`` to
+            skip orientation constraints.
+        insertion_constraints: If ``True`` append the insertion point-to-line
+            and angular (phi_z) constraints for the first vessel.
+        needle_offset: Dual quaternion relating the needle tip to the needle
+            frame (defaults to the identity).
+
+    Returns:
+        The stacked constraint Jacobian ``W``, shape ``(n_constraints, DOF)``
+        (``(0, DOF)`` when no constraints are active).
+
+    Raises:
+        ValueError: If ``insertion_constraints`` is ``True`` but
+            ``ps_vessel`` or ``ns_vessel`` is empty.
+    """
     p_needle = translation(x_needle)
     r_needle = rotation(x_needle)
 
@@ -351,7 +397,7 @@ def needle_jacobian(
 def needle_w(
     x_needle: DQ,
     ps_vessel: list[DQ],
-    ns_vessel: list[DQ],
+    ns_vessel: list[DQ] | None,
     needle_radius: float,
     vfi_gain_planes: float,
     vfi_gain_radius: float,
@@ -363,7 +409,38 @@ def needle_w(
     insertion_constraints: bool = False,
     needle_offset: DQ = DQ([1.0]),
 ) -> np.ndarray:
-    """Construct the needle constraint vector."""
+    """Construct the needle inequality constraint right-hand side ``w``.
+
+    Mirrors :func:`needle_jacobian` row-for-row: for each constraint row in
+    ``W`` this returns the corresponding signed margin in ``w``, so that the
+    QP enforces ``W @ q_dot <= w``. All rows are scaled by their VFI gain.
+
+    Args:
+        x_needle: Dual-quaternion pose of the needle frame.
+        ps_vessel: Vessel positions as pure dual quaternions.
+        ns_vessel: Vessel normals as pure dual quaternions, or ``None`` to
+            skip orientation constraints.
+        needle_radius: Physical needle radius (m).
+        vfi_gain_planes: VFI gain for plane-distance constraints.
+        vfi_gain_radius: VFI gain for radius constraints.
+        vfi_gain_angles: VFI gain for orientation (dot-product) constraints.
+        d_safe_planes: Safe plane-distance margin (m).
+        d_safe_radius: Safe radius margin (m).
+        d_safe_angles: Safe orientation margin (rad).
+        verbose: If ``True`` print the per-constraint margins and violations.
+        insertion_constraints: If ``True`` append the insertion point-to-line
+            and angular (phi_z) constraint rows for the first vessel.
+        needle_offset: Dual quaternion relating the needle tip to the needle
+            frame (defaults to the identity).
+
+    Returns:
+        The constraint right-hand-side vector ``w``, shape
+        ``(n_constraints, 1)`` (``(0, 1)`` when no constraints are active).
+
+    Raises:
+        ValueError: If ``insertion_constraints`` is ``True`` but
+            ``ps_vessel`` or ``ns_vessel`` is empty.
+    """
     p_needle = translation(x_needle)
     r_needle = rotation(x_needle)
     w_needle = None

marinholab/working/needlemanipulation/example.py

@@ -1,6 +1,10 @@
 """
 Copyright (C) 2025 Murilo Marques Marinho (www.murilomarinho.info)
 LGPLv3 License
+
+Example: build a simple 3-DOF serial manipulator with
+:class:`M3_SerialManipulatorSimulatorFriendly` and, when
+``dqrobotics_extensions`` and ``matplotlib`` are available, plot it in 3D.
 """
 from dqrobotics import *
 from marinholab.working.needlemanipulation import M3_SerialManipulatorSimulatorFriendly
@@ -11,7 +15,16 @@
 except ImportError:
     dqp = None
 
-def main():
+
+def main() -> None:
+    """Build a 3-DOF robot and (optionally) plot it.
+
+    The three joints have offsets that rotate about the X, Y, Z axes with a
+    0.5 m translation along each axis before actuation.
+
+    When the plotting backend is importable the robot is drawn at the home
+    configuration and the figure blocks until closed.
+    """
     offsets_before = [
         1 + 0.5*E_*i_,
         1 + 0.5*E_*j_,

marinholab/working/needlemanipulation/example_create_needle_controller.py

@@ -1,14 +1,28 @@
 """
 Copyright (C) 2025 Murilo Marques Marinho (www.murilomarinho.info)
 LGPLv3 License
+
+Example: build a :class:`NeedleController` from the bundled
+``left_robot.yaml`` model. This is the minimal construction path used by the
+needle insertion examples; the vessels, normals and needle pose are
+illustrative placeholder values.
 """
 from importlib.resources import files
 from dqrobotics import *
 
 from marinholab.working.needlemanipulation import NeedleController
 from marinholab.working.needlemanipulation.example_load_from_file import get_information_from_file
 
-def main():
+
+def main() -> None:
+    """Load the left robot model and build a needle controller for it.
+
+    Loads the robot and RCM constraint data from the packaged
+    ``left_robot.yaml``, applies joint limits (expressed in radians), and
+    constructs a :class:`NeedleController` with two RCM constraints at
+    joint index 6, an illustrative needle pose, and two vessel
+    point/normal pairs.
+    """
     lrobot, lrcm1, lrcm2 = get_information_from_file(
         files('marinholab.working.needlemanipulation').joinpath('left_robot.yaml').read_text())