diff --git a/AGENTS.md b/AGENTS.md index e26e248..db0d3e2 100644 --- a/AGENTS.md +++ b/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. + diff --git a/include/M3_SerialManipulatorSimulatorFriendly.h b/include/M3_SerialManipulatorSimulatorFriendly.h index ed77de0..225824d 100644 --- a/include/M3_SerialManipulatorSimulatorFriendly.h +++ b/include/M3_SerialManipulatorSimulatorFriendly.h @@ -1,11 +1,15 @@ #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 @@ -13,16 +17,26 @@ MIT Lience 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 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& offset_before, const std::vector& offset_after, const std::vector& 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 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; }; diff --git a/marinholab/working/needlemanipulation/_core.pyi b/marinholab/working/needlemanipulation/_core.pyi index c85d9a9..f2e7d04 100644 --- a/marinholab/working/needlemanipulation/_core.pyi +++ b/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, diff --git a/marinholab/working/needlemanipulation/_impl.py b/marinholab/working/needlemanipulation/_impl.py index 903c553..63026e1 100644 --- a/marinholab/working/needlemanipulation/_impl.py +++ b/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 diff --git a/marinholab/working/needlemanipulation/example.py b/marinholab/working/needlemanipulation/example.py index 24f1ed5..a34abcf 100644 --- a/marinholab/working/needlemanipulation/example.py +++ b/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_, diff --git a/marinholab/working/needlemanipulation/example_create_needle_controller.py b/marinholab/working/needlemanipulation/example_create_needle_controller.py index c56e198..9ad1d9a 100644 --- a/marinholab/working/needlemanipulation/example_create_needle_controller.py +++ b/marinholab/working/needlemanipulation/example_create_needle_controller.py @@ -1,6 +1,11 @@ """ 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 * @@ -8,7 +13,16 @@ 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()) diff --git a/marinholab/working/needlemanipulation/example_load_from_file.py b/marinholab/working/needlemanipulation/example_load_from_file.py index 907c219..a47f4cf 100644 --- a/marinholab/working/needlemanipulation/example_load_from_file.py +++ b/marinholab/working/needlemanipulation/example_load_from_file.py @@ -1,6 +1,11 @@ """ Copyright (C) 2025 Murilo Marques Marinho (www.murilomarinho.info) LGPLv3 License + +Example: load the 9-DOF "left robot" model and RCM constraint spheres from +the bundled ``left_robot.yaml``, then run the +:class:`ICRA19TaskSpaceController` on it for a short trajectory and save an +MP4 animation (when the plotting backend is available). """ from importlib.resources import files import yaml @@ -15,7 +20,9 @@ except ImportError: dqp = None -def _set_plot_labels(): + +def _set_plot_labels() -> None: + """Set x/y/z axis labels on the current 3D axes.""" ax = plt.gca() ax.set( xlabel='x [m]', @@ -23,7 +30,14 @@ def _set_plot_labels(): zlabel='z [m]' ) -def _set_plot_limits(lmin: float = -0.5, lmax: float = 0.5): + +def _set_plot_limits(lmin: float = -0.5, lmax: float = 0.5) -> None: + """Set x/y/z axis limits on the current 3D axes. + + Args: + lmin: Lower limit applied to all three axes (default ``-0.5``). + lmax: Upper limit applied to all three axes (default ``0.5``). + """ ax = plt.gca() ax.set( xlim=[lmin, lmax], @@ -31,13 +45,33 @@ def _set_plot_limits(lmin: float = -0.5, lmax: float = 0.5): zlim=[lmin, lmax] ) -def get_information_from_file(file_contents: str) -> (M3_SerialManipulatorSimulatorFriendly, tuple[DQ, float], tuple[DQ, float]): - """ - The actuation types must be a list of strings. Currently, only 'RX' is accepted. - The offsets must be a list of DQ objects. They will be normalized. - :param file_contents: The file after .read() was applied in a suitable format. - :return: A M3_SerialManipulatorSimulatorFriendly object. +def get_information_from_file( + file_contents: str, +) -> tuple[ + M3_SerialManipulatorSimulatorFriendly, + dict[str, DQ | float], + dict[str, DQ | float], +]: + """Parse the robot YAML definition and build the corresponding objects. + + The YAML file is expected to contain, at minimum, the keys + ``actuation_types``, ``offsets_before``, ``offsets_after``, ``rcm1`` and + ``rcm2``. Each RCM entry is a 2-element list: ``[position_DQ, radius]``. + + Args: + file_contents: The text content of the YAML file (the result of + ``Path.read_text()`` or similar). + + Returns: + A 3-tuple ``(robot, rcm1, rcm2)`` where ``robot`` is the + :class:`M3_SerialManipulatorSimulatorFriendly` model and ``rcm1`` / + ``rcm2`` are dictionaries with keys ``"position"`` (a pure + :class:`dqrobotics.DQ`) and ``"radius"`` (a float). + + Raises: + RuntimeError: If an unsupported actuation type (anything other than + ``"RX"``) appears in the YAML. """ data_loaded = yaml.safe_load(file_contents) @@ -69,7 +103,12 @@ def get_information_from_file(file_contents: str) -> (M3_SerialManipulatorSimula # Animation function -def animate_robot(n, robot, stored_qs, stored_time): +def animate_robot( + n: int, + robot: M3_SerialManipulatorSimulatorFriendly, + stored_qs: list, + stored_time: list, +) -> None: """ Create an animation function compatible with `plt`. Adapted from https://marinholab.github.io/OpenExecutableBooksRobotics//lesson-dq8-optimization-based-robot-control. @@ -88,7 +127,12 @@ def animate_robot(n, robot, stored_qs, stored_time): cylinder_color="c", cylinder_alpha=0.3) -def example_plot(q, robot, rcm1, rcm2): +def example_plot( + q: list, + robot: M3_SerialManipulatorSimulatorFriendly, + rcm1: dict[str, DQ | float], + rcm2: dict[str, DQ | float], +) -> None: """ Plots a 3D representation of a robot's configuration along with two red and blue spherical regions of constraint. @@ -113,7 +157,19 @@ def example_plot(q, robot, rcm1, rcm2): plt.show(block=True) -def main(): +def main() -> None: + """Run the end-to-end example. + + Loads the bundled ``left_robot.yaml`` via + :func:`get_information_from_file`, constructs an + :class:`ICRA19TaskSpaceController` with the two RCM constraints from the + YAML, steps the controller forward for ``time_final`` seconds at + ``sampling_time`` resolution, and — when the plotting backend is + available — records the resulting trajectory as an MP4 animation. + + Any ``KeyboardInterrupt`` is swallowed so the plot window can be closed + gracefully. + """ try: lrobot, lrcm1, lrcm2 = get_information_from_file(files('marinholab.working.needlemanipulation').joinpath('left_robot.yaml').read_text()) diff --git a/marinholab/working/needlemanipulation/icra2019_controller.py b/marinholab/working/needlemanipulation/icra2019_controller.py index e0f8335..99954f3 100644 --- a/marinholab/working/needlemanipulation/icra2019_controller.py +++ b/marinholab/working/needlemanipulation/icra2019_controller.py @@ -1,8 +1,12 @@ """ Copyright (C) 2020-25 Murilo Marques Marinho (www.murilomarinho.info) LGPLv3 License + +Task-space controller with remote-centre-of-motion (RCM) and joint-limit +constraints, implemented as a quadratic program over joint velocities. """ import math +from typing import Optional, Sequence, Tuple from dqrobotics.robot_modeling import DQ_Kinematics, DQ_SerialManipulator from dqrobotics.utils import DQ_Geometry @@ -27,9 +31,9 @@ def __init__(self, gain: float, damping: float, alpha: float, - rcm_constraints: list[tuple[DQ, float, int]], + rcm_constraints: Sequence[Tuple[DQ, float, int]], vfi_gain: float = 2.0, - **kwargs): + **kwargs) -> None: """ Initialize the controller. :param kinematics: A suitable DQ_SerialManipulator object. @@ -38,6 +42,9 @@ def __init__(self, :param alpha: A float between 0 and 1. Soft priority between translation and rotation. :param rcm_constraints: A list of tuples (p, r, ith), where p is the position of the constraint as a pure quaternion r is the radius of the constraint, and ith is the index of the joint this constraint relates to. + :param vfi_gain: Violation Field Indicator gain applied to the RCM constraints. + :param kwargs: Optional keyword arguments. Recognized keys: ``verbose`` + (bool) — print constraint errors at every control step. """ self.qp_solver = DQ_QuadprogSolver() @@ -45,17 +52,18 @@ def __init__(self, self.gain: float = gain self.damping: float = damping self.alpha: float = alpha - self.rcm_constraints: list[tuple[DQ, float, int]] = rcm_constraints + self.rcm_constraints: Sequence[Tuple[DQ, float, int]] = rcm_constraints self.vfi_gain: float = vfi_gain if "verbose" in kwargs: - self.verbose = kwargs["verbose"] + self.verbose: bool = bool(kwargs["verbose"]) else: self.verbose = False - self.last_x: np.array = None - self.last_Jx: np.array = None - self.last_error: np.array = None + # Pose / Jacobian / task error of the last control step. + self.last_x: Optional[DQ] = None + self.last_Jx: Optional[np.ndarray] = None + self.last_error: Optional[np.ndarray] = None def get_last_robot_pose(self) -> DQ: """ @@ -66,20 +74,26 @@ def get_last_robot_pose(self) -> DQ: :return: The last recorded x-axis position of the robot. :rtype: DQ """ + assert self.last_x is not None, "No control step has been computed yet." return self.last_x - def get_last_error(self) -> np.array: + def get_last_error(self) -> Optional[np.ndarray]: + """Return the task error of the last control step, if any. + + :return: The stacked translation/rotation error vector of the last + control step, or ``None`` before the first step. + """ return self.last_error @staticmethod - def get_rcm_constraint(Jx: np.array, + def get_rcm_constraint(Jx: np.ndarray, x: DQ, primitive: DQ, p: DQ, d_safe: float, eta_d: float, - ) -> (np.array, np.array): + ) -> Tuple[np.ndarray, np.ndarray]: """ This static method computes the Remote Centre of Motion (RCM) constraint for the end-effector represented by x and its Jacobian Jx. It calculates the @@ -125,7 +139,28 @@ def get_rcm_constraint(Jx: np.array, return W, w - def _get_optimization_parameters(self, q, xd): + def _get_optimization_parameters( + self, + q: np.ndarray, + xd: DQ, + ) -> Tuple[np.ndarray, np.ndarray, Optional[np.ndarray], Optional[np.ndarray]]: + """Assemble the QP cost (``H``, ``f``) and inequality constraints (``W``, ``w``). + + Computes the soft-priority blend of the translation and rotation + task-space costs plus the damping term, and stacks the joint-limit + and (if any) RCM inequality constraints. Also records the end + effector pose, its pose Jacobian, and the task error for later use. + + Args: + q: Current joint configuration vector. + xd: Desired end-effector pose (a unit dual quaternion). + + Returns: + A 4-tuple ``(H, f, W, w)`` where ``H`` and ``f`` are the QP cost + matrix and linear term, and ``W``/``w`` are the stacked + inequality matrix and right-hand side (``None`` when only the + always-present joint-limit rows exist, i.e. ``W == W_jl``). + """ DOF = len(q) # Get current pose information @@ -172,11 +207,9 @@ def _get_optimization_parameters(self, q, xd): w_jl = np.hstack((-1.0 * (lower_joint_limits - q), 1.0 * (upper_joint_limits - q))) # RCM constraints - W = W_jl - w = w_jl - - if self.verbose: - constraint_counter = 0 + W: Optional[np.ndarray] = W_jl + w: Optional[np.ndarray] = w_jl + constraint_counter = 0 if self.rcm_constraints is not None: for constraint in self.rcm_constraints: @@ -207,13 +240,22 @@ def _get_optimization_parameters(self, q, xd): return H, f, W, w - def compute_setpoint_control_signal(self, q, xd) -> np.array: - """ - Get the control signal for the next step as the result of the constrained optimization. - Joint limits are currently not considered. - :param q: The current joint positions. - :param xd: The desired pose. - :return: The desired joint positions that should be sent to the robot. + def compute_setpoint_control_signal(self, q: np.ndarray, xd: DQ) -> np.ndarray: + """Compute the control signal for the next step. + + Solves the constrained quadratic program built by + :meth:`_get_optimization_parameters` for the joint velocity that + drives the end effector toward the desired pose. + + Args: + q: The current joint positions (vector of size ``DOF``). + xd: The desired end-effector pose (a unit dual quaternion). + + Returns: + The joint-velocity vector ``u`` to be applied for this step. + + Raises: + Exception: If ``xd`` is not a unit dual quaternion. """ DOF = len(q) if not is_unit(xd): @@ -223,7 +265,7 @@ def compute_setpoint_control_signal(self, q, xd) -> np.array: H, f, W, w = self._get_optimization_parameters(q, xd) # Solve the quadratic program - if W is not None: + if W is not None and w is not None: u = self.qp_solver.solve_quadratic_program(H, f, W, np.squeeze(w), None, None) else: W = np.zeros((DOF, DOF)) @@ -233,7 +275,20 @@ def compute_setpoint_control_signal(self, q, xd) -> np.array: return u @staticmethod - def _get_rotation_error(x, xd): + def _get_rotation_error(x: DQ, xd: DQ) -> np.ndarray: + """Return the 4-component rotation error between two poses. + + Uses the dual-quaternion invariant ``conj(r_x) * r_xd``: the error + that is closer to the identity (i.e. whose dual part is smaller in + norm) is selected, avoiding the 180° ambiguity. + + Args: + x: Current end-effector pose. + xd: Desired end-effector pose. + + Returns: + The 4-component rotation error vector. + """ # Calculate error from invariant error_1 = vec4(conj(rotation(x))*rotation(xd) - 1) error_2 = vec4(conj(rotation(x))*rotation(xd) + 1) diff --git a/marinholab/working/needlemanipulation/needle_controller.py b/marinholab/working/needlemanipulation/needle_controller.py index f1f6700..76d9c0a 100644 --- a/marinholab/working/needlemanipulation/needle_controller.py +++ b/marinholab/working/needlemanipulation/needle_controller.py @@ -1,58 +1,128 @@ """ Copyright (C) 2025 Murilo Marques Marinho (www.murilomarinho.info) LGPLv3 License + +Needle manipulation controller. Extends +:class:`ICRA19TaskSpaceController` with VFI constraints that keep the needle +inside the safe volume around one or more vessel primitives and (optionally) +within a depth-dependent angular insertion band. """ -import numpy as np import math -from marinholab.working.needlemanipulation.icra2019_controller import ICRA19TaskSpaceController +import numpy as np + from dqrobotics import * from dqrobotics.robot_modeling import DQ_SerialManipulator -from marinholab.working.needlemanipulation import needle_jacobian, needle_w +from marinholab.working.needlemanipulation._impl import needle_jacobian, needle_w +from marinholab.working.needlemanipulation.icra2019_controller import ICRA19TaskSpaceController + class NeedleController(ICRA19TaskSpaceController): - def __init__(self, - kinematics: DQ_SerialManipulator, - gain: float, - damping: float, - alpha: float, - rcm_constraints: list[tuple[DQ, float, int]], - relative_needle_pose: DQ, - vessel_positions: list[DQ], - needle_radius: float, - vfi_gain: float = 2.0, - insertion_constraints: bool = False, - **kwargs): - super().__init__(kinematics, gain, damping, alpha, rcm_constraints, vfi_gain, **kwargs) + """A task-space controller for needle insertion with vessel VFI constraints. + + Inherits the QP-based task-space control of + :class:`ICRA19TaskSpaceController` and stacks an additional set of VFI + inequality constraints (see :mod:`marinholab.working.needlemanipulation._impl`) + onto the joint-limit and RCM constraints before solving. + """ + + def __init__( + self, + kinematics: DQ_SerialManipulator, + gain: float, + damping: float, + alpha: float, + rcm_constraints: list[tuple[DQ, float, int]], + relative_needle_pose: DQ, + vessel_positions: list[DQ], + needle_radius: float, + vfi_gain: float = 2.0, + insertion_constraints: bool = False, + **kwargs, + ) -> None: + """Initialize the needle controller. + Args: + kinematics: The serial-manipulator model of the robot. + gain: Proportional task-space gain. + damping: Damping factor (scalar or a ``DOF x DOF`` matrix). + alpha: Soft priority between translation (``alpha``) and rotation + (``1 - alpha``); typically close to 1. + rcm_constraints: List of ``(position, radius, joint_index)`` RCM + constraints, as in the parent class. + relative_needle_pose: Fixed dual-quaternion pose of the needle + expressed in the end-effector frame. + vessel_positions: Vessel primitives as pure dual quaternions + (typically points on the vessel wall). + needle_radius: Physical needle radius (m). + vfi_gain: Default VFI gain applied to constraints that do not + override it via the ``vfi_gain_*`` kwargs. + insertion_constraints: If ``True``, enable the tip point-to-line + insertion constraint and the depth-dependent angular + insertion constraint on the first vessel. + kwargs: Optional keyword arguments, passed to the parent class + and additionally used to override per-category VFI gains and + safety margins. Recognized keys: + + * ``verbose`` (bool) — print margins at every control step. + * ``vessel_normals`` (list[DQ]) — vessel normals as pure DQs. + * ``vfi_gain_planes``, ``vfi_gain_radius``, + ``vfi_gain_angles`` (float) — per-category VFI gains. + * ``d_safe_planes``, ``d_safe_radius`` (float) — safe + distance margins (m). + * ``d_safe_angles`` (float) — safe angular margin (rad). + """ + super().__init__( + kinematics, + gain, + damping, + alpha, + rcm_constraints, + vfi_gain, + **kwargs, + ) + + # Optional per-category VFI gains and safety margins; if not + # supplied, :meth:`compute_setpoint_control_signal` falls back to + # ``self.vfi_gain`` and small defaults. if "vfi_gain_planes" in kwargs: - self.vfi_gain_planes = kwargs["vfi_gain_planes"] + self.vfi_gain_planes: float = kwargs["vfi_gain_planes"] if "vfi_gain_radius" in kwargs: - self.vfi_gain_radius = kwargs["vfi_gain_radius"] + self.vfi_gain_radius: float = kwargs["vfi_gain_radius"] if "vfi_gain_angles" in kwargs: - self.vfi_gain_angles = kwargs["vfi_gain_angles"] + self.vfi_gain_angles: float = kwargs["vfi_gain_angles"] if "d_safe_planes" in kwargs: - self.d_safe_planes = kwargs["d_safe_planes"] + self.d_safe_planes: float = kwargs["d_safe_planes"] if "d_safe_radius" in kwargs: - self.d_safe_radius = kwargs["d_safe_radius"] + self.d_safe_radius: float = kwargs["d_safe_radius"] if "vessel_normals" in kwargs: - self.vessel_normals = kwargs["vessel_normals"] + self.vessel_normals: list[DQ] = kwargs["vessel_normals"] if "d_safe_angles" in kwargs: - self.d_safe_angles = kwargs["d_safe_angles"] + self.d_safe_angles: float = kwargs["d_safe_angles"] - self.relative_needle_pose = relative_needle_pose - self.vessel_positions = vessel_positions - self.needle_radius = needle_radius - self.insertion_constraints = insertion_constraints + self.relative_needle_pose: DQ = relative_needle_pose + self.vessel_positions: list[DQ] = vessel_positions + self.needle_radius: float = needle_radius + self.insertion_constraints: bool = insertion_constraints - def compute_setpoint_control_signal(self, q, xd) -> np.array: - """ - Get the control signal for the next step as the result of the constrained optimization. - Joint limits are currently not considered. - :param q: The current joint positions. - :param xd: The desired pose. - :return: The desired joint positions that should be sent to the robot. + def compute_setpoint_control_signal(self, q: np.ndarray, xd: DQ) -> np.ndarray: + """Compute the control signal for the next step with vessel constraints. + + Extends the parent's QP with the VFI constraints derived from + :func:`marinholab.working.needlemanipulation._impl.needle_jacobian` + and :func:`marinholab.working.needlemanipulation._impl.needle_w` + and solves the augmented QP. + + Args: + q: Current joint positions. + xd: Desired end-effector pose (a unit dual quaternion). + + Returns: + The joint-velocity vector to be applied for this step. + + Raises: + Exception: If ``xd`` is not a unit dual quaternion. """ DOF = len(q) if not is_unit(xd): @@ -64,6 +134,11 @@ def compute_setpoint_control_signal(self, q, xd) -> np.array: # The relative transformation of the needle is time-constant x = self.last_x Jx = self.last_Jx + if x is None or Jx is None: + raise RuntimeError( + "Internal error: last_x / last_Jx not set. " + "Did you call _get_optimization_parameters first?" + ) Jx_needle = haminus8(self.relative_needle_pose) @ Jx x_needle = x * self.relative_needle_pose @@ -72,25 +147,25 @@ def compute_setpoint_control_signal(self, q, xd) -> np.array: Jx_needle, x_needle, self.vessel_positions, - self.vessel_normals if hasattr(self,"vessel_normals") else None, + self.vessel_normals if hasattr(self, "vessel_normals") else None, self.insertion_constraints, - needle_offset=conj(self.relative_needle_pose) + needle_offset=conj(self.relative_needle_pose), ) # VFI w w_needle = needle_w( x_needle=x_needle, ps_vessel=self.vessel_positions, - ns_vessel=self.vessel_normals if hasattr(self,"vessel_normals") else None, + ns_vessel=self.vessel_normals if hasattr(self, "vessel_normals") else None, needle_radius=self.needle_radius, - vfi_gain_planes=self.vfi_gain_planes if hasattr(self,"vfi_gain_planes") else self.vfi_gain, - vfi_gain_radius=self.vfi_gain_radius if hasattr(self,"vfi_gain_radius") else self.vfi_gain, + vfi_gain_planes=self.vfi_gain_planes if hasattr(self, "vfi_gain_planes") else self.vfi_gain, + vfi_gain_radius=self.vfi_gain_radius if hasattr(self, "vfi_gain_radius") else self.vfi_gain, vfi_gain_angles=self.vfi_gain_angles if hasattr(self, "vfi_gain_angles") else self.vfi_gain, - d_safe_planes=self.d_safe_planes if hasattr(self,"d_safe_planes") else 0.0005, - d_safe_radius=self.d_safe_radius if hasattr(self,"d_safe_radius") else 0.0005, - d_safe_angles=self.d_safe_angles if hasattr(self,"d_safe_angles") else math.pi/4, + d_safe_planes=self.d_safe_planes if hasattr(self, "d_safe_planes") else 0.0005, + d_safe_radius=self.d_safe_radius if hasattr(self, "d_safe_radius") else 0.0005, + d_safe_angles=self.d_safe_angles if hasattr(self, "d_safe_angles") else math.pi / 4, verbose=self.verbose, insertion_constraints=self.insertion_constraints, - needle_offset=conj(self.relative_needle_pose) + needle_offset=conj(self.relative_needle_pose), ).reshape((W_needle.shape[0],)) if W is not None and w is not None: @@ -107,4 +182,4 @@ def compute_setpoint_control_signal(self, q, xd) -> np.array: assert np.squeeze(w).dtype == np.float64 u = self.qp_solver.solve_quadratic_program(H, f, W, np.squeeze(w), None, None) - return u \ No newline at end of file + return u diff --git a/pyproject.toml b/pyproject.toml index da0d934..cc036a6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -52,5 +52,10 @@ include = ["marinholab"] exclude = [ "**/example*.py", ] +# `dqrobotics` is an untyped compiled package (no `py.typed`, no shipped +# stubs). `stubs/dqrobotics/` declares the closed set of symbols this +# project imports so pyright can fully check the codebase. If `dqrobotics` +# ever ships type information, delete `stubs/` and this `stubPath` entry. +stubPath = "stubs" venvPath = "." venv = "" \ No newline at end of file diff --git a/saul/insertion_1.py b/saul/insertion_1.py index 21d0491..4e7e834 100644 --- a/saul/insertion_1.py +++ b/saul/insertion_1.py @@ -1,176 +1,212 @@ -import math -import os - - -from importlib.resources import files - -import dqrobotics as dq -from dqrobotics.utils.DQ_Math import deg2rad -import numpy as np -from dqrobotics import rotation - -import PedriatricSimulator -import time -from marinholab.working.needlemanipulation.example_load_from_file import get_information_from_file -from marinholab.working.needlemanipulation.icra2019_controller import ICRA19TaskSpaceController -from marinholab.working.needlemanipulation import NeedleController - -from marinholab.working.needlemanipulation import M3_SerialManipulatorSimulatorFriendly - - -rcm1_joint_index = 7 -rcm2_joint_index = 6 - -sim = PedriatricSimulator.PediatricSimulator() -sim.connect("127.0.0.1") -#sim.restart() -sim.clear_frames() -time.sleep(1) - -#project_path = os.getcwd() -#sim.load_simulation_state(project_path + "/before_insertion.simstate") -#time.sleep(5) - - -print("needle radius {}".format(sim.get_needle_radius())) - - -def make_robot(base_frame, transforms): - n = len(transforms) - offsets_before = [base_frame] - offsets_after = transforms - actuation_types = [] - - for j in range(n): - if j > 0: - offsets_before.append(1 + 0 * dq.E_) - actuation_types.append(M3_SerialManipulatorSimulatorFriendly.ActuationType.RX) - - return M3_SerialManipulatorSimulatorFriendly( - offsets_before, - offsets_after, - actuation_types - ) - -dofs = sim.get_robot_dofs() - -lrobot = make_robot(sim.get_left_robot_base(), [sim.get_left_robot_model_ith(k) for k in range(dofs)]) -rrobot = make_robot(sim.get_right_robot_base(), [sim.get_right_robot_model_ith(k) for k in range(dofs)]) - -lower_q_limit = deg2rad([-85, -85, 5, -265, -85, -355, -170, -30, -30]) -upper_q_limit = deg2rad([85, 85, 120, 0, 85, 355, 170, 30, 30]) - -lrobot.set_lower_q_limit(lower_q_limit) -lrobot.set_upper_q_limit(upper_q_limit) -rrobot.set_lower_q_limit(lower_q_limit) -rrobot.set_upper_q_limit(upper_q_limit) - -lrcm1 = {"position": sim.get_left_center_sphere()[0], "radius": sim.get_left_center_sphere()[1]} -lrcm2 = {"position": sim.get_left_trocar_sphere()[0], "radius": sim.get_left_trocar_sphere()[1]} -rrcm1 = {"position": sim.get_right_center_sphere()[0], "radius": sim.get_right_center_sphere()[1]} -rrcm2 = {"position": sim.get_right_trocar_sphere()[0], "radius": sim.get_right_trocar_sphere()[1]} - -translate = 1 + 0.5 * dq.E_ * dq.j_ * 0.0005 -angle = -math.pi / 2.0 -rotate1 = math.cos(angle / 2.0) + math.sin(angle / 2.0) * (dq.i_ * 1.0 + dq.j_ * 0.0 + dq.k_ * 0.0) -angle = math.pi / 2.0 -rotate2 = math.cos(angle / 2.0) + math.sin(angle / 2.0) * (dq.i_ * 0.0 + dq.j_ * 0.0 + dq.k_ * 1.0) - -p1 = translate * sim.get_right_tube_target_point() * rotate1 * rotate2 -sim.set_frame("p1", p1) - -needle_pose = sim.get_control_needle_pose() -q = sim.get_right_robot_joints() -ee = rrobot.fkm(q) -relative_needle_pose = dq.conj(ee) * needle_pose -radius = sim.get_needle_radius() - - -needle_tip_pose = sim.get_needle_frame_at(0.0) -relative_needle_tip_pose = dq.conj(ee) * needle_tip_pose - - -rrobot.set_effector(relative_needle_tip_pose) - -v = dq.j_ -n = dq.Ad(dq.rotation(needle_pose), dq.k_) -n1 = dq.Ad(dq.rotation(p1), dq.k_) - -needle_controller = NeedleController( - kinematics=rrobot, - gain=200.0, - damping=np.diag([1,1,1,1,1,1,0,0,0]), - alpha=0.9999, - rcm_constraints=[ - (rrcm1["position"], rrcm1["radius"], rcm1_joint_index), - (rrcm2["position"], rrcm2["radius"], rcm2_joint_index)], - relative_needle_pose=relative_needle_pose, - #vessel_positions=[dq.translation(p1), dq.translation(p2)], - #vessel_normals=[n1, n2], - vessel_positions=[dq.translation(p1)], - vessel_normals=[v], - needle_radius=radius, - d_safe_angles=np.pi / 8.0, - vfi_gain=1.0, - verbose=True, - insertion_constraints=True - ) - -q = sim.get_right_robot_joints() -for i in range(200): - print(i) - sim.set_frame("needle", sim.get_control_needle_pose()) - - x = rrobot.fkm(q) - sim.set_frame("x", x) - - # controlled target pose - dx = 1 + 0.5 * dq.E_ * -0.0001 * dq.j_ # Move downwards - xdc = dx * x - sim.set_frame("xd", xdc) - - sampling_time = 0.008 - for step in range(10): - # Solve the quadratic program - u = needle_controller.compute_setpoint_control_signal(q, xdc) - - # Update the current joint positions - q = q + u * sampling_time - - sim.set_right_robot_joints(q) - - time.sleep(1.0 / 60.0) - - - - - - - - -sim.clear_frames() - -needle_frame = sim.get_control_needle_pose() -x = sim.get_right_robot_effector() -x_wrt_needle_frame = dq.conj(needle_frame) * x - -# insert needle -for i in range(300): - - angle = 0.001 * i * math.pi / 2.0 - rotate = math.cos(angle / 2.0) + math.sin(angle / 2.0) * (dq.i_ * 0.0 + dq.j_ * 0.0 + dq.k_ * 1.0) - - translate = 1 + 0.5 * dq.E_ * (dq.i_ * 0.0 + dq.j_ * -0.00001 * i + dq.k_ * 0.0) - - xd = translate * needle_frame * rotate * x_wrt_needle_frame - - sim.set_frame("xd", xd) - sim.set_frame("x", sim.get_right_robot_effector()) - - sim.set_right_robot_target_pose(xd) - - time.sleep(1/60) - - - -sim.disconnect() +""" +Saul's pediatric insertion scenario, driven live against a running +PedriatricSimulator over TCP (``127.0.0.1``). + +The script: + +1. Builds left and right :class:`M3_SerialManipulatorSimulatorFriendly` + models from the simulator's joint transforms. +2. Configures the joint limits (rad) and RCM constraint spheres for both. +3. Constructs a :class:`NeedleController` for the right robot with the + insertion constraints enabled, using a vessel point and normal derived + from the simulator. +4. Runs the closed-loop for a fixed number of outer loops (200) with 10 + inner QP steps each, streaming the needle pose to the simulator. +5. Finally performs a 300-step pure kinematic insertion sweep and + disconnects. + +Requires ``PedriatricSimulator`` on ``PYTHONPATH`` and a running simulator +instance. +""" +import math +import os + + +from importlib.resources import files + +import dqrobotics as dq +from dqrobotics.utils.DQ_Math import deg2rad +import numpy as np +from dqrobotics import rotation + +import PedriatricSimulator +import time +from marinholab.working.needlemanipulation.example_load_from_file import get_information_from_file +from marinholab.working.needlemanipulation.icra2019_controller import ICRA19TaskSpaceController +from marinholab.working.needlemanipulation import NeedleController + +from marinholab.working.needlemanipulation import M3_SerialManipulatorSimulatorFriendly + + +rcm1_joint_index = 7 +rcm2_joint_index = 6 + +sim = PedriatricSimulator.PediatricSimulator() +sim.connect("127.0.0.1") +#sim.restart() +sim.clear_frames() +time.sleep(1) + +#project_path = os.getcwd() +#sim.load_simulation_state(project_path + "/before_insertion.simstate") +#time.sleep(5) + + +print("needle radius {}".format(sim.get_needle_radius())) + + +def make_robot(base_frame, transforms): + """Build a :class:`M3_SerialManipulatorSimulatorFriendly` model. + + The first joint is anchored to ``base_frame``; all subsequent joints + are anchored to the identity and all joints are treated as + :attr:`ActuationType.RX`. + + Args: + base_frame: Base-frame dual quaternion of the robot. + transforms: Sequence of per-joint dual-quaternion transforms as + returned by the simulator (``get_left_robot_model_ith`` / + ``get_right_robot_model_ith``). + + Returns: + A :class:`M3_SerialManipulatorSimulatorFriendly` instance with + ``len(transforms)`` DOF. + """ + n = len(transforms) + offsets_before = [base_frame] + offsets_after = transforms + actuation_types = [] + + for j in range(n): + if j > 0: + offsets_before.append(1 + 0 * dq.E_) + actuation_types.append(M3_SerialManipulatorSimulatorFriendly.ActuationType.RX) + + return M3_SerialManipulatorSimulatorFriendly( + offsets_before, + offsets_after, + actuation_types + ) + +dofs = sim.get_robot_dofs() + +lrobot = make_robot(sim.get_left_robot_base(), [sim.get_left_robot_model_ith(k) for k in range(dofs)]) +rrobot = make_robot(sim.get_right_robot_base(), [sim.get_right_robot_model_ith(k) for k in range(dofs)]) + +lower_q_limit = deg2rad([-85, -85, 5, -265, -85, -355, -170, -30, -30]) +upper_q_limit = deg2rad([85, 85, 120, 0, 85, 355, 170, 30, 30]) + +lrobot.set_lower_q_limit(lower_q_limit) +lrobot.set_upper_q_limit(upper_q_limit) +rrobot.set_lower_q_limit(lower_q_limit) +rrobot.set_upper_q_limit(upper_q_limit) + +lrcm1 = {"position": sim.get_left_center_sphere()[0], "radius": sim.get_left_center_sphere()[1]} +lrcm2 = {"position": sim.get_left_trocar_sphere()[0], "radius": sim.get_left_trocar_sphere()[1]} +rrcm1 = {"position": sim.get_right_center_sphere()[0], "radius": sim.get_right_center_sphere()[1]} +rrcm2 = {"position": sim.get_right_trocar_sphere()[0], "radius": sim.get_right_trocar_sphere()[1]} + +translate = 1 + 0.5 * dq.E_ * dq.j_ * 0.0005 +angle = -math.pi / 2.0 +rotate1 = math.cos(angle / 2.0) + math.sin(angle / 2.0) * (dq.i_ * 1.0 + dq.j_ * 0.0 + dq.k_ * 0.0) +angle = math.pi / 2.0 +rotate2 = math.cos(angle / 2.0) + math.sin(angle / 2.0) * (dq.i_ * 0.0 + dq.j_ * 0.0 + dq.k_ * 1.0) + +p1 = translate * sim.get_right_tube_target_point() * rotate1 * rotate2 +sim.set_frame("p1", p1) + +needle_pose = sim.get_control_needle_pose() +q = sim.get_right_robot_joints() +ee = rrobot.fkm(q) +relative_needle_pose = dq.conj(ee) * needle_pose +radius = sim.get_needle_radius() + + +needle_tip_pose = sim.get_needle_frame_at(0.0) +relative_needle_tip_pose = dq.conj(ee) * needle_tip_pose + + +rrobot.set_effector(relative_needle_tip_pose) + +v = dq.j_ +n = dq.Ad(dq.rotation(needle_pose), dq.k_) +n1 = dq.Ad(dq.rotation(p1), dq.k_) + +needle_controller = NeedleController( + kinematics=rrobot, + gain=200.0, + damping=np.diag([1,1,1,1,1,1,0,0,0]), + alpha=0.9999, + rcm_constraints=[ + (rrcm1["position"], rrcm1["radius"], rcm1_joint_index), + (rrcm2["position"], rrcm2["radius"], rcm2_joint_index)], + relative_needle_pose=relative_needle_pose, + #vessel_positions=[dq.translation(p1), dq.translation(p2)], + #vessel_normals=[n1, n2], + vessel_positions=[dq.translation(p1)], + vessel_normals=[v], + needle_radius=radius, + d_safe_angles=np.pi / 8.0, + vfi_gain=1.0, + verbose=True, + insertion_constraints=True + ) + +q = sim.get_right_robot_joints() +for i in range(200): + print(i) + sim.set_frame("needle", sim.get_control_needle_pose()) + + x = rrobot.fkm(q) + sim.set_frame("x", x) + + # controlled target pose + dx = 1 + 0.5 * dq.E_ * -0.0001 * dq.j_ # Move downwards + xdc = dx * x + sim.set_frame("xd", xdc) + + sampling_time = 0.008 + for step in range(10): + # Solve the quadratic program + u = needle_controller.compute_setpoint_control_signal(q, xdc) + + # Update the current joint positions + q = q + u * sampling_time + + sim.set_right_robot_joints(q) + + time.sleep(1.0 / 60.0) + + + + + + + + +sim.clear_frames() + +needle_frame = sim.get_control_needle_pose() +x = sim.get_right_robot_effector() +x_wrt_needle_frame = dq.conj(needle_frame) * x + +# insert needle +for i in range(300): + + angle = 0.001 * i * math.pi / 2.0 + rotate = math.cos(angle / 2.0) + math.sin(angle / 2.0) * (dq.i_ * 0.0 + dq.j_ * 0.0 + dq.k_ * 1.0) + + translate = 1 + 0.5 * dq.E_ * (dq.i_ * 0.0 + dq.j_ * -0.00001 * i + dq.k_ * 0.0) + + xd = translate * needle_frame * rotate * x_wrt_needle_frame + + sim.set_frame("xd", xd) + sim.set_frame("x", sim.get_right_robot_effector()) + + sim.set_right_robot_target_pose(xd) + + time.sleep(1/60) + + + +sim.disconnect() diff --git a/src/M3_SerialManipulatorSimulatorFriendly.cpp b/src/M3_SerialManipulatorSimulatorFriendly.cpp index 757ac1f..2585827 100644 --- a/src/M3_SerialManipulatorSimulatorFriendly.cpp +++ b/src/M3_SerialManipulatorSimulatorFriendly.cpp @@ -1,6 +1,15 @@ /** -(C) Copyright 2025 Murilo Marinho (murilomarinho@ieee.org) -*/ + * @file M3_SerialManipulatorSimulatorFriendly.cpp + * @brief Implementation of M3_SerialManipulatorSimulatorFriendly. + * + * (C) Copyright 2025 Murilo Marinho (murilomarinho@ieee.org) + * + * The implementation follows the standard dual-quaternion serial-manipulator + * construction: each joint contributes + * @c offset_before_(i) * actuation(q_i) * offset_after_(i), and the pose + * Jacobian columns are built from the joint axis transformed by the + * intermediate poses (see @ref DQ_SerialManipulator in dqrobotics-cpp). + */ #include @@ -23,6 +32,10 @@ M3_SerialManipulatorSimulatorFriendly::M3_SerialManipulatorSimulatorFriendly(con DQ M3_SerialManipulatorSimulatorFriendly::_joint_transformation(const double &q, const int &ith) const { + // Returns the dual-quaternion joint transformation + // offset_before_(ith) * actuation(q) * offset_after_(ith) + // where actuation(q) is the dual quaternion corresponding to the + // joint's actuation type at value q. const auto& before = offset_before_.at(ith); const auto& after = offset_after_.at(ith); @@ -54,6 +67,17 @@ DQ M3_SerialManipulatorSimulatorFriendly::_joint_transformation(const double &q, } +/** + * @brief Returns the spatial axis (as a dual quaternion) of the joint. + * + * For a revolute joint this is the unit quaternion of the rotation axis; + * for a prismatic joint it is the dual quaternion of the translation + * direction (i.e. @c E_*axis). + * + * @param ith Joint index. + * @return The dual quaternion representing the joint axis. + * @throws std::runtime_error if the joint's actuation type is invalid. + */ DQ M3_SerialManipulatorSimulatorFriendly::_get_w(const int &ith) const { switch(actuation_types_.at(ith)) diff --git a/stubs/dqrobotics/__init__.pyi b/stubs/dqrobotics/__init__.pyi new file mode 100644 index 0000000..2e98fa5 --- /dev/null +++ b/stubs/dqrobotics/__init__.pyi @@ -0,0 +1,167 @@ +""" +Type stubs for the third-party `dqrobotics` package. + +`dqrobotics` is a compiled (pybind11) library that ships **no** `py.typed` +marker and no `.pyi` stubs, so every name it exposes is opaque (`Unknown`) +to type checkers and star imports from it fail `reportUndefinedVariable`. + +This stub declares the closed set of symbols that +`marinholab.working.needlemanipulation` actually imports, so that the rest of +the codebase can be fully annotated and checked by pyright without globally +weakening the checks. It is placed under `stubs/dqrobotics/` and picked up +via pyright's `stubPath` (see `pyproject.toml`). + +If `dqrobotics` ever ships real type information, delete this directory and +drop the `stubPath` setting. +""" + +from __future__ import annotations + +from typing import Any, overload + +import numpy as np +from numpy.typing import ArrayLike, NDArray + +__all__ = [ + "DQ", + "k_", + "i_", + "j_", + "E_", + "conj", + "dot", + "cross", + "Ad", + "Adsharp", + "vec4", + "vec6", + "vec8", + "vec3", + "haminus4", + "hamiplus4", + "haminus8", + "hamiplus8", + "C4", + "C8", + "is_unit", + "translation", + "rotation", + "vec", +] + + +class DQ: + """A dual quaternion (8 real components: 4 quaternion + 4 dual part). + + The raw 8-component representation is exposed via :attr:`q`. All the + arithmetic helpers used by the package (``conj``, ``dot``, ``Ad``, ...) + operate on this type. + """ + + @overload + def __init__(self) -> None: ... + @overload + def __init__(self, q: DQ) -> None: ... + @overload + def __init__(self, other: "ndarray[Any, Any]") -> None: ... + @overload + def __init__( + self, + q0: float, + q1: float, + q2: float, + q3: float, + e0: float, + e1: float, + e2: float, + e3: float, + ) -> None: ... + + q: NDArray[np.float64] + + def normalize(self) -> "DQ": ... + def conj(self) -> "DQ": ... + def inv(self) -> "DQ": ... + def norm(self) -> float: ... + def to_string(self) -> str: ... + + # Component accessors. + def Q4(self) -> NDArray[np.float64]: ... + def Q8(self) -> NDArray[np.float64]: ... + def vec4(self) -> NDArray[np.float64]: ... + def vec8(self) -> NDArray[np.float64]: ... + def vec3(self) -> NDArray[np.float64]: ... + def vec6(self) -> NDArray[np.float64]: ... + + def translation(self) -> "DQ": ... + def rotation(self) -> "DQ": ... + def rotation_angle(self) -> float: ... + def rotation_axis(self) -> "DQ": ... + + def Ad(self, primitive: "DQ") -> "DQ": ... + def Adsharp(self, primitive: "DQ") -> "DQ": ... + + def generalized_jacobian(self, q: NDArray[np.float64]) -> NDArray[np.float64]: ... + + def exp(self) -> "DQ": ... + def log(self) -> "DQ": ... + def pow(self, exponent: "DQ") -> "DQ": ... + + # Operators. ``DQ`` participates in arithmetic with other ``DQ`` values and + # with real scalars; the reverse operators cover the ``scalar op DQ`` form + # (e.g. ``1 + 0 * E_``). Operands are typed ``object`` because the exact + # scalar/duaternion mix the C++ core accepts is not part of its public API. + def __mul__(self, other: object) -> "DQ": ... + def __rmul__(self, other: object) -> "DQ": ... + def __add__(self, other: object) -> "DQ": ... + def __radd__(self, other: object) -> "DQ": ... + def __sub__(self, other: object) -> "DQ": ... + def __rsub__(self, other: object) -> "DQ": ... + def __neg__(self) -> "DQ": ... + + @overload + def __eq__(self, other: "DQ") -> bool: ... + @overload + def __eq__(self, other: object) -> bool: ... + + def __repr__(self) -> str: ... + + +# Dual-quaternion unit basis and the dual part of a quaternion. +i_: DQ +j_: DQ +k_: DQ +E_: DQ + +# Convenience functions that mirror the ``DQ`` methods. +def conj(q: DQ) -> DQ: ... +def dot(a: DQ, b: DQ) -> DQ: ... +def cross(a: DQ, b: DQ) -> DQ: ... +def Ad(q: DQ, p: DQ) -> DQ: ... +def Adsharp(q: DQ, p: DQ) -> DQ: ... + + +def vec4(q: DQ) -> NDArray[np.float64]: ... +def vec6(q: DQ) -> NDArray[np.float64]: ... +def vec8(q: DQ) -> NDArray[np.float64]: ... +def vec3(q: DQ) -> NDArray[np.float64]: ... + + +# The (matrix) half of a dual quaternion: the 4x4 and 8x8 rotation matrices +# built from the pure / full dual quaternion. +def haminus4(q: DQ) -> NDArray[np.float64]: ... +def hamiplus4(q: DQ) -> NDArray[np.float64]: ... +def haminus8(q: DQ) -> NDArray[np.float64]: ... +def hamiplus8(q: DQ) -> NDArray[np.float64]: ... + + +# The 4x4 and 8x8 constant coupling matrices of the dual-quaternion +# representation. +def C4() -> NDArray[np.float64]: ... +def C8() -> NDArray[np.float64]: ... + + +def is_unit(q: DQ) -> bool: ... +def translation(q: DQ) -> DQ: ... +def rotation(q: DQ) -> DQ: ... +def vec(q: DQ) -> NDArray[np.float64]: ... diff --git a/stubs/dqrobotics/robot_modeling/__init__.pyi b/stubs/dqrobotics/robot_modeling/__init__.pyi new file mode 100644 index 0000000..08e5bda --- /dev/null +++ b/stubs/dqrobotics/robot_modeling/__init__.pyi @@ -0,0 +1,104 @@ +"""Stubs for `dqrobotics.robot_modeling` (see `stubs/dqrobotics/__init__.pyi`).""" + +from __future__ import annotations + +from typing import Any, Sequence + +import numpy as np +from numpy.typing import NDArray + +from dqrobotics import DQ + +__all__ = [ + "DQ_SerialManipulator", + "DQ_Kinematics", +] + + +class DQ_SerialManipulator: + """Base class for serial-manipulator kinematics models in dual quaternions. + + Subclasses (for example the C++-backed + ``M3_SerialManipulatorSimulatorFriendly``) provide forward kinematics + and Jacobians for a chain of actuated joints. + """ + + def fkm(self, q: NDArray[np.float64], idx: int = 0) -> DQ: ... + def fkm_derivative( + self, q: NDArray[np.float64], q_dot: NDArray[np.float64], idx: int = 0 + ) -> DQ: ... + + def pose_jacobian( + self, q: NDArray[np.float64], idx: int = 0 + ) -> NDArray[np.float64]: ... + def pose_jacobian_derivative( + self, + q: NDArray[np.float64], + q_dot: NDArray[np.float64], + idx: int = 0, + ) -> NDArray[np.float64]: ... + + def set_lower_q_limit(self, lower_q_limit: NDArray[np.float64]) -> None: ... + def get_lower_q_limit(self) -> NDArray[np.float64]: ... + def set_upper_q_limit(self, upper_q_limit: NDArray[np.float64]) -> None: ... + def get_upper_q_limit(self) -> NDArray[np.float64]: ... + + def set_effector(self, x_effector: DQ) -> None: ... + def get_effector(self) -> DQ: ... + def set_effector_target(self, xd_effector: DQ) -> None: ... + def get_effector_target(self) -> DQ: ... + + def get_name(self) -> str: ... + + +class DQ_Kinematics: + """A catalogue of closed-form Jacobians for common dual-quaternion tasks. + + All methods are stateless (static); each takes the pose Jacobian of the + end-effector and returns the task Jacobian as a NumPy matrix. + """ + + @staticmethod + def line_jacobian( + Jx: NDArray[np.float64], x: DQ, primitive: DQ + ) -> NDArray[np.float64]: ... + + @staticmethod + def line_to_point_distance_jacobian( + Jl: NDArray[np.float64], l: DQ, p: DQ + ) -> NDArray[np.float64]: ... + + @staticmethod + def rotation_jacobian( + Jx: NDArray[np.float64], + ) -> NDArray[np.float64]: ... + + @staticmethod + def translation_jacobian( + Jx: NDArray[np.float64], x: DQ, + ) -> NDArray[np.float64]: ... + + @staticmethod + def plane_jacobian( + Jx: NDArray[np.float64], x: DQ, primitive: DQ, + ) -> NDArray[np.float64]: ... + + @staticmethod + def point_to_point_distance_jacobian( + Jt: NDArray[np.float64], p1: DQ, p2: DQ, + ) -> NDArray[np.float64]: ... + + @staticmethod + def plane_to_point_distance_jacobian( + Jpi: NDArray[np.float64], p: DQ, + ) -> NDArray[np.float64]: ... + + @staticmethod + def point_to_plane_distance_jacobian( + Jt: NDArray[np.float64], p: DQ, plane: DQ, + ) -> NDArray[np.float64]: ... + + @staticmethod + def point_to_line_distance_jacobian( + Jt: NDArray[np.float64], p: DQ, line: DQ, + ) -> NDArray[np.float64]: ... diff --git a/stubs/dqrobotics/solvers/__init__.pyi b/stubs/dqrobotics/solvers/__init__.pyi new file mode 100644 index 0000000..88586e9 --- /dev/null +++ b/stubs/dqrobotics/solvers/__init__.pyi @@ -0,0 +1,47 @@ +"""Stubs for `dqrobotics.solvers` (see `stubs/dqrobotics/__init__.pyi`).""" + +from __future__ import annotations + +from typing import Optional + +import numpy as np +from numpy.typing import NDArray + +__all__ = [ + "DQ_QuadraticProgrammingSolver", + "DQ_QuadprogSolver", +] + + +class DQ_QuadraticProgrammingSolver: + """Abstract base for quadratic-programming solvers. + + Solves:: + + min_x 0.5 * x' H x + f' x + s.t. A x <= b, Aeq x = beq + """ + + def solve_quadratic_program( + self, + H: NDArray[np.float64], + f: NDArray[np.float64], + A: Optional[NDArray[np.float64]], + b: Optional[NDArray[np.float64]], + Aeq: Optional[NDArray[np.float64]], + beq: Optional[NDArray[np.float64]], + ) -> NDArray[np.float64]: ... + + +class DQ_QuadprogSolver(DQ_QuadraticProgrammingSolver): + """Concrete QP solver backed by the ``quadprog`` package. + + Not instantiated when ``quadprog`` is not installed (the import is + guarded by a bare ``try/except`` in ``dqrobotics.solvers``). + """ + + def __init__(self) -> None: ... + + def set_equality_constraints_tolerance(self, tolerance: float) -> None: ... + + def get_equality_constraints_tolerance(self) -> float: ... diff --git a/stubs/dqrobotics/utils/__init__.pyi b/stubs/dqrobotics/utils/__init__.pyi new file mode 100644 index 0000000..bfb160a --- /dev/null +++ b/stubs/dqrobotics/utils/__init__.pyi @@ -0,0 +1,27 @@ +"""Stubs for `dqrobotics.utils` (see `stubs/dqrobotics/__init__.pyi`).""" + +from __future__ import annotations + +import numpy as np + +from dqrobotics import DQ + +__all__ = [ + "DQ_Geometry", +] + + +class DQ_Geometry: + """Stateless geometric distances between dual-quaternion primitives.""" + + @staticmethod + def point_to_point_squared_distance(p1: DQ, p2: DQ) -> float: ... + + @staticmethod + def point_to_point_distance(p1: DQ, p2: DQ) -> float: ... + + @staticmethod + def point_to_plane_distance(p: DQ, plane: DQ) -> float: ... + + @staticmethod + def point_to_line_squared_distance(p: DQ, line: DQ) -> float: ...