diff --git a/README.md b/README.md index 224a14d..c508b11 100644 --- a/README.md +++ b/README.md @@ -37,11 +37,11 @@ project already defines those targets. Includes are namespaced by domain, e.g. | Category | Description | |--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Inverse Kinematics (Damped Least Squares) | +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Redundancy Resolution, Inverse Kinematics (Damped Least Squares) | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Forward-Dynamics Integration, Generic Joint Link | -| [Planning](doc/planning/README.md) | Primitive Distance Queries | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory | -| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation | +| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | +| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index aac69a1..a6825e5 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -119,11 +119,6 @@ existing `dynamics/` convention. | # | Component | Target module | Depends on | Difficulty | |-----|-----------------------------------------------------------------|---------------------------|----------------|------------| -| M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | -| M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | NT53, NT55 | ★★☆☆☆ | -| M11 | Manipulability ellipsoid / Yoshikawa index | `kinematics` | M8 | ★★☆☆☆ | -| M14 | Redundancy resolution / null-space projection | `kinematics` | M8, NT62 | ★★☆☆☆ | -| M12 | Computed-torque (inverse-dynamics) control | `controllers/manipulator` | M29 | ★★★☆☆ | | M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, NT64, M8 | ★★★☆☆ | | M15 | Product-of-Exponentials forward kinematics | `kinematics` | NT53 | ★★★☆☆ | | M16 | Momentum-based collision-detection observer | `dynamics` | M29, M31 | ★★★☆☆ | @@ -144,29 +139,10 @@ M2 (polynomial profile), M3 (trapezoidal profile), M4 (friction compensation), M (S-curve profile) moved to numerical-toolbox as NT55, NT56, NT57, NT53 and NT60; their numbers are retired here. -### Tier 1 — Trivial ★☆☆☆☆ - -**M32. Inverse dynamics with an external tool wrench.** RNEA overload taking the environment's wrench on the tool; satisfies `τ(w) − τ(0) = −Jᵀw`. -- *Algorithm / paper:* Luh, Walker, Paul (1980). - ### Tier 2 — Easy ★★☆☆☆ -**M10. Cartesian path + orientation interpolation.** Straight-line position with SLERP orientation, one shared time law, twist feed-forward. -- *Algorithm / paper:* Lynch & Park, Ch. 9; K. Shoemake, *SIGGRAPH* 1985. -- *Reuses / builds on:* NT53 poses; any upstream motion profile (NT55, NT56, NT60) as the time law. - -**M11. Manipulability ellipsoid / Yoshikawa index.** `√det(JJᵀ)` (or `√det(JᵀJ)` when the task has more rows than joints), condition number and ellipsoid axes via SVD, translational and rotational parts separately. -- *Algorithm / paper:* T. Yoshikawa, *Int. J. Robotics Research*, 4(2), 1985. - -**M14. Redundancy resolution / null-space projection.** `q̇ = J⁺_λ ẋ + (I − J⁺J)·q̇₀` with an exact (undamped, rank-thresholded) projector, bound to the arm's task Jacobian. -- *Algorithm / paper:* A. Liégeois, *IEEE Trans. SMC*, 7(12), 1977. -- *Reuses / builds on:* upstream `solvers::DampedPseudoInverse` (NT62). - ### Tier 3 — Moderate ★★★☆☆ -**M12. Computed-torque (inverse-dynamics) control.** `τ = ID(q, q̇, q̈_d + Kd·ė + Kp·e)` in one `O(n)` call, yielding decoupled error dynamics. -- *Algorithm / paper:* Spong et al., *Robot Modeling and Control*, Ch. 8; Luh, Walker, Paul (1980). - **M13. Full 6-DOF pose IK.** Damped least squares on the SE(3) log-map error with position/rotation weighting, adaptive damping, step and joint-limit clamping. - *Algorithm / paper:* S. R. Buss (2004); Nakamura & Hanafusa (1986); Chiaverini (1997). - *Reuses / builds on:* upstream `optimization::LevenbergMarquardt` (NT64) with a manipulability damping policy. @@ -427,16 +403,11 @@ Cross-family building blocks that several of the families below depend on. | # | Component | Target module | Depends on | Difficulty | |----|----------------------------------------------------------|---------------|------------|------------| -| P2 | Artificial potential fields | `planning` | P1 | ★★☆☆☆ | | P3 | A* / Dijkstra on bounded occupancy grids | `planning` | — | ★★★☆☆ | | P4 | Self-collision checking for a link chain (capsule model) | `planning` | P1, M30 | ★★★☆☆ | | P5 | GJK + EPA convex distance and penetration | `planning` | — | ★★★★☆ | | P6 | RRT-Connect in joint space with a fixed node pool | `planning` | P4 | ★★★★☆ | -**P2. Potential fields.** Quadratic-then-conic attraction and Khatib repulsion from obstacle proximity readings as a clamped task-space velocity; local minima are detected and reported. -- *Algorithm / paper:* O. Khatib, *IJRR*, 5(1), 1986. -- *Reuses / builds on:* P1. - **P3. Grid search.** A* and Dijkstra on 2-D/3-D grids with per-cell costs from a callable, an indexed binary heap with decrease-key, 16-bit indices on small grids and the octile heuristic (no heap allocation). - *Algorithm / paper:* P. Hart, N. Nilsson, B. Raphael, *IEEE Trans. Systems Science and Cybernetics*, 4(2), 1968. diff --git a/doc/controllers/manipulator/ComputedTorqueControl.md b/doc/controllers/manipulator/ComputedTorqueControl.md new file mode 100644 index 0000000..e962f9e --- /dev/null +++ b/doc/controllers/manipulator/ComputedTorqueControl.md @@ -0,0 +1,89 @@ +# Computed-Torque Control + +## Overview & Motivation + +A manipulator's joints are coupled. Accelerating one joint loads the others through the mass matrix; velocities create Coriolis and centrifugal torques; gravity pulls on every joint differently in every pose. Independent PD loops per joint fight these couplings and need high gains to track a trajectory. + +**Computed-torque control** uses the dynamic model to cancel the couplings exactly. It commands the torque that makes the arm behave like a set of independent, unit-mass double integrators. A linear PD-plus-feedforward law then shapes each joint's tracking error. +It is the textbook example of **feedback linearization** in robotics, and the reference against which model-based joint controllers are compared. + +## Mathematical Theory + +### Control Law + +For a desired trajectory $(q_d, \dot{q}_d, \ddot{q}_d)$ with errors $e = q_d - q$ and $\dot{e} = \dot{q}_d - \dot{q}$, the outer loop commands a joint acceleration + +$$a_q = \ddot{q}_d + K_d\,\dot{e} + K_p\,e$$ + +and the inner loop computes the torque that produces it at the measured state: + +$$\tau = M(q)\,a_q + C(q, \dot{q})\,\dot{q} + g(q)$$ + +### Closed-Loop Error Dynamics + +Substituting into the manipulator equation $M\ddot{q} + C\dot{q} + g = \tau$ gives $M(q)(\ddot{q} - a_q) = 0$. Since $M$ is positive definite, $\ddot{q} = a_q$, and therefore + +$$\ddot{e} + K_d\,\dot{e} + K_p\,e = 0$$ + +With diagonal gains every joint is an independent, linear, second-order system. Choosing $K_p = \omega_n^2$ and $K_d = 2\zeta\omega_n$ per joint sets its natural frequency and damping, and $\zeta = 1$ ($K_d = 2\sqrt{K_p}$) gives critical damping. The error converges exponentially for any symmetric positive-definite gains. + +### One-Pass Evaluation + +The torque is exactly inverse dynamics evaluated at $(q, \dot{q}, a_q)$. The recursive Newton–Euler algorithm returns $M a_q + C\dot{q} + g$ in a single $O(n)$ pass, without forming, multiplying or inverting the mass matrix. That keeps the controller linear-time and numerically benign. + +### Model Error + +With an imperfect model $\hat{M}, \hat{h}$, the closed loop becomes $\ddot{e} + K_d\dot{e} + K_p e = M^{-1}\left[(M - \hat{M})a_q + (h - \hat{h})\right]$. The error no longer vanishes but stays bounded for bounded model error, and higher gains shrink it at the price of bandwidth. + +## Complexity Analysis + +| Part | Time | Space | Notes | +|-------------------------|-----------------|----------|---------------------------------------------------------------| +| Outer loop | $O(n^2)$ | $O(n^2)$ | Two gain products; $O(n)$ when the gains are diagonal | +| Inverse dynamics | $O(n)$ | $O(n)$ | One recursive Newton–Euler pass at the commanded acceleration | +| Total per control cycle | $O(n)$–$O(n^2)$ | $O(n^2)$ | No mass matrix is built or inverted | + +## Step-by-Step Walkthrough + +A two-joint arm with $K_p = \mathrm{diag}(100, 100)$ and $K_d = \mathrm{diag}(20, 20)$, so each joint is critically damped at $\omega_n = 10$ rad/s: + +1. Errors $e = (0.1, -0.2)$ rad, $\dot{e} = 0$, $\ddot{q}_d = 0$ give the commanded acceleration $a_q = K_p e = (10, -20)$ rad/s². +2. Inverse dynamics at the measured $(q, \dot{q})$ with $\ddot{q} = a_q$ returns the torque that makes the arm accelerate exactly at $a_q$, including the coupling, velocity and gravity terms. +3. With an exact model, each joint's error then follows $e(t) = (e_0 + (\dot{e}_0 + 10 e_0)t)\,e^{-10t}$. Starting at rest with $e_0 = 0.3$ rad, the error is about $1.5 \times 10^{-4}$ rad after one second, with no overshoot. +4. Adding velocity and acceleration feedforward makes the same law track a moving trajectory with the same error dynamics. + +## Pitfalls & Edge Cases + +- **Model fidelity**: cancellation is only as good as the model. Payload, friction and unmodelled actuator dynamics appear as disturbances; add integral action or adaptation, or update the link parameters when the payload changes. +- **Actuator saturation**: aggressive gains or trajectories demand torques the motors cannot deliver. Once saturated, the cancellation and the linear error dynamics no longer hold. Plan trajectories within the torque limits. +- **Sample rate**: the continuous-time analysis assumes the loop runs much faster than the closed-loop bandwidth. A digital loop with delay tolerates lower gains than the ideal $K_p$, $K_d$. +- **Velocity noise**: $K_d$ and the Coriolis term both use the measured $\dot{q}$. Filter it or use an observer. +- **Measured versus desired state**: the inverse dynamics must use the **measured** $q, \dot{q}$. Evaluating them at the desired state gives the cheaper "feedforward" variant, which no longer linearizes the loop. + +## Variants & Generalizations + +- **PD plus feedforward**: $\tau = \mathrm{ID}(q_d, \dot{q}_d, \ddot{q}_d) + K_p e + K_d \dot{e}$, where the model is evaluated along the trajectory. It allows precomputation but only approximately decouples the joints. +- **Computed torque with integral action**: an integral term rejects constant model error and gravity mismatch. +- **Robust computed torque**: a sliding-mode or Lyapunov-redesign term bounds the effect of model uncertainty. +- **Adaptive control (Slotine–Li)**: estimates the inertial parameters online, keeping stability without an exact model. +- **Operational-space control**: the task-space analogue, which linearizes the end-effector dynamics instead of the joints. + +## Applications + +- High-performance trajectory tracking on industrial and research arms with accurate dynamic models. +- Fast pick-and-place where coupling torques dominate at high speed. +- Baseline controller for evaluating adaptive, robust and learning-based schemes. +- Model validation: tracking error under computed torque reveals model error directly. + +## Connections to Other Algorithms + +- [Chain Dynamics Model](../../dynamics/ChainDynamicsModel.md): supplies the one-call inverse dynamics this controller consumes. +- [Recursive Newton-Euler](../../dynamics/RecursiveNewtonEuler.md): evaluates $M a_q + C\dot{q} + g$ in one $O(n)$ pass. +- [PD Control with Gravity Compensation](PdGravityCompensation.md): the set-point special case that needs only $g(q)$ and no trajectory feedforward. +- [Cubic Spline Trajectory](../../trajectory/CubicSplineTrajectory.md): provides the $C^2$ reference $(q_d, \dot{q}_d, \ddot{q}_d)$ the feedforward needs. + +## References & Further Reading + +- Luh, J.Y.S., Walker, M.W. and Paul, R.P.C. (1980). "Resolved-acceleration control of mechanical manipulators." *IEEE Transactions on Automatic Control*, 25(3), 468–474. +- Spong, M.W., Hutchinson, S. and Vidyasagar, M. (2020). *Robot Modeling and Control*. 2nd ed. Wiley. Chapter 9. +- Siciliano, B., Sciavicco, L., Villani, L. and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 8. diff --git a/doc/controllers/manipulator/README.md b/doc/controllers/manipulator/README.md index 079be7a..df7a04e 100644 --- a/doc/controllers/manipulator/README.md +++ b/doc/controllers/manipulator/README.md @@ -4,6 +4,7 @@ Joint- and task-space controllers for robot manipulators. ## Algorithms -| Algorithm | Description | -|------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------| -| [PD Control with Gravity Compensation](PdGravityCompensation.md) | Globally stable set-point regulator $\tau = K_p e - K_d \dot{q} + g(q)$ — needs only the gravity torque | +| Algorithm | Description | +|------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------| +| [PD Control with Gravity Compensation](PdGravityCompensation.md) | Globally stable set-point regulator $\tau = K_p e - K_d \dot{q} + g(q)$ — needs only the gravity torque | +| [Computed-Torque Control](ComputedTorqueControl.md) | Feedback linearization: one inverse-dynamics pass at the commanded acceleration gives decoupled error dynamics $\ddot{e} + K_d\dot{e} + K_p e = 0$ | diff --git a/doc/dynamics/README.md b/doc/dynamics/README.md index a730924..a90298a 100644 --- a/doc/dynamics/README.md +++ b/doc/dynamics/README.md @@ -4,13 +4,13 @@ Rigid-body dynamics algorithms for robotics and mechanical systems. ## Algorithms -| Algorithm | Description | -|------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------| -| [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) | $O(n)$ forward dynamics for serial chains — computes joint accelerations from applied torques | -| [Chain Dynamics Model](ChainDynamicsModel.md) | Manipulator equation terms $M$, $C\dot{q}$, $g$ and one-pass inverse dynamics from a link chain, behind abstract interfaces | -| [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | -| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | -| [Forward-Dynamics Integration](ForwardDynamicsIntegrator.md) | Fixed-step simulation of a chain on the articulated-body dynamics — semi-implicit Euler or RK4 | -| [Generic Joint Link](JointLink.md) | Revolute or prismatic joints with limits and armature through one motion-subspace abstraction | -| [Newton-Euler](NewtonEuler.md) | Single rigid-body forward and inverse dynamics using Newton's and Euler's equations | -| [Recursive Newton-Euler](RecursiveNewtonEuler.md) | $O(n)$ inverse dynamics for serial kinematic chains via recursive velocity/force propagation | +| Algorithm | Description | +|------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------| +| [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) | $O(n)$ forward dynamics for serial chains — computes joint accelerations from applied torques | +| [Chain Dynamics Model](ChainDynamicsModel.md) | Manipulator equation terms $M$, $C\dot{q}$, $g$ and one-pass inverse dynamics from a link chain, behind abstract interfaces | +| [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | +| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | +| [Forward-Dynamics Integration](ForwardDynamicsIntegrator.md) | Fixed-step simulation of a chain on the articulated-body dynamics — semi-implicit Euler or RK4 | +| [Generic Joint Link](JointLink.md) | Revolute or prismatic joints with limits and armature through one motion-subspace abstraction | +| [Newton-Euler](NewtonEuler.md) | Single rigid-body forward and inverse dynamics using Newton's and Euler's equations | +| [Recursive Newton-Euler](RecursiveNewtonEuler.md) | $O(n)$ inverse dynamics for serial kinematic chains via recursive velocity/force propagation, optionally with an external tool wrench | diff --git a/doc/dynamics/RecursiveNewtonEuler.md b/doc/dynamics/RecursiveNewtonEuler.md index 87f0501..6b34643 100644 --- a/doc/dynamics/RecursiveNewtonEuler.md +++ b/doc/dynamics/RecursiveNewtonEuler.md @@ -67,6 +67,14 @@ $$a_{J_i} = R_i^T \left( a_{J_{i-1}} + \dot{\omega}_{i-1} \times r^{(i)} + \omeg The joint effort projects the force instead of the moment, $\tau_{q_i} = \hat{z}_i^T f_i$. With the motion subspace $(s_\omega; s_v)$ both cases read $\tau_{q_i} = s_\omega^T \tau_i + s_v^T f_i$. Reflected actuator inertia (armature) adds $J_{a,i}\ddot{q}_i$ to each joint effort. +### External Tool Wrench + +When the environment pushes on the tool with a wrench $(f_e; n_e)$ (base frame, moment about the tool point), with the tool point $t$ given in the last link frame,, the last link receives that wrench in addition to its inertial load. Rotated into the last link frame and moved to its joint origin: + +$$f_e' = {}^{0}R_{n-1}^T f_e, \qquad n_e' = {}^{0}R_{n-1}^T n_e + t \times f_e'$$ + +The backward pass starts from $f_{n-1} - f_e'$ and $\tau_{n-1} - n_e'$: the environment carries part of the load, so the joints need less. The result satisfies $\tau(w) - \tau(0) = -J^T w$ with the geometric Jacobian at the tool point. A held payload of mass $m_p$ is the wrench $(m_p g; 0)$, with $g$ the gravity vector. + ### Rodrigues' Rotation Formula The rotation matrix for angle $\theta$ about unit axis $\hat{u} = [u_x, u_y, u_z]^T$: diff --git a/doc/kinematics/ManipulabilityIndex.md b/doc/kinematics/ManipulabilityIndex.md new file mode 100644 index 0000000..074050a --- /dev/null +++ b/doc/kinematics/ManipulabilityIndex.md @@ -0,0 +1,91 @@ +# Manipulability Index + +## Overview & Motivation + +Near a **singular configuration** a manipulator loses the ability to move its tool in some direction. A stretched arm cannot reach further outwards, and a wrist with aligned axes cannot rotate about a third one. Approaching such a pose is dangerous: Jacobian inverses blow up, joint rates spike, and small tool motions demand huge joint motions. + +The **manipulability index** of Yoshikawa (1985) measures how far a configuration is from singularity. It is the volume of the **velocity ellipsoid**, the set of tool velocities reachable with joint rates of unit norm. +The singular values of the Jacobian give that ellipsoid's axes, and their ratio, the **condition number**, measures how isotropic the arm is. +Planners maximize manipulability, controllers monitor it before inverting the Jacobian, and arm designers use it to place the workspace in the dexterous region. + +## Mathematical Theory + +### Velocity Ellipsoid + +For a task Jacobian block $B \in \mathbb{R}^{r \times n}$ with tool velocity $\dot{x} = B\dot{q}$, the image of the unit sphere $\|\dot{q}\| \le 1$ is the ellipsoid + +$$\dot{x}^T (B B^T)^{-1} \dot{x} \le 1$$ + +whose semi-axes are the singular values $\sigma_1 \ge \sigma_2 \ge \dots$ of $B$, along the left singular vectors. + +### Yoshikawa Index + +$$w = \sqrt{\det(B B^T)} = \prod_i \sigma_i \quad (r \le n), \qquad w = \sqrt{\det(B^T B)} \quad (r > n)$$ + +With more task rows than joints, $B B^T$ has rank at most $n$ and its determinant is identically zero. $\sqrt{\det(B^T B)}$ is then the volume of the $n$-dimensional ellipsoid the arm actually spans. For a square block, $w = |\det B|$. In both cases $w = 0$ exactly at a singularity. + +### Units + +Linear rows have units of m/s per unit joint rate; angular rows, rad/s. A determinant over a full 6-row Jacobian mixes the two and changes with the length unit. The index is therefore computed separately for the **translational** block (rows 0–2) and the **rotational** block (rows 3–5). + +### Condition Number + +$$\kappa = \frac{\sigma_{\max}}{\sigma_{\min}} \in [1, \infty]$$ + +$\kappa = 1$ is an isotropic configuration that moves equally well in every direction; $\kappa \to \infty$ at a singularity. Unlike $w$, $\kappa$ is scale-invariant and depends only on the ellipsoid's shape. A smallest singular value below a relative threshold is reported as an infinite condition number. + +## Complexity Analysis + +| Quantity | Time | Space | Notes | +|--------------------------|-----------------------|-----------|---------------------------------------------------| +| Index $w$ | $O(r\,n\,m) + O(m^3)$ | $O(m^2)$ | Gram product and LU determinant, $m = \min(r, n)$ | +| Ellipsoid axes, $\kappa$ | $O(r\,n\,m)$ | $O(r\,n)$ | One Golub–Kahan SVD of the $r \times n$ block | + +The Gram/LU path is cheaper and suits a per-cycle monitor. The SVD is more accurate near singularity, since it avoids squaring the Jacobian, and also yields the axes and directions. + +## Step-by-Step Walkthrough + +A planar two-link arm with unit links, translational rows, elbow angle $q_2$: + +1. The linear Jacobian has two independent columns spanning the plane, and $\sqrt{\det(B^T B)} = |\sin q_2|$. +2. At $q_2 = \pi/2$, $w = 1$, the maximum. The singular values are $(\varphi, 1/\varphi) = (1.618, 0.618)$, their product is $1$, and $\kappa = \varphi^2 = 2.618$. +3. At $q_2 = 0.1$, $w = 0.0998$ and $\kappa \approx 50$; at $q_2 = 0.01$, $\kappa \approx 500$. The ellipsoid flattens towards a line. +4. At $q_2 = 0$ (stretched) or $\pi$ (folded), $w = 0$ and $\kappa = \infty$: the tool cannot move along the arm. +5. A redundant three-link planar arm at $q = (0, \pi/2, \pi/2)$ has $B B^T = \begin{bmatrix} 2 & 1 \\ 1 & 2 \end{bmatrix}$, so $w = \sqrt{3}$ with axes $(\sqrt{3}, 1)$. + +## Pitfalls & Edge Cases + +- **Wrong Gram product**: with fewer joints than task rows, $\det(B B^T) \equiv 0$ regardless of configuration. Use $B^T B$. +- **Planar arms with 3D position rows**: the $z$ row is zero, so a redundant planar arm measured with three position rows reports $w \equiv 0$. Measure planar tasks with the planar rows only. +- **Mixed units**: never take one determinant over linear and angular rows together; the result changes with the choice of metres versus millimetres. +- **Single precision near singularity**: forming $B B^T$ squares the condition number. The determinant reaches the singular threshold once $\sigma_{\min}/\sigma_{\max} \lesssim 10^{-3}$, while the singular values stay accurate below that. +- **Not a distance**: $w$ is not linear in the distance to singularity, and its scale depends on link lengths. Compare values only within one arm, or normalize by the maximum. + +## Variants & Generalizations + +- **Force manipulability**: the force ellipsoid is the dual of the velocity ellipsoid, with axes $1/\sigma_i$. Directions that move fast resist force poorly. +- **Dynamic manipulability**: weights by the inverse mass matrix, $J M^{-1}$, to measure achievable accelerations under torque limits (Yoshikawa 1985b). +- **Inverse condition number and local conditioning index**: $1/\kappa \in [0, 1]$, often preferred as a bounded objective. +- **Global conditioning index**: the average of $1/\kappa$ over the workspace, a design criterion. +- **Manipulability gradients**: for null-space optimization of redundant arms, $\nabla_q w$ moves the arm away from singularities. + +## Applications + +- Singularity monitoring and switching to damped inverses before a Jacobian is inverted. +- Secondary objective for redundant arms: maximizing $w$ in the null space keeps them dexterous. +- Workspace and design analysis: placing tasks and choosing link lengths for high average dexterity. +- Teleoperation feedback that warns the operator before a singular pose. + +## Connections to Other Algorithms + +- [Geometric Jacobian](GeometricJacobian.md): the source of the task Jacobian blocks measured here. +- [Inverse Kinematics](InverseKinematics.md): damped least squares is the remedy once manipulability drops. +- [Chain Pose Kinematics](ChainPoseKinematics.md) and [Denavit-Hartenberg Parameters](DenavitHartenberg.md): produce the frame chains behind the Jacobian. +- The singular value and LU decompositions come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Yoshikawa, T. (1985). "Manipulability of robotic mechanisms." *International Journal of Robotics Research*, 4(2), 3–9. +- Salisbury, J.K. and Craig, J.J. (1982). "Articulated hands: force control and kinematic issues." *International Journal of Robotics Research*, 1(1), 4–17. +- Siciliano, B., Sciavicco, L., Villani, L. and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 3. +- Lynch, K.M. and Park, F.C. (2017). *Modern Robotics*. Cambridge University Press. Chapter 5. diff --git a/doc/kinematics/README.md b/doc/kinematics/README.md index ba855d4..1517dd9 100644 --- a/doc/kinematics/README.md +++ b/doc/kinematics/README.md @@ -4,10 +4,12 @@ Geometry and motion algorithms for serial kinematic chains. ## Algorithms -| Algorithm | Description | -|-------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------| -| [Forward Kinematics](ForwardKinematics.md) | Computes 3D joint positions and the tool point from joint angles for serial chains — $O(n)$ complexity | -| [Chain Pose Kinematics](ChainPoseKinematics.md) | Full tool pose in SE(3) plus every joint origin and axis in the base frame (the frame chain) — $O(n)$ | -| [Denavit-Hartenberg Parameters](DenavitHartenberg.md) | Standard and modified DH tables to tool pose and frame chain, with constant joint offsets — $O(n)$ | -| [Geometric Jacobian](GeometricJacobian.md) | $6 \times n$ Jacobian, bias acceleration $\dot{J}\dot{q}$ and statics $\tau = J^T w$ from any frame chain — $O(n)$ | -| [Inverse Kinematics](InverseKinematics.md) | Solves joint angles that place the tool point on a target via Damped Least Squares — $O(k \cdot n)$ iterative | +| Algorithm | Description | +|-------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------| +| [Forward Kinematics](ForwardKinematics.md) | Computes 3D joint positions and the tool point from joint angles for serial chains — $O(n)$ complexity | +| [Chain Pose Kinematics](ChainPoseKinematics.md) | Full tool pose in SE(3) plus every joint origin and axis in the base frame (the frame chain) — $O(n)$ | +| [Denavit-Hartenberg Parameters](DenavitHartenberg.md) | Standard and modified DH tables to tool pose and frame chain, with constant joint offsets — $O(n)$ | +| [Geometric Jacobian](GeometricJacobian.md) | $6 \times n$ Jacobian, bias acceleration $\dot{J}\dot{q}$ and statics $\tau = J^T w$ from any frame chain — $O(n)$ | +| [Manipulability Index](ManipulabilityIndex.md) | Yoshikawa index, velocity-ellipsoid axes and condition number per unit-consistent Jacobian block — singularity monitor | +| [Redundancy Resolution](RedundancyResolution.md) | Damped pseudo-inverse primary task plus exact null-space projection of a secondary objective for redundant arms | +| [Inverse Kinematics](InverseKinematics.md) | Solves joint angles that place the tool point on a target via Damped Least Squares — $O(k \cdot n)$ iterative | diff --git a/doc/kinematics/RedundancyResolution.md b/doc/kinematics/RedundancyResolution.md new file mode 100644 index 0000000..0f204de --- /dev/null +++ b/doc/kinematics/RedundancyResolution.md @@ -0,0 +1,105 @@ +# Redundancy Resolution + +## Overview & Motivation + +A manipulator is **redundant** for a task when it has more joints than the task has dimensions, such as a 7-joint arm positioning and orienting its tool (6 dimensions), or any 4+ joint arm doing a pure positioning task (3). The extra freedom forms a continuous family of joint motions that leave the tool still: the elbow can swing while the hand holds a cup. + +Redundancy resolution uses that freedom deliberately. The joint velocity is split into a **primary** part that achieves the commanded tool velocity and a **secondary** part confined to the Jacobian's null space. The secondary part pursues another objective without disturbing the tool: + +- avoiding joint limits; +- staying away from singularities; +- keeping clear of obstacles; +- minimizing effort. + +This is the classic gradient-projection method of Liégeois (1977), the foundation of whole-body and task-priority control. + +## Mathematical Theory + +### Primary and Secondary Motion + +With the task Jacobian $J \in \mathbb{R}^{m \times n}$, $n > m$: + +$$\dot{q} = J^{+}_{\lambda}\,\dot{x} + \left(\mathbf{I} - J^{+}J\right)\dot{q}_0$$ + +- $J^{+}_{\lambda} = J^T (J J^T + \lambda^2 \mathbf{I})^{-1}$ is the **damped** pseudo-inverse, which reproduces $\dot{x}$ with the minimum joint-velocity norm. Damping trades exactness for bounded joint rates near singularities. +- $P = \mathbf{I} - J^{+}J$ is the **null-space projector**. It maps any $\dot{q}_0$ to the closest joint velocity that produces no tool motion, so $J P = 0$. + +### Singular Value Form + +With the SVD $J = U \Sigma V^T$ and singular values $\sigma_i$: + +$$J^{+}_{\lambda} = \sum_i \frac{\sigma_i}{\sigma_i^2 + \lambda^2}\, v_i u_i^T, \qquad P = \mathbf{I} - \sum_{\sigma_i > \epsilon\,\sigma_1} v_i v_i^T$$ + +The damped gain $\sigma/(\sigma^2 + \lambda^2)$ never exceeds $1/(2\lambda)$, so joint rates stay bounded as $\sigma_{\min} \to 0$. +The projector is built **undamped** from the rank-thresholded singular vectors, which keeps it an exact orthogonal projector: symmetric, idempotent and invisible to the task for any $\lambda$. The damped $\mathbf{I} - J^{+}_{\lambda}J$ is not, and leaks secondary motion into the tool. + +### Task Accuracy Under Damping + +With $\lambda > 0$ the primary task is met only approximately: + +$$\|J\dot{q} - \dot{x}\| \le \frac{\lambda^2}{\sigma_{\min}^2 + \lambda^2}\,\|\dot{x}\|$$ + +Far from singularities ($\sigma_{\min} \gg \lambda$) the error is negligible; near them, accuracy is traded for bounded joint rates. + +### Secondary Objectives + +Choosing $\dot{q}_0 = -k\,\nabla H(q)$ descends a cost $H$ inside the self-motion manifold. Since $P$ is a projector, $\nabla H^T P \nabla H \ge 0$, and the projected motion never increases $H$ to first order. Common choices: + +- the negative gradient of manipulability, to avoid singularities; +- the squared normalized distance to joint centres, to avoid joint limits; +- obstacle potentials evaluated on the links. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|----------------------|------------------------------|----------|--------------------------------------------------------| +| Resolve | $O(m\,n\,\min(m,n)) + O(mn)$ | $O(mn)$ | One SVD; the projector is applied without being formed | +| Pseudo-inverse | + $O(n\,m\,r)$ | $O(nm)$ | Diagnostic accessor | +| Null-space projector | + $O(n^2 r)$ | $O(n^2)$ | Diagnostic accessor; $r$ is the numerical rank | + +## Step-by-Step Walkthrough + +A 7-joint anthropomorphic arm with a full-pose task ($m = 6$): + +1. At a generic configuration the six singular values are all well above the threshold, so the null space has dimension $7 - 6 = 1$: the elbow's self-motion. +2. With $\lambda = 0$, the resolved velocity reproduces the commanded tool twist exactly, and $J J^{+} = \mathbf{I}_6$. +3. With $\lambda = 0.01$ and $\sigma_{\min} \approx 0.18$, the tool-velocity error is a few $10^{-4}$, while the null-space part still produces no tool motion at all. +4. Feeding $\dot{q}_0 = -\nabla H$ for $H = \tfrac12 \sum (q_i/q_{\max})^2$ with a zero task velocity moves the elbow so that $H$ decreases, while the hand stays put. +5. For a position-only task ($m = 3$) the same arm has a 4-dimensional null space. At a stretched configuration two singular values vanish, the rank drops to 4 and the full-pose null space grows from 1 to 3 dimensions. + +## Pitfalls & Edge Cases + +- **Damped projector leakage**: building $P$ from $J^{+}_{\lambda}$ lets secondary motion disturb the tool. Use the undamped, rank-thresholded projector. +- **Lost directions at singularities**: once a singular value drops below the threshold, its task direction joins the null space. Secondary motion may then move the tool along that direction, by design, because the arm cannot control it anyway. +- **Gain of the secondary term**: $\dot{q}_0$ is a velocity. Over-scaled gradients saturate joint rates without affecting the task, so scale them with the control period and joint limits. +- **Not a full priority hierarchy**: a single null-space term protects one task. Several prioritized tasks need recursive projectors (Siciliano–Slotine) or hierarchical quadratic programs. +- **Drift**: integrating velocities accumulates task error. Close the loop with a task-space error term ($\dot{x} = \dot{x}_d + K e$). + +## Variants & Generalizations + +- **Task-priority redundancy resolution**: secondary tasks with their own Jacobians, $\dot{q} = J_1^{+}\dot{x}_1 + (J_2 P_1)^{+}(\dot{x}_2 - J_2 J_1^{+}\dot{x}_1)$. +- **Weighted pseudo-inverse**: minimizes $\dot{q}^T W \dot{q}$ instead of the plain norm, for instance with $W = M(q)$ for the dynamically consistent inverse. +- **Acceleration-level resolution**: the same structure at the acceleration level, with the bias acceleration $\dot{J}\dot{q}$, for torque-controlled arms. +- **Inequality constraints**: joint limits as hard bounds lead to quadratic programs instead of projections. + +## Applications + +- Joint-limit and singularity avoidance on 7-joint collaborative arms. +- Obstacle avoidance with the elbow while the tool follows a path. +- Posture optimization for stiffness, manipulability or energy. +- Mobile manipulators, where the base adds redundancy to the arm. + +## Connections to Other Algorithms + +- [Geometric Jacobian](GeometricJacobian.md): provides the task Jacobian whose pseudo-inverse and null space are used. +- [Manipulability Index](ManipulabilityIndex.md): its gradient is a common secondary objective, and its value warns when damping matters. +- [Inverse Kinematics](InverseKinematics.md): damped least squares is the primary term without a null-space contribution. +- [Denavit-Hartenberg Parameters](DenavitHartenberg.md): describes the redundant arms these methods are typically applied to. +- The damped pseudo-inverse and exact null-space projector come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Liégeois, A. (1977). "Automatic supervisory control of the configuration and behavior of multibody mechanisms." *IEEE Transactions on Systems, Man, and Cybernetics*, 7(12), 868–871. +- Nakamura, Y. (1991). *Advanced Robotics: Redundancy and Optimization*. Addison-Wesley. +- Siciliano, B. and Slotine, J.-J. (1991). "A general framework for managing multiple tasks in highly redundant robotic systems." *Proc. ICAR*, 1211–1216. +- Chiaverini, S. (1997). "Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators." *IEEE Transactions on Robotics and Automation*, 13(3), 398–410. diff --git a/doc/planning/PotentialField.md b/doc/planning/PotentialField.md new file mode 100644 index 0000000..0de935d --- /dev/null +++ b/doc/planning/PotentialField.md @@ -0,0 +1,90 @@ +# Artificial Potential Fields + +## Overview & Motivation + +A robot often needs a velocity command **right now**, using only what its sensors see, rather than a complete collision-free path. Examples are a mobile base nudging around a person, or a manipulator's elbow dodging a moving obstacle. +**Artificial potential fields** (Khatib, 1986) answer with a single gradient step. The goal is an attractive well, each nearby obstacle an expanding hill, and the robot follows the negative gradient of their sum. + +The method is constant-time per obstacle, needs no map and no search, and reacts instantly to new proximity readings, which makes it a classic reactive layer below a global planner. Its well-known weakness is **local minima**, where attraction and repulsion cancel short of the goal. Here they are detected and reported, so the caller can switch strategy. + +## Mathematical Theory + +### Attractive Potential + +With the error $e = g - p$ and distance $d = \|e\|$, a quadratic well near the goal blends into a cone farther away: + +$$U_{\text{att}} = \begin{cases} \tfrac12 k_a d^2 & d \le d^* \\ k_a d^* d - \tfrac12 k_a d^{*2} & d > d^* \end{cases} \qquad F_{\text{att}} = -\nabla U_{\text{att}} = \begin{cases} k_a\,e & d \le d^* \\ k_a d^*\,\dfrac{e}{d} & d > d^* \end{cases}$$ + +The quadratic part gives a pull proportional to the distance, so the robot slows smoothly into the goal. The conic part caps the pull at $k_a d^*$, so a far goal cannot overwhelm the repulsion. + +### Repulsive Potential + +For an obstacle at surface distance $\rho$, with unit direction $n$ pointing from the obstacle towards the robot: + +$$U_{\text{rep}} = \begin{cases} \tfrac12 k_r \left(\dfrac{1}{\rho} - \dfrac{1}{\rho_0}\right)^2 & \rho < \rho_0 \\ 0 & \rho \ge \rho_0 \end{cases} \qquad F_{\text{rep}} = k_r \left(\frac{1}{\rho} - \frac{1}{\rho_0}\right)\frac{1}{\rho^2}\,n$$ + +The repulsion is zero beyond the influence distance $\rho_0$, grows without bound as $\rho \to 0$, and contributions from several obstacles add vectorially. + +### Command and Status + +The total force $F = F_{\text{att}} + \sum F_{\text{rep}}$ is used as a velocity, clamped to a maximum speed. The result is classified as: + +- **reached** when $d$ is within the goal tolerance; +- **stalled** when $\|F\|$ falls below a small threshold away from the goal, which is a local minimum or saddle; +- **moving** otherwise. + +### Local Minima + +With an obstacle exactly between the robot and the goal, the forces are collinear and opposite. For $k_a d^* = 1$, $k_r = 0.1$ and $\rho_0 = 1$, they balance where $0.1(1/\rho - 1)/\rho^2 = 1$, at $\rho^* = 0.393$: an equilibrium short of the goal. No local rule removes such points in general, which is why the field is paired with a global planner or an escape behaviour. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|---------------|--------|--------|-------------------------------------------------| +| $M$ obstacles | $O(M)$ | $O(1)$ | One division chain per obstacle inside $\rho_0$ | + +## Step-by-Step Walkthrough + +Gains $k_a = 1$, $d^* = 1$, $k_r = 0.1$, $\rho_0 = 1$, speed limit $0.5$, robot at the origin: + +1. Goal at $(0.4, 0.3, 0)$: $d = 0.5 \le d^*$, so $F = (0.4, 0.3, 0)$. The magnitude $0.5$ is within the limit, and the robot moves straight at the goal. +2. Goal at $(3, 4, 0)$: $d = 5 > d^*$, the conic pull $(0.6, 0.8, 0)$ has magnitude $1$, and it is clamped to $(0.3, 0.4, 0)$. +3. Add an obstacle $0.5$ m away, pushing along $-y$: $F_{\text{rep}} = 0.1 \cdot (2 - 1)/0.25 = 0.4$ along $-y$. The total $(0.6, 0.4, 0)$ is clamped to $(0.416, 0.277, 0)$, so the robot veers away from the obstacle. +4. Goal at $(3, 0, 0)$ with an obstacle at $\rho = 0.393$ pushing along $-x$: both forces have magnitude $1$ and cancel. The status is **stalled**. +5. With the same obstacle at $\rho = 0.3$, the repulsion of $2.59$ wins and the robot backs away at the speed limit. + +## Pitfalls & Edge Cases + +- **Local minima and saddles**: detection is cheap, escape is not. Use a random walk, wall following, or intermediate goals from a global planner. +- **Oscillation in corridors**: opposing walls create lateral forces that alternate sign. Damping the command or a lower $k_r$ helps. +- **Goals near obstacles (GNRON)**: with the repulsion above, a goal inside $\rho_0$ of an obstacle may be unreachable. Variants scale the repulsion by the goal distance. +- **Contact**: as $\rho \to 0$ the repulsion diverges. The speed clamp keeps commands finite, but the caller must stop at contact; signed distances below zero are not valid input. +- **Units and tuning**: $k_a$ has units of 1/s, and $k_r$ of m³/s for a velocity output. Tune the speed limit and $\rho_0$ for the robot's stopping distance. + +## Variants & Generalizations + +- **Navigation functions** (Rimon and Koditschek): potentials with a unique minimum on sphere worlds, at the cost of global knowledge. +- **Harmonic potentials**: solutions of Laplace's equation have no interior local minima, but require a grid solve. +- **Vortex and circulation fields**: tangential components steer around obstacles instead of pushing directly away. +- **Manipulator control points**: evaluate the field at several points on the arm and map each force through the transposed point Jacobian into joint torques or rates. +- **Elastic bands**: deform a global path with repulsive forces instead of commanding velocity directly. + +## Applications + +- Reactive obstacle avoidance for mobile robots and drones between global replanning cycles. +- Collision avoidance for manipulator links in shared workspaces. +- Local refinement and smoothing of sampled paths. +- Teleoperation assistance that pushes the operator away from obstacles. + +## Connections to Other Algorithms + +- [Primitive Distance Queries](PrimitiveDistance.md): produce the proximity readings (distance and direction) fed into the repulsive term. +- [Geometric Jacobian](../kinematics/GeometricJacobian.md): maps the task-space field force at a control point to joint space. +- [Redundancy Resolution](../kinematics/RedundancyResolution.md): a repulsive term in the null space keeps a redundant arm clear of obstacles without moving the tool. + +## References & Further Reading + +- Khatib, O. (1986). "Real-time obstacle avoidance for manipulators and mobile robots." *International Journal of Robotics Research*, 5(1), 90–98. +- Koren, Y. and Borenstein, J. (1991). "Potential field methods and their inherent limitations for mobile robot navigation." *Proc. IEEE ICRA*, 1398–1404. +- Rimon, E. and Koditschek, D.E. (1992). "Exact robot navigation using artificial potential functions." *IEEE Transactions on Robotics and Automation*, 8(5), 501–518. +- Choset, H. et al. (2005). *Principles of Robot Motion*. MIT Press. Chapter 4. diff --git a/doc/planning/README.md b/doc/planning/README.md index 4b1efca..00f3bee 100644 --- a/doc/planning/README.md +++ b/doc/planning/README.md @@ -7,3 +7,4 @@ Geometric queries and planners for collision-free robot motion. | Algorithm | Description | |----------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------| | [Primitive Distance Queries](PrimitiveDistance.md) | Closed-form closest points and signed distances between points, segments, spheres, capsules and oriented boxes — $O(1)$ | +| [Artificial Potential Fields](PotentialField.md) | Quadratic-then-conic attraction plus Khatib repulsion as a clamped velocity, with local-minimum detection — $O(M)$ | diff --git a/doc/trajectory/CartesianSlerpInterpolation.md b/doc/trajectory/CartesianSlerpInterpolation.md new file mode 100644 index 0000000..2694582 --- /dev/null +++ b/doc/trajectory/CartesianSlerpInterpolation.md @@ -0,0 +1,89 @@ +# Cartesian Path with SLERP Orientation + +## Overview & Motivation + +Many tool motions are specified in Cartesian space rather than joint space: move straight along a weld seam, insert a peg along its axis, approach a part without sweeping through the fixture. A joint-space spline between the end poses produces a curved, unpredictable tool path. +A **Cartesian path** keeps the tool on a straight line, while its orientation rotates steadily from the start attitude to the goal attitude. + +Orientation is the subtle part. Interpolating rotation matrices or Euler angles element-wise leaves the rotation group or produces uneven, gimbal-dependent motion. +**Spherical linear interpolation (SLERP)** of unit quaternions moves along the shortest great-circle arc at constant angular velocity about a fixed axis. Shoemake (1985) introduced it, and it is the standard way to blend attitudes. +Coupling position and orientation through one scalar time law makes both arrive together and gives the controller a consistent velocity feed-forward. + +## Mathematical Theory + +### Path Parameter + +A scalar progress $s(t) \in [0, 1]$, produced by any time law (cubic or quintic polynomial, trapezoidal, S-curve), drives both channels. Its rate $\dot{s}$ scales every velocity. + +### Position + +$$p(s) = p_0 + s\,(p_1 - p_0), \qquad v = \dot{s}\,(p_1 - p_0)$$ + +### Orientation + +With unit quaternions $q_0$, $q_1$ of the start and goal rotations, sign-fixed so that $q_0 \cdot q_1 \ge 0$ (both $q$ and $-q$ represent the same rotation, and the sign picks the short arc): + +$$q(s) = \frac{\sin\left((1-s)\Omega\right)}{\sin\Omega}\,q_0 + \frac{\sin(s\Omega)}{\sin\Omega}\,q_1, \qquad \cos\Omega = q_0 \cdot q_1$$ + +For nearly identical rotations ($\cos\Omega \to 1$) the formula is ill-conditioned, so a normalized linear blend replaces it. + +### Angular Velocity + +The relative rotation $R_1 R_0^T$ has rotation vector $\phi\,\hat{k}$ (axis $\hat{k}$, angle $\phi \le \pi$). SLERP rotates about that fixed axis at a rate proportional to $\dot{s}$, so the angular velocity in the base frame is + +$$\omega = \dot{s}\,\phi\,\hat{k}$$ + +The rotation vector is obtained from the relative quaternion $d = q_1 q_0^{*}$ as $\phi\,\hat{k} = 2\,\mathrm{atan2}(\|d_v\|, d_w)\,d_v/\|d_v\|$. This form is well-conditioned near $\phi \to 0$, where $\arccos$ loses precision, and it falls back to $2\,d_v$ as $\|d_v\| \to 0$. The twist $(v;\omega)$ is the feed-forward for task-space controllers. + +## Complexity Analysis + +| Phase | Time | Space | Notes | +|----------|--------|--------|----------------------------------------------------------------------------------------| +| Planning | $O(1)$ | $O(1)$ | Two matrix-to-quaternion conversions, one quaternion product, one atan2 | +| Sampling | $O(1)$ | $O(1)$ | One time-law sample, a vector blend, one SLERP and one quaternion-to-matrix conversion | + +## Step-by-Step Walkthrough + +From the identity attitude at the origin to a $90°$ rotation about $z$ at $(1, 0, 0)$, with a linear time law ($s = t$, $\dot{s} = 1$, duration $1$ s): + +1. $q_0 = (1, 0, 0, 0)$ and $q_1 = (\cos 45°, 0, 0, \sin 45°)$, so $q_0 \cdot q_1 > 0$ and no sign fix is needed. The relative rotation vector is $(0, 0, \pi/2)$. +2. At $t = 0.5$: the position is $(0.5, 0, 0)$, and SLERP gives $q = (\cos 22.5°, 0, 0, \sin 22.5°) = (0.92388, 0, 0, 0.38268)$, a $45°$ rotation. +3. The twist is constant: $v = (1, 0, 0)$ m/s and $\omega = (0, 0, \pi/2)$ rad/s. +4. With a goal of $270°$ about $z$, the quaternions have a negative dot product. After the sign fix the arc goes $-90°$, the midpoint is $-45°$ and $\omega = (0, 0, -\pi/2)$. +5. With a rest-to-rest quintic law over $2$ s, $\dot{s}(1) = 15/16 = 0.9375$, so the mid-path twist is $(0.9375, 0, 0;\ 0, 0, 1.4726)$, and both the velocity and the angular velocity vanish at the ends. + +## Pitfalls & Edge Cases + +- **Quaternion double cover**: without the sign fix, SLERP takes the long way round ($270°$ instead of $90°$), and the twist no longer matches the motion. +- **Twist consistency**: compute the rotation vector **after** the sign fix, from the same pair of quaternions that SLERP blends. +- **Near-identical orientations**: $\sin\Omega \to 0$ makes the SLERP weights ill-conditioned; use the normalized linear blend there. +- **$180°$ rotations**: both arcs are equally short and the choice is arbitrary but consistent; plan through an intermediate pose if a specific direction matters. +- **Joint-space feasibility**: a straight Cartesian line can pass through a singularity or outside the workspace even when both ends are reachable. Check the path with inverse kinematics and manipulability before execution. +- **Time-law limits act on $s$**: the Cartesian speed $|v| = \dot{s}|\Delta p|$ and angular speed $|\omega| = \dot{s}\phi$ follow from the limits on $\dot{s}$; scale them per segment. + +## Variants & Generalizations + +- **Screw (SE(3)) interpolation**: $T(s) = T_0 \exp(s \log(T_0^{-1} T_1))$ moves along a helix, coupling translation and rotation. It is natural for some tasks but not a straight line. +- **SQUAD and quaternion splines**: smooth blending through several attitudes with continuous angular velocity. +- **Via-point blending**: polynomial blends between consecutive Cartesian segments avoid stopping at each corner. +- **Separate time laws**: decoupled position and orientation schedules, with arrival synchronized by scaling the shorter one. + +## Applications + +- Straight-line tool moves: welding, gluing, insertion, approach and retreat. +- Camera and sensor pointing with smooth, constant-axis attitude changes. +- Feed-forward twists for operational-space and impedance control. +- Teleoperation and teaching pendants that move the tool in Cartesian jog modes. + +## Connections to Other Algorithms + +- [Cubic Spline Trajectory](CubicSplineTrajectory.md): the joint-space counterpart for via points. +- [Geometric Jacobian](../kinematics/GeometricJacobian.md): maps the Cartesian twist to joint rates, and its transpose maps task forces back to torques. +- [Inverse Kinematics](../kinematics/InverseKinematics.md): converts sampled poses into joint targets. +- The quaternion SLERP, rigid transforms and polynomial time laws come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Shoemake, K. (1985). "Animating rotation with quaternion curves." *Proc. SIGGRAPH*, 19(3), 245–254. +- Lynch, K.M. and Park, F.C. (2017). *Modern Robotics*. Cambridge University Press. Chapter 9. +- Biagiotti, L. and Melchiorri, C. (2008). *Trajectory Planning for Automatic Machines and Robots*. Springer. Chapter 8. diff --git a/doc/trajectory/README.md b/doc/trajectory/README.md index 986d4f4..dd1cc6c 100644 --- a/doc/trajectory/README.md +++ b/doc/trajectory/README.md @@ -4,6 +4,7 @@ Time-parameterized joint and Cartesian references for manipulators. ## Algorithms -| Algorithm | Description | -|-----------------------------------------------------|-----------------------------------------------------------------------------------------------------------------| -| [Cubic Spline Trajectory](CubicSplineTrajectory.md) | $C^2$ joint trajectory through timed via points, rest-to-rest by default — $O(nK)$ plan, $O(\log K + n)$ sample | +| Algorithm | Description | +|-------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------| +| [Cubic Spline Trajectory](CubicSplineTrajectory.md) | $C^2$ joint trajectory through timed via points, rest-to-rest by default — $O(nK)$ plan, $O(\log K + n)$ sample | +| [Cartesian Path with SLERP Orientation](CartesianSlerpInterpolation.md) | Straight-line position and shortest-arc quaternion SLERP on one time law, with twist feed-forward — $O(1)$ per sample | diff --git a/roadmap/README.md b/roadmap/README.md index 8185c6a..69c36d9 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -23,7 +23,7 @@ All pseudocode respects the library constraints: **no heap**, bounded containers (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. The canonical worked example is -[controllers/manipulator/ComputedTorqueControl](controllers/manipulator/ComputedTorqueControl/implementation.md). +[controllers/manipulator/ImpedanceControl](controllers/manipulator/ImpedanceControl/implementation.md). Conventions shared by every spec: 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches `(f; n)` — as fixed by upstream [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md) (NT53), and planar @@ -89,22 +89,22 @@ prerequisites. ## Index ### `kinematics` -`ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) +`PoseInverseKinematics` (M13) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) ### `dynamics` -`MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) +`MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CoriolisMatrixAndRegressor` (M31) ### `trajectory` -`CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) +`TimeOptimalPathParameterization` (M27) ### `mobile` `PlanarOdometry` (F1) · `DifferentialDrive` (W1) · `CarLikeKinematics` (W2) · `OmnidirectionalKinematics` (W3) · `SkidSteerKinematics` (W4) · `PurePursuit` (W5) · `StanleyController` (W6) · `KanayamaController` (W7) ### `planning` -`PotentialField` (P2) · `GridSearch` (P3) · `SelfCollision` (P4) · `ConvexCollision` (P5) · `RrtConnect` (P6) · `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) +`GridSearch` (P3) · `SelfCollision` (P4) · `ConvexCollision` (P5) · `RrtConnect` (P6) · `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) ### `localization` `OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) ### `controllers/manipulator` -`ComputedTorqueControl` (M12) · `ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) +`ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md b/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md deleted file mode 100644 index 067bc03..0000000 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# Computed-Torque Control — Overview - -## What it is -The workhorse model-based *tracking* law for manipulators. An inverse-dynamics feedforward plus a -PD correction linearises and decouples the arm into independent unit double integrators: -`τ = M(q)(q̈_d + Kd·ė + Kp·e) + C(q,q̇)q̇ + g(q)`. - -## Why it matters (embedded) -A fixed PID tuned at one posture misbehaves at another because a robot's inertia and gravity change -with configuration. Computed-torque erases that variation with a single inverse-dynamics call — one -recursive Newton–Euler pass evaluated at the commanded acceleration, which never forms the mass -matrix. A *single* gain set therefore tracks fast trajectories across the entire workspace — no gain -scheduling, no lookup tables — at a deterministic `O(n)` cost (with diagonal gains). - -## How it works (intuition) -Work out the acceleration you actually want: the desired trajectory acceleration plus a PD term that -corrects position and velocity error. Then ask the dynamics model, "what joint torque produces -exactly that acceleration *right now*?" The answer, `M(q)·a + C(q,q̇)q̇ + g(q)`, cancels the arm's -inertial coupling, Coriolis, and gravity, leaving clean, identical second-order error dynamics on -every joint. - -## Key parameters -- **model** — injected inverse-dynamics model returning `M(q)q̈ + C(q,q̇)q̇ + g(q)` in one call - (`ChainDynamicsModel`, M29). -- **Kp, Kd** — error gains for the linearised double integrator; pick `Kd = 2√Kp` for critical damping. - -## Reference -M. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 8; Luh, Walker, Paul (1980). - -## See also -`PdGravityCompensation` (set-point-only special case); `SlotineLiAdaptiveControl` (adapts unknown -parameters); `ChainDynamicsModel` (M29, the one-pass RNEA model); general-plant feedback linearization in -[numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md b/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md deleted file mode 100644 index 534c6d1..0000000 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/implementation.md +++ /dev/null @@ -1,78 +0,0 @@ -# Computed-Torque Control — Implementation Pseudocode - -> Roadmap ref: #M12 (Tier 3) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class ComputedTorqueControl: - const dynamics::InverseDynamicsModel& model # τ = M(q)q̈ + C(q,q̇)q̇ + g(q) in one call (M29) - math::SquareMatrix Kp # position-error gain - math::SquareMatrix Kd # velocity-error gain -``` - -## Interface - -```cpp -# Inverse-dynamics model injected (DIP); gains chosen for the resulting double integrator: -ComputedTorqueControl(const dynamics::InverseDynamicsModel& model, - const SquareMatrix& Kp, const SquareMatrix& Kd) - -Vector ComputeTorque(const StateVector& q, const StateVector& qDot, - const StateVector& qd, const StateVector& qdDot, - const StateVector& qdDdot) # hot path -``` - -## Algorithm (pseudocode) - -```text -function ComputeTorque(q, qDot, qd, qdDot, qdDdot): # OPTIMIZE_FOR_SPEED - e = qd - q - eDot = qdDot - qDot - # inner-loop joint-space acceleration command (feedforward + PD): - aq = qdDdot + Kd * eDot + Kp * e - # inverse dynamics at the measured state and the commanded acceleration: - # τ = M(q)·aq + C(q,q̇)q̇ + g(q) — ONE O(n) RNEA pass, M is never formed - return model.ComputeInverseDynamics(q, qDot, aq) -``` - -## Complexity & memory - -- Time: one `ComputeInverseDynamics` call — `O(Dof)` with `ChainDynamicsModel` (single RNEA pass) — - plus `O(Dof²)` for the gain products (`O(Dof)` when the gains are diagonal). No `Dof×Dof` mass matrix - is built or multiplied. -- Memory: `O(Dof²)` for the two gains; no dynamic state, no heap. - -## Numerical / embedded notes - -- Substituting `τ` into `M q̈ + Cq̇ + g = τ` gives the **decoupled** linear error dynamics - `ë + Kd·ė + Kp·e = 0` — every joint becomes an independent, tunable second-order system. -- `M(q)` is neither formed nor inverted: RNEA evaluated with `q̈ = aq` returns `M·aq + Cq̇ + g` - directly, so the hot path is `O(n)` and well-conditioned (unlike forward dynamics). -- The model is `dynamics::InverseDynamicsModel` (M29), implemented by `ChainDynamicsModel`. The interface - is a virtual seam for StrictMock tests; hard real-time builds may template the controller on the - concrete `ChainDynamicsModel` to avoid the virtual call (AGENTS.md: no virtual calls in real-time paths). -- Model error leaves a residual (`ë + Kd·ė + Kp·e = M⁻¹Δ`); pair with an integral, robust - (sliding-mode), or adaptive (`SlotineLiAdaptiveControl`) term to reject it. -- Choose `Kd = 2√Kp` per channel for a critically-damped response. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `robotics/controllers/manipulator/ComputedTorqueControl.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class ComputedTorqueControl;` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/controllers/manipulator/ComputedTorqueControl.cpp` → - `template class ComputedTorqueControl;` / `` -- Test: `robotics/controllers/manipulator/test/TestComputedTorqueControl.cpp` -- Doc: `doc/controllers/manipulator/ComputedTorqueControl.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestComputedTorqueControl.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, - add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a - `doc/controllers/manipulator/` folder. -- Depends on: M29 (`InverseDynamicsModel`, `ChainDynamicsModel`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md b/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md deleted file mode 100644 index 17ac4ba..0000000 --- a/roadmap/controllers/manipulator/ComputedTorqueControl/tests.md +++ /dev/null @@ -1,54 +0,0 @@ -# Computed-Torque Control — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestComputedTorqueControl : public ::testing::Test: - StrictMock> model - SquareMatrix Kp = diag(100, 100) - SquareMatrix Kd = diag( 20, 20) - ComputedTorqueControl controller{ model, Kp, Kd } -# each case below is a TEST_F(TestComputedTorqueControl, ) -# single-step cases set EXPECT_CALL(model, ComputeInverseDynamics(q, qDot, aqExpected)).Times(1) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -pure_feedforward_passes_desired_acceleration: - Arrange: q = qd, qDot = qdDot, qdDdot = a; model returns r - Act: τ = ComputeTorque(q, qDot, qd, qdDot, qdDdot) - Assert: called once with (q, qDot, a); τ == r (model output returned unchanged) - -position_error_enters_command: - Arrange: e = qd − q != 0, eDot = 0, qdDdot = 0 - Assert: called once with aq == Kp · e - -velocity_error_enters_command: - Arrange: eDot = qdDot − qDot != 0, e = 0, qdDdot = 0 - Assert: called once with aq == Kd · eDot - -full_command_superposition: - Arrange: e, eDot, qdDdot all nonzero - Assert: called once with aq == qdDdot + Kd·eDot + Kp·e, and with the MEASURED q, qDot - -decoupled_error_dynamics: - Arrange: 2-link plant M(q)q̈ + C(q,q̇)q̇ + g(q) = τ; mock Invoke returns M(q)·aq + C(q,q̇)q̇ + g(q) - of the same plant (exact model); run K steps from e(0) != 0 - Assert: e(t) matches the solution of ë + Kd·ė + Kp·e = 0 (same integrator), ||e|| -> 0 -``` - -## Reference vectors - -- With the exact model, closed-loop error obeys `ë + Kd·ė + Kp·e = 0` — for `Kp = 100`, `Kd = 20` - critically damped: `e(t) = (e₀ + (ė₀ + 10e₀)t)·e^{−10t}` per joint. -- `e = [0.1, −0.2]`, `ė = 0`, `q̈d = 0` ⇒ expected `aq = [10, −20]` passed to the model. - -## Edge cases - -- Only `ComputeInverseDynamics` exists on the mock — no mass-matrix query is possible (StrictMock - fails on any unexpected call), pinning the single `O(n)` call. -- Model mismatch (plant `M` scaled 1.2 vs the mocked model): bounded tracking error, closed loop stays stable. -- Large `qdDdot`: torque stays within the documented actuator model. diff --git a/roadmap/dynamics/RneaExternalWrench/explanation.md b/roadmap/dynamics/RneaExternalWrench/explanation.md deleted file mode 100644 index 99b553b..0000000 --- a/roadmap/dynamics/RneaExternalWrench/explanation.md +++ /dev/null @@ -1,28 +0,0 @@ -# Inverse Dynamics with an External Tool Wrench — Overview - -## What it is -Inverse dynamics that also accounts for a force and moment applied to the tool by the environment — -a payload's weight, a contact force, a tool reaction. - -## Why it matters (embedded) -Force-controlled tasks and payload handling need torques that include the contact wrench, and -collision observers need a model to compare against. Adding the wrench inside the recursion keeps -the cost linear and avoids building the Jacobian just to map one wrench. - -## How it works (intuition) -The backward pass of the recursive Newton-Euler algorithm accumulates, from the tip inward, the -wrench each joint must transmit. An external wrench on the last link simply offsets that -accumulation at the tip: the environment already supplies part of what the joints would otherwise -have to provide. - -## Key parameters -- **Wrench** — force and moment on the tool, in the base frame, with a fixed sign convention. -- **Tool offset** — where the wrench acts, in the last link's frame. - -## Reference -J. Y. S. Luh, M. W. Walker, R. P. C. Paul, "On-Line Computational Scheme for Mechanical Manipulators," -*ASME J. Dyn. Sys. Meas. Control*, 102(2), 1980; B. Siciliano et al., *Robotics* (2009), Ch. 7. - -## See also -`RecursiveNewtonEuler`, `GeometricJacobian` (M8, `Jᵀw` equivalence), `ImpedanceControl` (M17), -`HybridPositionForceControl` (M19), `MomentumObserver` (M16). diff --git a/roadmap/dynamics/RneaExternalWrench/implementation.md b/roadmap/dynamics/RneaExternalWrench/implementation.md deleted file mode 100644 index fb1881d..0000000 --- a/roadmap/dynamics/RneaExternalWrench/implementation.md +++ /dev/null @@ -1,63 +0,0 @@ -# Inverse Dynamics with an External Tool Wrench — Implementation Pseudocode - -> Roadmap ref: #M32 (Tier 1) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Extends the shipped `RecursiveNewtonEuler` so contact forces at the tool enter inverse dynamics: -`τ = M q̈ + C q̇ + g − Jᵀ w_ext`. Needed by force control (M17, M19), by payload/contact estimation, -and to cross-check the momentum observer (M16). - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct ToolWrench: - Vector3 force # f, exerted BY the environment ON the tool, base frame - Vector3 moment # n, about the tool point, base frame - Vector3 toolOffset # tool point in the last link frame (same as FK) -``` - -## Interface - -```text -# new overload next to the shipped InverseDynamics(links, q, q̇, q̈, gravity): -JointVector InverseDynamics(const LinkArray& links, const JointVector& q, const JointVector& qDot, - const JointVector& qDDot, const Vector3& gravity, - const ToolWrench& wrench) const # hot path -``` - -## Algorithm (pseudocode) - -```text -function InverseDynamics(links, q, q̇, q̈, g, w): # OPTIMIZE_FOR_SPEED - states = ComputeForwardPass(links, q, q̇, q̈, g) # unchanged - Rlast = R_0 · R_1 ··· R_{N−1} # base ← last link (product of states[i].R) - fExt = Rlastᵀ · w.force # into the last link frame - nExt = Rlastᵀ · w.moment + CrossProduct(w.toolOffset, fExt) # moment about the last joint origin - # backward pass: the environment pushes on the last link, so the joints need less wrench - force[N−1] −= fExt - torque[N−1] −= nExt - ... continue the shipped backward pass unchanged ... - return τ -``` - -## Complexity & memory - -- `O(N)`; one extra rotation product chain (already available from the forward pass) and two cross - products. No additional storage. - -## Numerical / embedded notes - -- Sign convention (normative): `w` is the wrench the environment applies to the robot. Holding a - 0.5 kg payload with gravity `(0, 0, −9.81)` is `w.force = (0, 0, −4.905)`. -- Equivalence to the Jacobian-transpose form: `τ(w) − τ(0) = −Jᵀ·(f; n)` with the geometric - Jacobian (M8) at the same tool point — the key test. -- Orders `(f; n)` linear first, as everywhere (NT53). -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Extend `robotics/dynamics/RecursiveNewtonEuler.hpp` (overload) and add `ToolWrench` to it; the - coverage `.cpp` already instantiates ``. -- Test: new cases in `robotics/dynamics/test/TestRecursiveNewtonEuler.cpp` -- Doc: extend `doc/dynamics/RecursiveNewtonEuler.md` (external forces section). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/RneaExternalWrench/tests.md b/roadmap/dynamics/RneaExternalWrench/tests.md deleted file mode 100644 index 2abf8cb..0000000 --- a/roadmap/dynamics/RneaExternalWrench/tests.md +++ /dev/null @@ -1,36 +0,0 @@ -# Inverse Dynamics with an External Tool Wrench — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestRecursiveNewtonEuler : public ::testing::Test: # extends the shipped fixture - RecursiveNewtonEuler rnea3 - std::array, 3> chain{ ... } # skewed axes, tool offset (0.3, 0.05, 0) -# each case below is a TEST_F(TestRecursiveNewtonEuler, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -zero_wrench_matches_shipped_overload: - Assert: InverseDynamics(..., ToolWrench{0, 0, tool}) == InverseDynamics(...) -wrench_enters_as_minus_jacobian_transpose: - Arrange: w = ((1, −2, 0.5); (0.1, 0, −0.2)), J from GeometricJacobian (M8) at the same tool point - Assert: τ(w) − τ(0) ≈ −Jᵀ·w -horizontal_link_holding_a_payload: - Arrange: single y-axis rod length l, payload force (0, 0, −m_p·g) at the tip, static - Assert: τ = −(m l/2 + m_p l)·g (both moments add) -pure_moment_projects_on_the_axis: - Arrange: single z-axis link, w.moment = (0, 0, 2) - Assert: τ = −2 -``` - -## Reference vectors - -- Rod `m = 1, l = 1`, payload `0.5 kg`: `τ = −(0.5 + 0.5)·9.81 = −9.81 N·m`. - -## Edge cases - -- Tool offset zero ⇒ the force acts at the last joint origin (no moment arm from the offset). diff --git a/roadmap/kinematics/ManipulabilityIndex/explanation.md b/roadmap/kinematics/ManipulabilityIndex/explanation.md deleted file mode 100644 index f067812..0000000 --- a/roadmap/kinematics/ManipulabilityIndex/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Manipulability Index — Overview - -## What it is -A single number that scores how well an arm can move in its current posture. Yoshikawa's measure is -the volume of the "velocity ellipsoid" — the set of tool velocities reachable with unit-norm joint -rates. Big means dexterous; zero means the arm is at a singularity. Its companions are the ellipsoid's -axis lengths (the singular values) and their ratio (the condition number). - -## Why it matters (embedded) -Singularities are where Jacobian-based controllers blow up: joint rates rocket toward infinity for a -finite tool motion. A cheap scalar that flags "you are getting close" lets a real-time controller slow -down, raise damping in its inverse, or switch strategy *before* the actuators saturate. It is also the -objective a redundant arm maximizes in its null space. - -## How it works (intuition) -Map the unit sphere of joint velocities through the Jacobian and it becomes an ellipsoid; its axes are -the singular values and its volume their product. If there are at least as many joints as measured -task directions, the volume is `√det(J·Jᵀ)`. If there are fewer joints — a two-joint planar arm measured -in 3-D position — the ellipsoid is flat in the task space, `det(J·Jᵀ)` is always zero, and the -meaningful volume is that of the lower-dimensional ellipsoid, `√det(Jᵀ·J)`. Linear and angular rows -have different units, so for full-pose tasks the translational and rotational volumes are reported -separately rather than multiplied into one unit-dependent number. - -## Key parameters -- **Jacobian provider and task dimension** — which rows are measured (position only, planar, or full pose). -- **singularity threshold `eps`** — below which `NearSingular` trips. - -## Reference -T. Yoshikawa, "Manipulability of Robotic Mechanisms," *Int. J. Robotics Research*, 4(2), 1985. - -## See also -`GeometricJacobian` / `JacobianProvider` (M8, the input), `SingularValueDecomposition` and -`LuDecomposition` ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)), -`PoseInverseKinematics` (M13, adaptive damping), `RedundancyResolution` (M14, a null-space objective). diff --git a/roadmap/kinematics/ManipulabilityIndex/implementation.md b/roadmap/kinematics/ManipulabilityIndex/implementation.md deleted file mode 100644 index f8a86aa..0000000 --- a/roadmap/kinematics/ManipulabilityIndex/implementation.md +++ /dev/null @@ -1,97 +0,0 @@ -# Manipulability Index (Yoshikawa) — Implementation Pseudocode - -> Roadmap ref: #M11 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class ManipulabilityIndex: - const JacobianProvider& jacobian # M8 seam, injected - # measured blocks share one physical unit: - static constexpr std::size_t Rows = (TaskDim == 6) ? 3 : TaskDim # TaskDim = 6 → linear / angular blocks - static constexpr std::size_t Axes = min(Rows, Dof) -``` - -## Interface - -```cpp -explicit ManipulabilityIndex(const JacobianProvider& jacobian) -T Compute(const JointVector& q) const # w of rows 0..Rows−1 (translational for TaskDim 3/6); hot path -T ComputeRotational(const JointVector& q) const requires (TaskDim == 6) # rows 3–5 -std::array EllipsoidAxes(const JointVector& q) const # singular values of the Compute block, descending -T ConditionNumber(const JointVector& q) const # σ_max / σ_min of that block; +∞ when σ_min ≤ ε -bool NearSingular(const JointVector& q, T eps) const # Compute(q) < eps -``` - -## Algorithm (pseudocode) - -```text -function GramRoot(B): # B: Rows × Dof, one unit - if Rows ≤ Dof: G = B·Bᵀ # Rows × Rows: task-space ellipsoid volume - else: G = Bᵀ·B # Dof × Dof: B·Bᵀ would be rank ≤ Dof ⇒ det ≡ 0 - lu = solvers::LuDecomposition - if !lu.Decompose(G): return 0 # upstream flags pivot < 1e-6·max ⇒ σ_min/σ_max ≲ 1e-3 - return sqrt(max(0, lu.Determinant())) # clamp −0 from rounding - -function Compute(q): # OPTIMIZE_FOR_SPEED - J = jacobian.Jacobian(q) # TaskDim × Dof - return GramRoot(J.rows(0 .. Rows−1)) - -function ComputeRotational(q): # TaskDim = 6 - return GramRoot(jacobian.Jacobian(q).rows(3 .. 5)) - -function EllipsoidAxes(q): - B = jacobian.Jacobian(q).rows(0 .. Rows−1) - svd = solvers::SingularValueDecomposition # tall or wide; Axes = SingularValueCount - svd.Decompose(B) - return svd.SingularValues() # descending; ∏ σᵢ = GramRoot(B) - -function ConditionNumber(q): - σ = EllipsoidAxes(q) - return σ_min ≤ ε·σ_max ? +∞ : σ_max / σ_min # upstream ConditionNumber() returns 0 when singular — map to +∞ - -function NearSingular(q, eps): - return Compute(q) < eps -``` - -## Complexity & memory - -- `Compute`: `O(Rows·Dof·min(Rows, Dof))` for the Gram product + `O(min(Rows, Dof)³)` LU. -- `EllipsoidAxes` / `ConditionNumber`: one Golub–Kahan SVD of the `Rows × Dof` block (a wide block is - factored through its transpose upstream, at the same cost). -- Memory: one `TaskDim×Dof` Jacobian, one Gram matrix or SVD factors; stack only. - -## Numerical / embedded notes - -- **Branch choice matters:** with fewer joints than measured rows (`Dof < Rows`), `det(B·Bᵀ)` is - identically zero; `√det(Bᵀ·B)` is the volume of the `Dof`-dimensional ellipsoid actually reachable. - For the 2-link planar arm with position rows this gives `|sin q₂|`. -- **Units:** linear rows are in m/s per unit rate, angular rows in rad/s. For `TaskDim = 6` the two - blocks are measured separately; a single 6-row determinant mixes units and depends on the length unit. -- A planar arm with `TaskDim = 3` and `Dof ≥ 3` has a zero `z` row ⇒ `w ≡ 0`; use a `TaskDim = 2` - provider (planar position rows) for planar tasks. -- `det(G) = (∏ σᵢ)²`; when conditioning or axes are needed use the SVD (upstream - `numerical/solvers/SingularValueDecomposition.hpp`, - [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)) — it avoids the - squaring of `G` (the Gram/LU path reports `0` once `σ_min/σ_max ≲ 1e-3`; `∏ EllipsoidAxes` stays - accurate below that). For `Rows = Dof`, `w = |det B|` directly. -- Use `NearSingular` as a guard *before* any Jacobian inverse. -- `ComputeRotational` is constrained with `requires` (C++20), not `static_assert`, so explicit - coverage instantiations with `TaskDim ≠ 6` still compile. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/ManipulabilityIndex.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class ManipulabilityIndex;` / `` / `` / - `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/ManipulabilityIndex.cpp` → the same instantiations. -- Test: `robotics/kinematics/test/TestManipulabilityIndex.cpp` -- Doc: `doc/kinematics/ManipulabilityIndex.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestManipulabilityIndex.cpp` → the `_test` target. -- Depends on: M8 (`JacobianProvider`); upstream `solvers::SingularValueDecomposition` (wide-matrix - support; needs a numerical-toolbox pin that includes it), `solvers::LuDecomposition`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ManipulabilityIndex/tests.md b/roadmap/kinematics/ManipulabilityIndex/tests.md deleted file mode 100644 index 9256974..0000000 --- a/roadmap/kinematics/ManipulabilityIndex/tests.md +++ /dev/null @@ -1,56 +0,0 @@ -# Manipulability Index — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestManipulabilityIndex : public ::testing::Test: - # 2-link unit planar arm (z-axis joints, link lengths 1), link model M30 - std::array, 2> planar{ ... } - ChainTaskJacobian position{ planar, SE3Transform{ I, (1, 0, 0) } } - ChainTaskJacobian full{ planar, SE3Transform{ I, (1, 0, 0) } } - ManipulabilityIndex w{ position } - StrictMock> planarTask # 3-link planar arm, xy rows - ManipulabilityIndex wRedundant{ planarTask } -# each case below is a TEST_F(TestManipulabilityIndex, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -position_manipulability_is_abs_sin_elbow: - Assert: w.Compute({0, q₂}) ≈ |sin q₂| for q₂ ∈ {0.3, 1.2, 2.5} (Dof < Rows ⇒ Jᵀ·J branch) -right_angle_elbow_is_most_dexterous: - Assert: w.Compute({0, π/2}) ≈ 1 and > w.Compute({0, π/2 ± 0.1}) -stretched_and_folded_arm_are_singular: - Assert: w.Compute({0, 0}) ≈ 0 and w.Compute({0, π}) ≈ 0 -redundant_task_uses_j_jt_branch: - Arrange: EXPECT_CALL(planarTask, Jacobian) returns [[−1, −1, 0], [0, −1, −1]] # q = (0, π/2, π/2) - Assert: wRedundant.Compute(q) ≈ √3 = 1.732051; EllipsoidAxes ≈ (1.732051, 1) -six_row_task_measures_units_separately: - Arrange: ManipulabilityIndex{ full } - Assert: Compute({0, 1.2}) ≈ 0.932039 (= |sin 1.2|); ComputeRotational({0, 1.2}) ≈ 0 (parallel axes) -ellipsoid_axes_are_sorted_singular_values: - Assert: w.EllipsoidAxes({0, π/2}) ≈ (1.618034, 0.618034); product ≈ w.Compute({0, π/2}) -condition_number_grows_near_singularity: - Assert: ConditionNumber({0, π/2}) ≈ 2.618034; ({0, 0.1}) ≈ 49.963; ({0, 0.01}) ≈ 499.996 (rel tol 1e-3) -exactly_singular_condition_number_is_infinite: - Assert: ConditionNumber({0, 0}) == +∞ -near_singular_flag_trips: - Assert: NearSingular({0, 0.01}, 0.05) == true; NearSingular({0, π/2}, 0.05) == false -``` - -## Reference vectors - -- 2-link unit arm, position rows: `w(q₂) = |sin q₂|` — `0.295520` (0.3), `0.932039` (1.2), `1` (π/2), - `0.598472` (2.5), `0` at `{0, π}`. The full 6-row `√det(J·Jᵀ)` is `0` for every `q` (rank ≤ 2). -- `q = (0, π/2)`: `σ = (φ, 1/φ) = (1.618034, 0.618034)`, `κ = φ² = 2.618034`. -- 3-link unit planar arm, `q = (0, π/2, π/2)`: `J_xy = [[−1,−1,0],[0,−1,−1]]`, `J_xy·J_xyᵀ = [[2,1],[1,2]]` - ⇒ `w = √3`, `σ = (√3, 1)`. - -## Edge cases - -- `Rows = Dof` ⇒ `w = |det B|`. -- Planar arm with `TaskDim = 3`, `Dof ≥ 3` ⇒ zero `z` row, `w ≡ 0` (documented, use `TaskDim = 2`). -- Tiny `w` below `float` epsilon ⇒ `NearSingular` still deterministic; `Determinant` clamped at 0. diff --git a/roadmap/kinematics/RedundancyResolution/explanation.md b/roadmap/kinematics/RedundancyResolution/explanation.md deleted file mode 100644 index 6e9317c..0000000 --- a/roadmap/kinematics/RedundancyResolution/explanation.md +++ /dev/null @@ -1,37 +0,0 @@ -# Redundancy Resolution — Overview - -## What it is -A way to use a robot that has *more* joints than the task needs. A 7-DOF arm holding a 6-DOF pose — or -any arm tracking only a 3-D position — has spare freedom; redundancy resolution splits the joint motion -into a **primary** part that achieves the task and a **secondary** part, living in the Jacobian's null -space, that pursues an extra goal without disturbing the tool. - -## Why it matters (embedded) -Extra joints let a robot dodge its own joint limits, avoid obstacles, or stay away from singularities -*while still doing the job*. The formula `q̇ = J⁺ẋ + (I − J⁺J)q̇₀` is cheap enough to run every control -tick — the difference between an arm that jams at a joint stop and one that reconfigures around it. - -## How it works (intuition) -Factor the Jacobian into singular directions. The primary term inverts it along each direction it can -move the tool in — with a little damping so that a nearly-lost direction does not demand huge joint -rates — which gives the smallest joint motion producing (almost exactly) the desired tool velocity. -The projector removes from any secondary joint velocity every component the tool can feel; it must be -built from the *undamped*, rank-thresholded factorization, because a damped projector leaks a small -amount of motion into the task. Pick a secondary velocity — typically the downhill direction of a cost -such as distance to joint limits — project it, and add it on: the tool does not notice, and the spare -freedom does useful work. - -## Key parameters -- **task rate `ẋ`** and **task dimension** (position only or full pose). -- **secondary rate `q̇₀`** — gradient of the objective optimized in the null space. -- **damping λ** — singularity robustness of the primary term (task error `≈ λ²/σ_min²`). -- **rank tolerance** — which singular values count as lost directions. - -## Reference -A. Liégeois, "Automatic Supervisory Control of the Configuration and Behavior of Multibody -Mechanisms," *IEEE Trans. Systems, Man, and Cybernetics*, 7(12), 1977; S. Chiaverini, "Singularity-Robust -Task-Priority Redundancy Resolution," *IEEE Trans. Robotics and Automation*, 13(3), 1997. - -## See also -`JacobianProvider` (M8, supplies `J`), upstream `DampedPseudoInverse` (NT62, the damped inverse and -projector), `ManipulabilityIndex` (M11, a common secondary objective), `PoseInverseKinematics` (M13). diff --git a/roadmap/kinematics/RedundancyResolution/implementation.md b/roadmap/kinematics/RedundancyResolution/implementation.md deleted file mode 100644 index a94c850..0000000 --- a/roadmap/kinematics/RedundancyResolution/implementation.md +++ /dev/null @@ -1,68 +0,0 @@ -# Redundancy Resolution (Null-Space Projection) — Implementation Pseudocode - -> Roadmap ref: #M14 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -`q̇ = J⁺_λ·ẋ + (I − J⁺J)·q̇₀` for a redundant arm. The linear algebra — damped pseudo-inverse and exact -null-space projector from one SVD, tall or wide — is upstream `solvers::DampedPseudoInverse` (NT62); this -item binds it to the arm's task Jacobian. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); static_assert(Dof > TaskDim) -class RedundancyResolution: # instantiated for float - const JacobianProvider& jacobian # M8 seam; TaskDim 3 (position) or 6 (pose) - solvers::DampedPseudoInverse inverse # NT62: damping λ (primary only), rank tolerance -``` - -## Interface - -```text -RedundancyResolution(const JacobianProvider& jacobian, T damping, T rankTolerance = 1e-4) -JointVector Resolve(const JointVector& q, const TaskVector& xDot, - const JointVector& qDot0) # hot path -Matrix DampedPseudoInverse(const JointVector& q) # J⁺_λ (λ = 0 ⇒ rank-thresholded J⁺) -Matrix NullSpaceProjector(const JointVector& q) # P = I − J⁺J, undamped -``` - -## Algorithm (pseudocode) - -```cpp -function Resolve(q, ẋ, q̇₀): # OPTIMIZE_FOR_SPEED - inverse.Factor(jacobian.Jacobian(q)) # one SVD of the TaskDim × Dof Jacobian - return inverse.Solve(ẋ) + inverse.Project(q̇₀) # primary (row space) + secondary (null space) - -function DampedPseudoInverse(q): inverse.Factor(jacobian.Jacobian(q)); return inverse.Inverse() -function NullSpaceProjector(q): inverse.Factor(jacobian.Jacobian(q)); return inverse.NullSpaceProjector() -``` - -## Complexity & memory - -- One SVD of the `TaskDim × Dof` Jacobian per call plus `O(Dof·TaskDim)` (upstream); the diagnostic - accessors add `O(Dof²·r)`; `Resolve` never forms `P`. -- Memory: the upstream SVD factors; stack only. - -## Numerical / embedded notes - -- **What the tool sees:** the secondary term adds nothing to the task velocity regardless of `λ` or - `‖q̇₀‖`, because the projector is built undamped from the thresholded SVD (the damped `I − J⁺_λJ` would - leak: 7-DOF fixture, `λ = 0.01`: `‖J·P_λ·q̇₀‖ = 2.8e-4`). With `λ > 0` the primary task is met only - approximately: `‖J·q̇ − ẋ‖ ≤ λ²/(σ_min² + λ²)·‖ẋ‖`. -- Near a singularity `σ_min → 0` the damped gain stays bounded (`≤ 1/(2λ)`) and the rank threshold grows - the null space (the lost task direction is not "protected" — by design). -- Common `q̇₀`: gradient of manipulability (M11), distance to joint limits, obstacle potentials. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/RedundancyResolution.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Resolve`, and - `extern template class RedundancyResolution;` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/RedundancyResolution.cpp` → the same instantiations. -- Test: `robotics/kinematics/test/TestRedundancyResolution.cpp` -- Doc: `doc/kinematics/RedundancyResolution.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestRedundancyResolution.cpp` → the `_test` target. -- Depends on: M8 (`JacobianProvider`); NT62 (`solvers::DampedPseudoInverse`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/RedundancyResolution/tests.md b/roadmap/kinematics/RedundancyResolution/tests.md deleted file mode 100644 index b505905..0000000 --- a/roadmap/kinematics/RedundancyResolution/tests.md +++ /dev/null @@ -1,58 +0,0 @@ -# Redundancy Resolution — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -Projector algebra (idempotence, symmetry, right inverse, minimum norm, damping bound) is tested upstream -with `solvers::DampedPseudoInverse` (NT62). These cases cover the binding to a real redundant arm. - -## Fixture - -```cpp -class TestRedundancyResolution : public ::testing::Test: - # 7R arm (iiwa-like), standard DH (a, α, d): (0,−π/2,0.34) (0,π/2,0) (0,π/2,0.4) (0,−π/2,0) - # (0,−π/2,0.4) (0,π/2,0) (0,0,0.126) - DenavitHartenberg arm{ iiwaTable, Standard } - DhTaskJacobian pose{ arm } - DhTaskJacobian position{ arm } - JointVector q { 0.1, 0.5, −0.3, −1.2, 0.4, 0.8, −0.2 } - TaskVector xDot{ 0.1, −0.05, 0.02, 0.1, 0.2, −0.1 } - JointVector qDot0{ 1, −1, 0.5, 0.3, −0.2, 0.7, −0.4 } -# each case below is a TEST_F(TestRedundancyResolution, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -undamped_primary_task_is_exact: - Arrange: RedundancyResolution{ pose, λ = 0 } - Assert: pose.Jacobian(q)·Resolve(q, xDot, qDot0) ≈ xDot (tol 1e-5) -damped_resolution_matches_reference: - Arrange: λ = 0.01 - Assert: Resolve(q, xDot, qDot0) ≈ (−0.093934, 0.325324, −0.092001, 0.551001, 0.088089, 0.448837, 0.140035) -null_space_motion_is_task_invisible_despite_damping: - Arrange: λ = 0.01 - Assert: J·NullSpaceProjector(q)·qDot0 ≈ 0 (tol 1e-5) -secondary_objective_reduces_cost_without_moving_the_tool: - Arrange: H(q) = ½Σ(qᵢ/2.9)²; qDot0 = −∇H; xDot = 0; λ = 0 - Act: q̇ = Resolve(q, 0, qDot0) - Assert: ∇H·q̇ ≈ −0.0051809 (< 0); ‖J·q̇‖ ≈ 0 -position_task_has_four_dimensional_null_space: - Arrange: RedundancyResolution{ position, λ = 0 } - Assert: trace NullSpaceProjector(q) ≈ 4; J_pos·P ≈ 0 -rank_loss_grows_the_null_space: - Arrange: stretched q_s = (0.1, 0, −0.3, 0, 0.4, 0, −0.2) (σ = 2, 1.950561, 0.498565, 0.311084, 0, 0) - Assert: trace P ≈ 3; J(q_s)·P ≈ 0; Resolve finite with λ = 0.01 -``` - -## Reference vectors - -- 7R fixture at `q`: `σ(J) = (1.851950, 1.718255, 1.317197, 0.446306, 0.267222, 0.178251)`. -- `λ = 0`: `Resolve(q, xDot, qDot0) = (−0.094193, 0.326285, −0.092053, 0.552967, 0.088112, 0.449943, 0.140364)`. -- `λ = 0.01`: `Resolve(q, xDot, qDot0) = (−0.093934, 0.325324, −0.092001, 0.551001, 0.088089, 0.448837, 0.140035)`, - `‖Jq̇ − ẋ‖ = 4.44e-4`, `‖J·P·q̇₀‖` at rounding level (python3/numpy, standard DH). -- Position rows only: `σ = (0.839288, 0.805624, 0.257020)`, `trace P = 4`. - -## Edge cases - -- `Dof = TaskDim` rejected at compile time (`static_assert(Dof > TaskDim)`); use M13 instead. -- Conflicting or over-scaled `q̇₀` ⇒ primary task unaffected (projector exact); only the secondary objective suffers. diff --git a/roadmap/planning/PotentialField/explanation.md b/roadmap/planning/PotentialField/explanation.md deleted file mode 100644 index 6ddabd6..0000000 --- a/roadmap/planning/PotentialField/explanation.md +++ /dev/null @@ -1,30 +0,0 @@ -# Artificial Potential Fields — Overview - -## What it is -A reactive way to move a robot towards a goal while staying clear of obstacles. The goal attracts like a -spring, each nearby obstacle pushes back harder the closer it is, and the robot follows the sum of these -pushes and pulls. - -## Why it matters (embedded) -Potential fields need no map search and no planning horizon: one evaluation per control cycle, a few -operations per nearby obstacle. That makes them a common last-line avoidance layer under a global planner, -on drones, mobile bases and manipulators alike, and easy to run on a microcontroller. - -## How it works (intuition) -The attraction grows with distance to the goal up to a cap, so far goals do not overpower everything -else. Repulsion only starts inside an influence radius and rises steeply as the gap closes. The -resulting velocity is clamped to a safe speed. When an obstacle sits exactly between robot and goal the two -effects can cancel, and the robot stops in a local minimum; the method detects and reports this, so a -higher-level planner can supply a detour. - -## Key parameters -- **attractive gain** and **conic distance** — pull strength and where it stops growing. -- **repulsive gain** and **influence distance** — how hard and from how far obstacles push. -- **maximum speed**, **stall speed** and **goal tolerance**. - -## Reference -O. Khatib, "Real-time obstacle avoidance for manipulators and mobile robots," *IJRR*, 5(1), 1986. - -## See also -`PrimitiveDistance` (P1, obstacle proximity), `GridSearch` (P3) and `RrtConnect` (P6, global plans that escape -local minima). diff --git a/roadmap/planning/PotentialField/implementation.md b/roadmap/planning/PotentialField/implementation.md deleted file mode 100644 index 390a3ed..0000000 --- a/roadmap/planning/PotentialField/implementation.md +++ /dev/null @@ -1,95 +0,0 @@ -# Artificial Potential Fields — Implementation Pseudocode - -> Roadmap ref: #P2 (★★☆☆☆) · Target: `robotics/planning` · Namespace `planning` · Type: `float` (templated on `T`, instantiated for `float` only) - -Reactive motion as gradient descent on an attractive goal potential plus repulsive obstacle potentials -(Khatib, 1986). The attractive term is quadratic near the goal and conic (bounded) farther away. The -repulsive term acts only inside an influence distance and grows without bound at contact. Obstacles come -in as proximity readings (distance plus a direction away from the obstacle), typically from P1/P5 -queries against the robot's control point. The field's well-known weakness, local minima, is detected and -reported rather than hidden. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct PotentialFieldParameters: - T attractiveGain # k_a, 1/s - T conicDistance # d*: quadratic inside, conic outside (bounds the pull) - T repulsiveGain # k_r, m³/s - T influenceDistance # ρ₀: obstacles farther than this are ignored - T maxSpeed # output clamp, m/s - T stallSpeed # |F| below this away from the goal ⇒ local minimum - T goalTolerance - -template -struct ObstacleProximity: - math::Vector3 direction # unit, from the obstacle's closest point towards the robot point - T distance # surface distance ρ > 0 (P1/P5) - -enum class FieldStatus : std::uint8_t { moving, reached, stalled } - -template -struct FieldCommand: - math::Vector3 velocity # clamped gradient step - T attractiveMagnitude, repulsiveMagnitude - FieldStatus status - -template -class PotentialField: - PotentialFieldParameters parameters -``` - -## Interface - -```cpp -explicit PotentialField(const PotentialFieldParameters& parameters) -FieldCommand Compute(const math::Vector3& position, const math::Vector3& goal, - std::span> obstacles) const # hot path -``` - -Planar robots pass `z = 0`. For a manipulator, evaluate the field at one or more control points and map -each velocity through the transposed point Jacobian (M8); the field itself is task-space only. - -## Algorithm (pseudocode) - -```text -function Compute(p, g, obstacles): # OPTIMIZE_FOR_SPEED - e = g − p; d = ‖e‖ - F_att = (d ≤ d*) ? k_a·e : k_a·d*·e/d # −∇U_att: quadratic well, conic beyond d* - F_rep = 0 - for (n, ρ) in obstacles with ρ < ρ₀: - F_rep += k_r·(1/ρ − 1/ρ₀)·(1/ρ²)·n # −∇ of ½k_r(1/ρ − 1/ρ₀)² - F = F_att + F_rep; m = ‖F‖ - v = (m ≤ maxSpeed) ? F : F·maxSpeed/m - status = d ≤ goalTolerance ? reached : (m < stallSpeed ? stalled : moving) - return { v, ‖F_att‖, ‖F_rep‖, status } -``` - -## Complexity & memory - -- `O(M)` for `M` obstacle readings: one division chain per active obstacle. Five scalars of state. No heap. - -## Numerical / embedded notes - -- **Conic far field.** A purely quadratic attraction makes far goals pull arbitrarily hard and swamps - the repulsion. Above `d*` the pull is capped at `k_a·d*`. -- **Local minima.** An obstacle straight between the robot and the goal creates an equilibrium where the - forces cancel. `stalled` reports it (test: at `ρ = 0.393003` the two magnitudes are both 1). Escaping is - the caller's job: a random walk, a wall-following behaviour, or a global plan from P3/P6 with - intermediate goals. -- `ρ → 0` makes the repulsion unbounded. The speed clamp keeps the command finite, and the caller keeps - `ρ > 0` by stopping at contact (P1 returns signed distances). -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/planning/PotentialField.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class PotentialField;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/planning/PotentialField.cpp` → `template class PotentialField;` -- Test: `robotics/planning/test/TestPotentialField.cpp` -- Doc: `doc/planning/PotentialField.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources(robotics.planning ...)`; `.cpp` → `robotics_add_coverage_sources`; - test → `robotics.planning_test`. -- Depends on: P1 (proximity readings, caller side). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/planning/PotentialField/tests.md b/roadmap/planning/PotentialField/tests.md deleted file mode 100644 index 3e9c692..0000000 --- a/roadmap/planning/PotentialField/tests.md +++ /dev/null @@ -1,42 +0,0 @@ -# Artificial Potential Fields — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestPotentialField : public ::testing::Test: - PotentialFieldParameters parameters{ 1.0, 1.0, 0.1, 1.0, 0.5, 1e-3, 0.05 } - # k_a, d*, k_r, ρ₀, maxSpeed, stallSpeed, goalTolerance - PotentialField field{ parameters } - Vector3 origin{ 0, 0, 0 } -# each case below is a TEST_F(TestPotentialField, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -near_goal_the_pull_is_quadratic: - Assert: Compute(origin, (0.4, 0.3, 0), {}) ⇒ velocity (0.4, 0.3, 0), attractive 0.5, moving -far_goal_the_pull_is_conic_and_clamped: - Assert: Compute(origin, (3, 4, 0), {}) ⇒ velocity (0.3, 0.4, 0) # |F_att| = k_a·d* = 1, clamped to 0.5 -obstacle_inside_the_influence_distance_repels: - Assert: Compute(origin, (3, 4, 0), { ((0, −1, 0), 0.5) }) ⇒ repulsive 0.4, velocity ≈ (0.416025, 0.277350, 0) -obstacle_outside_the_influence_distance_is_ignored: - Assert: Compute(origin, (3, 4, 0), { ((0, −1, 0), 1.2) }) ⇒ repulsive 0, velocity (0.3, 0.4, 0) -goal_within_tolerance_is_reached: - Assert: Compute(origin, (0.03, 0, 0), {}).status == reached -opposing_obstacle_creates_a_detected_local_minimum: - Assert: Compute(origin, (3, 0, 0), { ((−1, 0, 0), 0.393003) }) ⇒ attractive ≈ repulsive ≈ 1, |velocity| < 1e-3, stalled - with ρ = 0.3 instead ⇒ moving, velocity (−0.5, 0, 0) # repulsion wins: 2.592593 -``` - -## Reference vectors - -- Local-minimum distance: the root of `0.1·(1/ρ − 1)/ρ² = 1`, `ρ* = 0.393003`. -- `ρ = 0.5`: `0.1·(2 − 1)/0.25 = 0.4`. - -## Edge cases - -- No obstacles and `d = 0` ⇒ zero velocity, `reached`. -- Several obstacles add vectorially; symmetric obstacles cancel laterally. diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md b/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md deleted file mode 100644 index ef24411..0000000 --- a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md +++ /dev/null @@ -1,36 +0,0 @@ -# Cartesian Path + Orientation (SLERP) Interpolation — Overview - -## What it is -A task-space motion generator: it moves the end-effector along a **straight line** in -Cartesian position while blending orientation with **SLERP** (spherical linear interpolation of -unit quaternions). A single scalar progress variable `s(t) ∈ [0, 1]` drives both channels so they -start and finish together. - -## Why it matters (embedded) -Many manipulator tasks are defined in the workspace, not joint space — draw a straight bead, keep a -tool square to a surface, approach along an axis. Interpolating position and orientation directly in -Cartesian space produces predictable, collision-friendly paths that joint-space interpolation cannot -guarantee. - -## How it works (intuition) -Position is a plain linear blend between the two endpoints. Orientation is trickier: naively -averaging quaternions leaves the sphere and changes speed. SLERP instead walks the **great-circle -arc** on the unit sphere at constant angular rate, giving the shortest, smoothest rotation. Feeding -`s(t)` from a trapezoidal or polynomial time law shapes how fast the pose advances along the path. -Because the path is fixed and only `s` moves, the end-effector twist is simply `ṡ` times the path -tangent: linear velocity `ṡ·(p1 − p0)` and angular velocity `ṡ·θ` about the fixed axis of the relative -rotation — a free feed-forward term for task-space controllers. - -## Key parameters -- **Start / goal pose** — rigid transforms (position + rotation); orientation handled as unit quaternions. -- **Time law `s(t)`** — any 0 → 1 profile (polynomial, trapezoidal, S-curve); it fixes the duration and - bounds the linear and angular speeds through `ṡ`. - -## Reference -K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 9 (trajectory generation); -K. Shoemake, "Animating rotation with quaternion curves," *SIGGRAPH* 1985 (SLERP). - -## See also -`Quaternion` from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp) -(provides `Slerp`), `math::SE3Transform` (NT53, pose and twist conventions), -upstream `TrapezoidalProfile` / `PolynomialProfile` / `SCurveProfile` (NT56/NT55/NT60, the `s(t)` drivers). diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md b/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md deleted file mode 100644 index 194fb8d..0000000 --- a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md +++ /dev/null @@ -1,109 +0,0 @@ -# Cartesian Path + Orientation (SLERP) Interpolation — Implementation Pseudocode - -> Roadmap ref: #M10 (Tier 2) · Target: `robotics/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -Poses are `math::SE3Transform` (NT53); twists are `math::Vector6` ordered linear-first -`(v; ω)`. Orientation is interpolated with the upstream `math::Quaternion` -(`numerical/math/Quaternion.hpp`, [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp): -`FromRotationMatrix`, `Slerp`, `ToRotationMatrix`, `Conjugate`). `TrajectoryState` comes from upstream -`numerical/motion_profiles/MotionProfileTypes.hpp` (NT55). - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct CartesianState: - math::SE3Transform pose - math::Vector6 twist # (v; ω), base frame — feed-forward for task-space control - -# TimeLaw requirement (static polymorphism, no virtual call on the hot path): -# TrajectoryState Sample(T t) const — position = progress s ∈ [0, 1], velocity = ṡ -# T Duration() const — s(0) = 0, s(Duration()) = 1 -# e.g. upstream motion_profiles::TrapezoidalProfile{0, 1, limits} (NT56) or -# motion_profiles::PolynomialProfile{{.q0 = 0, .qf = 1}, tf, Degree::Quintic} (NT55) - -template # static_assert(std::is_floating_point_v); instantiated for float -class CartesianSlerpInterpolation: - math::Vector p0, deltaP # start position, goal − start - math::Quaternion q0, q1 # start / goal orientation, q1 sign-fixed (q0·q1 ≥ 0) - math::Vector rotationVector # φ·k of R1·R0ᵀ (base frame), |φ| ≤ π - TimeLaw timeLaw -``` - -## Interface - -```text -CartesianSlerpInterpolation(const math::SE3Transform& start, - const math::SE3Transform& goal, const TimeLaw& timeLaw) -CartesianState Sample(T t) # hot path -T Duration() # = timeLaw.Duration() -``` - -## Algorithm (pseudocode) - -```cpp -function plan(start, goal): # orientation geometry once - p0 = start.p; deltaP = goal.p - start.p - q0 = Quaternion::FromRotationMatrix(start.R) - q1 = Quaternion::FromRotationMatrix(goal.R) - if dot4(q0, q1) < 0: q1 = -q1 # shortest of the two arcs (same rotation) - d = q1 * q0.Conjugate() # R1·R0ᵀ, base frame; d.w = dot4(q0, q1) ≥ 0 - n = |(d.x, d.y, d.z)| - if n < ε: rotationVector = 2·(d.x, d.y, d.z) # small angle, no division - else: rotationVector = 2·atan2(n, d.w)·(d.x, d.y, d.z)/n # φ·k, φ = 2·acos(d.w) - -function Sample(t): # OPTIMIZE_FOR_SPEED - law = timeLaw.Sample(clamp(t, 0, Duration())) - s = clamp(law.position, 0, 1); sDot = law.velocity - p = p0 + s*deltaP # straight line - q = Quaternion::Slerp(q0, q1, s) # great-circle arc; nlerp fallback for dot > 0.9995 upstream - v = sDot * deltaP # linear velocity - ω = sDot * rotationVector # angular velocity ṡ·φ·k, constant axis - return { SE3Transform{ q.ToRotationMatrix(), p }, (v; ω) } -``` - -Verified (python3, finite differences of `R(s(t))`): `vee(Ṙ·Rᵀ) = ṡ·φ·k` for arbitrary start/goal, -including the sign-flipped (obtuse) case. - -## Complexity & memory - -- Planning: `O(1)` — two matrix → quaternion conversions, one quaternion product, one `atan2`. -- `Sample`: `O(1)` — one time-law sample, a 3-vector lerp, one upstream `Slerp` (`acos` + 3 `sin`, or an - nlerp) and one quaternion → matrix conversion. -- Memory: `O(1)` — two quaternions, three 3-vectors and the time law; stack-resident, no heap. - -## Numerical / embedded notes - -- Reuse upstream `math::Quaternion::Slerp` (shortest-path flip + nlerp fallback built in) rather than - re-deriving the blend. -- **Shortest-path fix:** negate `q1` when `dot < 0` *before* computing `rotationVector`, so the twist - matches the arc `Slerp` actually takes. -- `rotationVector` uses `φ = 2·atan2(n, w)` — well-conditioned near `φ → 0` (where `acos` loses `float` - precision) and finite at `φ = π`; the angular part of `SE3Transform::PoseError(goal, start)` equals it. -- Position and orientation are **coupled** by sharing one scalar `s(t)` from the time law, so both reach - the goal simultaneously; limits of the time law (on `s`) bound `|v| = ṡ·|Δp|` and `|ω| = ṡ·|φ|`. -- `TimeLaw` is a template parameter, not an interface: no virtual call on the hot path. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `robotics/trajectory/CartesianSlerpInterpolation.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class CartesianSlerpInterpolation>;` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Uses `numerical/motion_profiles/MotionProfileTypes.hpp` (NT55), `numerical/math/SE3Transform.hpp` (NT53) - and `numerical/math/Quaternion.hpp`; `robotics.trajectory` links `robotics.kinematics` and - `numerical.motion_profiles`. -- Coverage: `robotics/trajectory/CartesianSlerpInterpolation.cpp` → - `template class CartesianSlerpInterpolation>;` -- Test: `robotics/trajectory/test/TestCartesianSlerpInterpolation.cpp` -- Doc: `doc/trajectory/CartesianSlerpInterpolation.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. -- CMake: `.hpp` → `target_sources(robotics.trajectory …)`; - `robotics_add_coverage_sources(robotics.trajectory CartesianSlerpInterpolation.cpp)`; - `TestCartesianSlerpInterpolation.cpp` → `robotics.trajectory_test`. -- New module (first trajectory item only): `robotics/trajectory/CMakeLists.txt` with - `robotics_add_header_library(robotics.trajectory)`, a `test/` subdir, `add_subdirectory(trajectory)` in - `robotics/CMakeLists.txt`, and a `doc/trajectory/` folder. -- Depends on: NT53, NT55 (any other `TimeLaw`, e.g. NT56/NT60, works without extra code). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/tests.md b/roadmap/trajectory/CartesianSlerpInterpolation/tests.md deleted file mode 100644 index 0db2ad0..0000000 --- a/roadmap/trajectory/CartesianSlerpInterpolation/tests.md +++ /dev/null @@ -1,66 +0,0 @@ -# Cartesian Path + Orientation (SLERP) Interpolation — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestCartesianSlerp : public ::testing::Test: - using Law = motion_profiles::PolynomialProfile # upstream NT55 - Law linearLaw{ { .q0 = 0, .qf = 1, .v0 = 1, .vf = 1 }, 1.0f, Degree::Cubic } # s(t) = t, ṡ = 1 - math::SE3Transform start{ .R = I, .p = {0,0,0} } - math::SE3Transform goal { .R = Rz(90°), .p = {1,0,0} } - CartesianSlerpInterpolation path{ start, goal, linearLaw } -# each case below is a TEST_F(TestCartesianSlerp, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -endpoints_match_start_and_goal: - Assert: Sample(0).pose ≈ start and Sample(tf).pose ≈ goal (R and p element-wise) - -position_is_straight_line: - Assert: Sample(tf/2).pose.p ≈ (0.5, 0, 0) - -rotation_stays_orthonormal: - Assert: Sample(tf/2).pose.R · Sample(tf/2).pose.Rᵀ ≈ I, det ≈ +1 - -slerp_midpoint_is_half_angle: - Assert: Sample(tf/2).pose.R ≈ Rz(45°) - -shortest_path_takes_short_arc: - Arrange: goal.R = Rz(270°) (= Rz(−90°)) - Assert: Sample(tf/2).pose.R ≈ Rz(−45°) and twist ω ≈ (0, 0, −π/2) - -near_parallel_orientation_is_finite: - Arrange: goal.R = Rz(0.01°) - Assert: no NaN/inf in pose or twist; pose.R ≈ I; ω_z ≈ 0.01°·π/180 (ṡ = 1) - -constant_angular_rate_for_linear_time_law: - Assert: angle(start.R, Sample(t).pose.R) ≈ t·90° for t ∈ {0.25, 0.5, 0.75} - -twist_is_sdot_times_path_tangent: - Arrange: quintic law { .q0 = 0, .qf = 1 }, tf = 2 ⇒ ṡ(1) = 0.9375 - Assert: Sample(1).twist ≈ (0.9375, 0, 0; 0, 0, 0.9375·π/2 = 1.472622) - i.e. v = ṡ·(p1 − p0), |ω| = ṡ·θ with θ = 90°, axis ẑ; Sample(0).twist ≈ 0 - -sample_clamps_outside_domain: - Assert: Sample(-1).pose ≈ start and Sample(tf+1).pose ≈ goal -``` - -## Reference vectors - -- SLERP of `I → Rz(90°)` at `s = 0.5` ⇒ `Rz(45°)` (quaternion `(0.923880, 0, 0, 0.382683)`). -- Straight-line position `p(s) = p0 + s(p1 − p0)`; midpoint of `(0,0,0)→(1,0,0)` is `(0.5,0,0)`. -- Twist `(v; ω) = ṡ·(p1 − p0; φ·k)`, `φ·k` = rotation vector of `R1·R0ᵀ`; for `Rz(90°)` and `ṡ = 1.3`, - `ω = (0, 0, 2.042035)` (matches finite differences of `R(s(t))`). -- Rest-to-rest quintic `0 → 1`, `tf = 2`: `ṡ(tf/2) = (15/8)/tf = 0.9375`. - -## Edge cases - -- Identical start and goal orientation ⇒ constant rotation, `ω = 0`, no division by zero. -- Antipodal quaternions (`dot ≈ −1`, from the sign ambiguity of `FromRotationMatrix`) ⇒ sign fix, - identical rotation, `ω = 0`. -- 180° rotation ⇒ either arc is shortest; `|ω| = ṡ·π`, pose and twist stay consistent. -- Zero-length Cartesian move with pure rotation ⇒ position constant, `v = 0`, orientation still slerps. diff --git a/robotics/controllers/manipulator/CMakeLists.txt b/robotics/controllers/manipulator/CMakeLists.txt index 605c2a8..c7d3dfc 100644 --- a/robotics/controllers/manipulator/CMakeLists.txt +++ b/robotics/controllers/manipulator/CMakeLists.txt @@ -11,10 +11,12 @@ target_link_libraries(robotics.controllers.manipulator ${ROBOTICS_VISIBILITY} ) target_sources(robotics.controllers.manipulator PRIVATE + ComputedTorqueControl.hpp PdGravityCompensation.hpp ) robotics_add_coverage_sources(robotics.controllers.manipulator + ComputedTorqueControl.cpp PdGravityCompensation.cpp ) diff --git a/robotics/controllers/manipulator/ComputedTorqueControl.cpp b/robotics/controllers/manipulator/ComputedTorqueControl.cpp new file mode 100644 index 0000000..b62e81f --- /dev/null +++ b/robotics/controllers/manipulator/ComputedTorqueControl.cpp @@ -0,0 +1,7 @@ +#include "robotics/controllers/manipulator/ComputedTorqueControl.hpp" + +namespace controllers +{ + template class ComputedTorqueControl; + template class ComputedTorqueControl; +} diff --git a/robotics/controllers/manipulator/ComputedTorqueControl.hpp b/robotics/controllers/manipulator/ComputedTorqueControl.hpp new file mode 100644 index 0000000..a9e85b3 --- /dev/null +++ b/robotics/controllers/manipulator/ComputedTorqueControl.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/InverseDynamicsModel.hpp" +#include +#include + +namespace controllers +{ + template + class ComputedTorqueControl + { + static_assert(std::is_floating_point_v, "ComputedTorqueControl supports floating-point types"); + static_assert(Dof > 0, "Degrees of freedom must be positive"); + + public: + using StateVector = math::Vector; + using GainMatrix = math::SquareMatrix; + + ComputedTorqueControl(const dynamics::InverseDynamicsModel& model, const GainMatrix& kp, const GainMatrix& kd); + + OPTIMIZE_FOR_SPEED StateVector ComputeTorque(const StateVector& q, const StateVector& qDot, + const StateVector& qDesired, const StateVector& qDotDesired, const StateVector& qDDotDesired) const; + + private: + const dynamics::InverseDynamicsModel& model; + GainMatrix kp; + GainMatrix kd; + }; + + template + ComputedTorqueControl::ComputedTorqueControl(const dynamics::InverseDynamicsModel& model, const GainMatrix& kp, const GainMatrix& kd) + : model{ model } + , kp{ kp } + , kd{ kd } + {} + + template + OPTIMIZE_FOR_SPEED typename ComputedTorqueControl::StateVector ComputedTorqueControl::ComputeTorque(const StateVector& q, const StateVector& qDot, + const StateVector& qDesired, const StateVector& qDotDesired, const StateVector& qDDotDesired) const + { + const StateVector acceleration{ qDDotDesired + kd * StateVector{ qDotDesired - qDot } + kp * StateVector{ qDesired - q } }; + return model.ComputeInverseDynamics(q, qDot, acceleration); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ComputedTorqueControl; + extern template class ComputedTorqueControl; +#endif +} diff --git a/robotics/controllers/manipulator/test/CMakeLists.txt b/robotics/controllers/manipulator/test/CMakeLists.txt index e6b4d63..3fe6846 100644 --- a/robotics/controllers/manipulator/test/CMakeLists.txt +++ b/robotics/controllers/manipulator/test/CMakeLists.txt @@ -9,5 +9,6 @@ target_link_libraries(robotics.controllers.manipulator_test PUBLIC ) target_sources(robotics.controllers.manipulator_test PRIVATE + TestComputedTorqueControl.cpp TestPdGravityCompensation.cpp ) diff --git a/robotics/controllers/manipulator/test/TestComputedTorqueControl.cpp b/robotics/controllers/manipulator/test/TestComputedTorqueControl.cpp new file mode 100644 index 0000000..99d5d7a --- /dev/null +++ b/robotics/controllers/manipulator/test/TestComputedTorqueControl.cpp @@ -0,0 +1,113 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/controllers/manipulator/ComputedTorqueControl.hpp" +#include "robotics/dynamics/ChainDynamicsModel.hpp" +#include "robotics/dynamics/EulerLagrangeSolver.hpp" +#include "robotics/dynamics/test_doubles/MockInverseDynamicsModel.hpp" +#include +#include +#include + +namespace +{ + using Vector2 = math::Vector; + using Vector3 = math::Vector; + using Matrix2 = math::SquareMatrix; + using Link = dynamics::JointLink; + + MATCHER_P(VectorNear, expected, "") + { + return std::abs(arg[0] - expected[0]) < math::Tolerance() && std::abs(arg[1] - expected[1]) < math::Tolerance(); + } + + Link MakeRod(float mass, float length, const Vector3& parentToJoint) + { + const float inertia{ mass * length * length / 12.0f }; + return Link{ mass, math::SquareMatrix{ { 0.0f, 0.0f, 0.0f }, { 0.0f, inertia, 0.0f }, { 0.0f, 0.0f, inertia } }, Vector3{ 0.0f, 1.0f, 0.0f }, parentToJoint, + Vector3{ length / 2.0f, 0.0f, 0.0f } }; + } + + class TestComputedTorqueControl + : public ::testing::Test + { + protected: + void ExpectCommand(const Vector2& acceleration, const Vector2& torque) + { + EXPECT_CALL(model, ComputeInverseDynamics(VectorNear(q), VectorNear(qDot), VectorNear(acceleration))).Times(1).WillOnce(testing::Return(torque)); + } + + testing::StrictMock> model; + Matrix2 kp{ { 100.0f, 0.0f }, { 0.0f, 100.0f } }; + Matrix2 kd{ { 20.0f, 0.0f }, { 0.0f, 20.0f } }; + controllers::ComputedTorqueControl controller{ model, kp, kd }; + Vector2 q{ 0.3f, -0.6f }; + Vector2 qDot{ 0.4f, -1.0f }; + }; +} + +TEST_F(TestComputedTorqueControl, pure_feedforward_passes_desired_acceleration) +{ + const Vector2 desiredAcceleration{ 1.5f, -0.7f }; + const Vector2 modelTorque{ 3.25f, -1.75f }; + ExpectCommand(desiredAcceleration, modelTorque); + + const auto tau{ controller.ComputeTorque(q, qDot, q, qDot, desiredAcceleration) }; + + EXPECT_EQ(tau[0], modelTorque[0]); + EXPECT_EQ(tau[1], modelTorque[1]); +} + +TEST_F(TestComputedTorqueControl, position_error_enters_command) +{ + ExpectCommand(Vector2{ 10.0f, -20.0f }, Vector2{}); + + controller.ComputeTorque(q, qDot, Vector2{ q[0] + 0.1f, q[1] - 0.2f }, qDot, Vector2{}); +} + +TEST_F(TestComputedTorqueControl, velocity_error_enters_command) +{ + ExpectCommand(Vector2{ 6.0f, 4.0f }, Vector2{}); + + controller.ComputeTorque(q, qDot, q, Vector2{ qDot[0] + 0.3f, qDot[1] + 0.2f }, Vector2{}); +} + +TEST_F(TestComputedTorqueControl, full_command_superposition) +{ + const Vector2 desired{ q[0] + 0.1f, q[1] - 0.2f }; + const Vector2 desiredVelocity{ qDot[0] + 0.3f, qDot[1] + 0.2f }; + const Vector2 desiredAcceleration{ 1.5f, -0.7f }; + ExpectCommand(Vector2{ 1.5f + 6.0f + 10.0f, -0.7f + 4.0f - 20.0f }, Vector2{}); + + controller.ComputeTorque(q, qDot, desired, desiredVelocity, desiredAcceleration); +} + +TEST_F(TestComputedTorqueControl, decoupled_error_dynamics) +{ + const std::array links{ MakeRod(1.2f, 0.6f, Vector3{}), MakeRod(0.7f, 0.45f, Vector3{ 0.6f, 0.0f, 0.0f }) }; + const dynamics::ChainDynamicsModel plant{ links, Vector3{ 0.0f, 0.0f, -9.81f } }; + const dynamics::EulerLagrangeSolver solver; + EXPECT_CALL(model, ComputeInverseDynamics(testing::_, testing::_, testing::_)).WillRepeatedly([&plant](const Vector2& position, const Vector2& velocity, const Vector2& acceleration) + { + return plant.ComputeInverseDynamics(position, velocity, acceleration); + }); + const Vector2 setpoint{ 0.5f, -0.2f }; + const float dt{ 1e-3f }; + + Vector2 position{ 0.2f, 0.3f }; + Vector2 velocity{}; + Vector2 error{ setpoint - position }; + Vector2 errorRate{}; + for (std::size_t step = 0; step < 1000; ++step) + { + const auto tau{ controller.ComputeTorque(position, velocity, setpoint, Vector2{}, Vector2{}) }; + velocity = velocity + solver.ForwardDynamics(plant, position, velocity, tau) * dt; + position = position + velocity * dt; + errorRate = errorRate + Vector2{ kd * errorRate + kp * error } * (-dt); + error = error + errorRate * dt; + + EXPECT_NEAR(setpoint[0] - position[0], error[0], math::Tolerance()); + EXPECT_NEAR(setpoint[1] - position[1], error[1], math::Tolerance()); + } + + EXPECT_NEAR(setpoint[0] - position[0], 0.0f, math::Tolerance()); + EXPECT_NEAR(setpoint[1] - position[1], 0.0f, math::Tolerance()); +} diff --git a/robotics/dynamics/RecursiveNewtonEuler.hpp b/robotics/dynamics/RecursiveNewtonEuler.hpp index 0263cf7..db21db4 100644 --- a/robotics/dynamics/RecursiveNewtonEuler.hpp +++ b/robotics/dynamics/RecursiveNewtonEuler.hpp @@ -6,9 +6,20 @@ #include "robotics/dynamics/JointLink.hpp" #include #include +#include namespace dynamics { + template + struct ToolWrench + { + static_assert(std::is_floating_point_v, "ToolWrench supports floating-point types"); + + math::Vector force{}; + math::Vector moment{}; + math::Vector toolOffset{}; + }; + template class RecursiveNewtonEuler { @@ -28,6 +39,10 @@ namespace dynamics const JointVector& q, const JointVector& qDot, const JointVector& qDDot, const Vector3& gravity) const; + OPTIMIZE_FOR_SPEED JointVector InverseDynamics(const LinkArray& links, + const JointVector& q, const JointVector& qDot, const JointVector& qDDot, + const Vector3& gravity, const ToolWrench& wrench) const; + private: struct LinkState { @@ -58,7 +73,7 @@ namespace dynamics static JointVector ComputeBackwardPass( const LinkArray& links, const std::array& states, - const JointVector& qDDot); + const JointVector& qDDot, const Vector3& tipForce, const Vector3& tipTorque); }; template @@ -132,7 +147,7 @@ namespace dynamics typename RecursiveNewtonEuler::JointVector RecursiveNewtonEuler::ComputeBackwardPass( const LinkArray& links, const std::array& states, - const JointVector& qDDot) + const JointVector& qDDot, const Vector3& tipForce, const Vector3& tipTorque) { std::array force; std::array torque; @@ -146,6 +161,11 @@ namespace dynamics if (i + 1 < NumLinks) AccumulateChildWrench(force[i], torque[i], force[i + 1], torque[i + 1], states[i + 1].R, states[i + 1].offset); + else + { + force[i] = force[i] - tipForce; + torque[i] = torque[i] - tipTorque; + } } JointVector tau{}; @@ -163,7 +183,25 @@ namespace dynamics const Vector3& gravity) const { auto states = ComputeForwardPass(links, q, qDot, qDDot, gravity); - return ComputeBackwardPass(links, states, qDDot); + return ComputeBackwardPass(links, states, qDDot, Vector3{}, Vector3{}); + } + + template + OPTIMIZE_FOR_SPEED + typename RecursiveNewtonEuler::JointVector + RecursiveNewtonEuler::InverseDynamics(const LinkArray& links, + const JointVector& q, const JointVector& qDot, const JointVector& qDDot, + const Vector3& gravity, const ToolWrench& wrench) const + { + auto states = ComputeForwardPass(links, q, qDot, qDDot, gravity); + + Matrix3 baseFromLast{ Matrix3::Identity() }; + for (const auto& state : states) + baseFromLast = baseFromLast * state.R; + + const Vector3 tipForce{ baseFromLast.Transpose() * wrench.force }; + const Vector3 tipTorque{ baseFromLast.Transpose() * wrench.moment + math::CrossProduct(wrench.toolOffset, tipForce) }; + return ComputeBackwardPass(links, states, qDDot, tipForce, tipTorque); } #ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD diff --git a/robotics/dynamics/test/CMakeLists.txt b/robotics/dynamics/test/CMakeLists.txt index 6fbb582..14fa6d8 100644 --- a/robotics/dynamics/test/CMakeLists.txt +++ b/robotics/dynamics/test/CMakeLists.txt @@ -5,6 +5,7 @@ emil_add_test(robotics.dynamics_test) target_link_libraries(robotics.dynamics_test PUBLIC gmock_main robotics.dynamics + robotics.kinematics ) target_sources(robotics.dynamics_test PRIVATE diff --git a/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp b/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp index 1d8ddbe..c858f5d 100644 --- a/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp +++ b/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp @@ -2,6 +2,8 @@ #include "robotics/dynamics/EulerLagrangeDynamics.hpp" #include "robotics/dynamics/EulerLagrangeSolver.hpp" #include "robotics/dynamics/RecursiveNewtonEuler.hpp" +#include "robotics/kinematics/ChainPoseKinematics.hpp" +#include "robotics/kinematics/GeometricJacobian.hpp" #include #include @@ -86,6 +88,7 @@ namespace protected: dynamics::RecursiveNewtonEuler rnea1; dynamics::RecursiveNewtonEuler rnea2; + dynamics::RecursiveNewtonEuler rnea3; }; } @@ -192,3 +195,82 @@ TEST_F(TestRecursiveNewtonEuler, prismatic_coriolis_term_on_rotating_base) EXPECT_NEAR(tau.at(0, 0), 2.0f * mass * radius * extensionRate * spinRate + mass * radius * radius * spinAcceleration, math::Tolerance()); EXPECT_NEAR(tau.at(1, 0), mass * (extensionAcceleration - radius * spinRate * spinRate), math::Tolerance()); } + +namespace +{ + const Vector3 toolOffset{ 0.3f, 0.05f, 0.0f }; + + Vector3 Normalized(float x, float y, float z) + { + const float norm{ std::sqrt(x * x + y * y + z * z) }; + return Vector3{ x / norm, y / norm, z / norm }; + } + + std::array SkewedChain() + { + const math::SquareMatrix inertia{ { 0.05f, 0.01f, -0.005f }, { 0.01f, 0.04f, 0.008f }, { -0.005f, 0.008f, 0.03f } }; + return std::array{ + Link{ 1.4f, inertia, Normalized(0.0f, 0.2f, 1.0f), Vector3{ 0.0f, 0.0f, 0.1f }, Vector3{ 0.02f, -0.01f, 0.15f } }, + Link{ 0.9f, inertia * 0.8f, Normalized(0.1f, 1.0f, 0.0f), Vector3{ 0.05f, 0.0f, 0.3f }, Vector3{ 0.2f, 0.03f, -0.02f } }, + Link{ 0.5f, inertia * 0.5f, Normalized(1.0f, 0.3f, -0.4f), Vector3{ 0.4f, -0.05f, 0.02f }, Vector3{ 0.1f, 0.02f, 0.03f } } + }; + } +} + +TEST_F(TestRecursiveNewtonEuler, zero_wrench_matches_shipped_overload) +{ + const auto chain{ SkewedChain() }; + const math::Vector q{ 0.4f, -0.7f, 1.1f }; + const math::Vector qDot{ 0.9f, -1.3f, 0.6f }; + const math::Vector qDDot{ 1.5f, -2.0f, 0.7f }; + + const auto withWrench{ rnea3.InverseDynamics(chain, q, qDot, qDDot, gravityVector, dynamics::ToolWrench{ Vector3{}, Vector3{}, toolOffset }) }; + const auto shipped{ rnea3.InverseDynamics(chain, q, qDot, qDDot, gravityVector) }; + + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(withWrench.at(i, 0), shipped.at(i, 0), math::Tolerance()); +} + +TEST_F(TestRecursiveNewtonEuler, wrench_enters_as_minus_jacobian_transpose) +{ + const auto chain{ SkewedChain() }; + const math::Vector q{ 0.4f, -0.7f, 1.1f }; + const math::Vector qDot{ 0.9f, -1.3f, 0.6f }; + const math::Vector qDDot{ 1.5f, -2.0f, 0.7f }; + const dynamics::ToolWrench wrench{ Vector3{ 1.0f, -2.0f, 0.5f }, Vector3{ 0.1f, 0.0f, -0.2f }, toolOffset }; + const kinematics::ChainPoseKinematics kinematics{ math::SE3Transform{ math::SquareMatrix::Identity(), toolOffset } }; + math::Vector6 stacked{}; + stacked.SetBlock(wrench.force, 0, 0); + stacked.SetBlock(wrench.moment, 3, 0); + + const auto loaded{ rnea3.InverseDynamics(chain, q, qDot, qDDot, gravityVector, wrench) }; + const auto free{ rnea3.InverseDynamics(chain, q, qDot, qDDot, gravityVector) }; + + using Jacobian = kinematics::GeometricJacobian; + const auto expected{ Jacobian::JointTorques(Jacobian::Compute(kinematics.Compute(chain, q)), stacked) }; + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(loaded.at(i, 0) - free.at(i, 0), -expected.at(i, 0), math::Tolerance()); +} + +TEST_F(TestRecursiveNewtonEuler, horizontal_link_holding_a_payload) +{ + const float mass{ 1.0f }; + const float length{ 1.0f }; + const float payload{ 0.5f }; + std::array links{ MakeRod(mass, length, yAxis, Vector3{}) }; + const dynamics::ToolWrench wrench{ Vector3{ 0.0f, 0.0f, -payload * gravity }, Vector3{}, Vector3{ length, 0.0f, 0.0f } }; + + auto tau = rnea1.InverseDynamics(links, math::Vector{ 0.0f }, math::Vector{}, math::Vector{}, gravityVector, wrench); + + EXPECT_NEAR(tau.at(0, 0), -(mass * length / 2.0f + payload * length) * gravity, math::Tolerance()); +} + +TEST_F(TestRecursiveNewtonEuler, pure_moment_projects_on_the_axis) +{ + std::array links{ MakeRod(1.0f, 1.0f, zAxis, Vector3{}) }; + const dynamics::ToolWrench wrench{ Vector3{}, Vector3{ 0.0f, 0.0f, 2.0f }, Vector3{ 1.0f, 0.0f, 0.0f } }; + + auto tau = rnea1.InverseDynamics(links, math::Vector{ 0.6f }, math::Vector{}, math::Vector{}, gravityVector, wrench); + + EXPECT_NEAR(tau.at(0, 0), -2.0f, math::Tolerance()); +} diff --git a/robotics/dynamics/test_doubles/CMakeLists.txt b/robotics/dynamics/test_doubles/CMakeLists.txt index e02acfa..fbcdd24 100644 --- a/robotics/dynamics/test_doubles/CMakeLists.txt +++ b/robotics/dynamics/test_doubles/CMakeLists.txt @@ -8,4 +8,5 @@ target_link_libraries(robotics.dynamics_test_doubles INTERFACE target_sources(robotics.dynamics_test_doubles PRIVATE MockEulerLagrangeDynamics.hpp + MockInverseDynamicsModel.hpp ) diff --git a/robotics/dynamics/test_doubles/MockInverseDynamicsModel.hpp b/robotics/dynamics/test_doubles/MockInverseDynamicsModel.hpp new file mode 100644 index 0000000..539f939 --- /dev/null +++ b/robotics/dynamics/test_doubles/MockInverseDynamicsModel.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include "robotics/dynamics/InverseDynamicsModel.hpp" +#include + +namespace dynamics +{ + template + class MockInverseDynamicsModel + : public InverseDynamicsModel + { + public: + using typename InverseDynamicsModel::StateVector; + + MOCK_METHOD(StateVector, ComputeInverseDynamics, (const StateVector& q, const StateVector& qDot, const StateVector& qDDot), (const, override)); + }; +} diff --git a/robotics/kinematics/CMakeLists.txt b/robotics/kinematics/CMakeLists.txt index fb8017b..5cac13d 100644 --- a/robotics/kinematics/CMakeLists.txt +++ b/robotics/kinematics/CMakeLists.txt @@ -20,6 +20,8 @@ target_sources(robotics.kinematics PRIVATE GeometricJacobian.hpp InverseKinematics.hpp JacobianProvider.hpp + ManipulabilityIndex.hpp + RedundancyResolution.hpp ) robotics_add_coverage_sources(robotics.kinematics @@ -28,6 +30,9 @@ robotics_add_coverage_sources(robotics.kinematics ForwardKinematics.cpp GeometricJacobian.cpp InverseKinematics.cpp + ManipulabilityIndex.cpp + RedundancyResolution.cpp ) add_subdirectory(test) +add_subdirectory(test_doubles) diff --git a/robotics/kinematics/ManipulabilityIndex.cpp b/robotics/kinematics/ManipulabilityIndex.cpp new file mode 100644 index 0000000..8aca0dc --- /dev/null +++ b/robotics/kinematics/ManipulabilityIndex.cpp @@ -0,0 +1,9 @@ +#include "robotics/kinematics/ManipulabilityIndex.hpp" + +namespace kinematics +{ + template class ManipulabilityIndex; + template class ManipulabilityIndex; + template class ManipulabilityIndex; + template class ManipulabilityIndex; +} diff --git a/robotics/kinematics/ManipulabilityIndex.hpp b/robotics/kinematics/ManipulabilityIndex.hpp new file mode 100644 index 0000000..3b08ba0 --- /dev/null +++ b/robotics/kinematics/ManipulabilityIndex.hpp @@ -0,0 +1,129 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/solvers/LuDecomposition.hpp" +#include "numerical/solvers/SingularValueDecomposition.hpp" +#include "robotics/kinematics/JacobianProvider.hpp" +#include +#include +#include +#include +#include + +namespace kinematics +{ + template + class ManipulabilityIndex + { + static_assert(std::is_floating_point_v, "ManipulabilityIndex supports floating-point types"); + + public: + static constexpr std::size_t Rows{ TaskDim == 6 ? 3 : TaskDim }; + static constexpr std::size_t Axes{ std::min(Rows, Dof) }; + + using JointVector = math::Vector; + using Block = math::Matrix; + using AxesArray = std::array; + + explicit ManipulabilityIndex(const JacobianProvider& jacobian); + + OPTIMIZE_FOR_SPEED T Compute(const JointVector& q) const; + T ComputeRotational(const JointVector& q) const + requires(TaskDim == 6); + AxesArray EllipsoidAxes(const JointVector& q) const; + T ConditionNumber(const JointVector& q) const; + bool NearSingular(const JointVector& q, T epsilon) const; + + private: + static constexpr T singularRatio{ static_cast(1e-6) }; + + template + static T GramRoot(const math::Matrix& block); + + Block TranslationalBlock(const JointVector& q) const; + + const JacobianProvider& jacobian; + }; + + template + ManipulabilityIndex::ManipulabilityIndex(const JacobianProvider& jacobian) + : jacobian{ jacobian } + {} + + template + OPTIMIZE_FOR_SPEED T ManipulabilityIndex::Compute(const JointVector& q) const + { + return GramRoot(TranslationalBlock(q)); + } + + template + T ManipulabilityIndex::ComputeRotational(const JointVector& q) const + requires(TaskDim == 6) + { + return GramRoot(math::Matrix{ jacobian.Jacobian(q).template GetBlock<3, Dof>(3, 0) }); + } + + template + typename ManipulabilityIndex::AxesArray ManipulabilityIndex::EllipsoidAxes(const JointVector& q) const + { + solvers::SingularValueDecomposition svd; + svd.Decompose(TranslationalBlock(q)); + + AxesArray axes{}; + for (std::size_t i = 0; i < Axes; ++i) + axes[i] = svd.SingularValues()[i]; + return axes; + } + + template + T ManipulabilityIndex::ConditionNumber(const JointVector& q) const + { + const auto axes{ EllipsoidAxes(q) }; + const T largest{ axes.front() }; + const T smallest{ axes.back() }; + + if (smallest <= singularRatio * largest) + return std::numeric_limits::infinity(); + + return largest / smallest; + } + + template + bool ManipulabilityIndex::NearSingular(const JointVector& q, T epsilon) const + { + return Compute(q) < epsilon; + } + + template + template + T ManipulabilityIndex::GramRoot(const math::Matrix& block) + { + constexpr std::size_t gramSize{ std::min(BlockRows, Dof) }; + math::SquareMatrix gram{}; + if constexpr (BlockRows <= Dof) + gram = block * block.Transpose(); + else + gram = block.Transpose() * block; + + solvers::LuDecomposition lu; + if (!lu.Decompose(gram)) + return T{}; + + return math::Sqrt(std::max(T{}, lu.Determinant())); + } + + template + typename ManipulabilityIndex::Block ManipulabilityIndex::TranslationalBlock(const JointVector& q) const + { + return jacobian.Jacobian(q).template GetBlock(0, 0); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ManipulabilityIndex; + extern template class ManipulabilityIndex; + extern template class ManipulabilityIndex; + extern template class ManipulabilityIndex; +#endif +} diff --git a/robotics/kinematics/RedundancyResolution.cpp b/robotics/kinematics/RedundancyResolution.cpp new file mode 100644 index 0000000..92bfb50 --- /dev/null +++ b/robotics/kinematics/RedundancyResolution.cpp @@ -0,0 +1,7 @@ +#include "robotics/kinematics/RedundancyResolution.hpp" + +namespace kinematics +{ + template class RedundancyResolution; + template class RedundancyResolution; +} diff --git a/robotics/kinematics/RedundancyResolution.hpp b/robotics/kinematics/RedundancyResolution.hpp new file mode 100644 index 0000000..97a6ebe --- /dev/null +++ b/robotics/kinematics/RedundancyResolution.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/solvers/DampedPseudoInverse.hpp" +#include "robotics/kinematics/JacobianProvider.hpp" +#include +#include + +namespace kinematics +{ + template + class RedundancyResolution + { + static_assert(std::is_floating_point_v, "RedundancyResolution supports floating-point types"); + static_assert(Dof > TaskDim, "Redundancy resolution needs more joints than task dimensions"); + + public: + using JointVector = math::Vector; + using TaskVector = math::Vector; + + RedundancyResolution(const JacobianProvider& jacobian, T damping, T rankTolerance = static_cast(1e-4)); + + OPTIMIZE_FOR_SPEED JointVector Resolve(const JointVector& q, const TaskVector& xDot, const JointVector& qDot0); + math::Matrix DampedPseudoInverse(const JointVector& q); + math::SquareMatrix NullSpaceProjector(const JointVector& q); + + private: + void Factor(const JointVector& q); + + const JacobianProvider& jacobian; + solvers::DampedPseudoInverse inverse; + }; + + template + RedundancyResolution::RedundancyResolution(const JacobianProvider& jacobian, T damping, T rankTolerance) + : jacobian{ jacobian } + , inverse{ damping, rankTolerance } + {} + + template + OPTIMIZE_FOR_SPEED typename RedundancyResolution::JointVector RedundancyResolution::Resolve(const JointVector& q, const TaskVector& xDot, const JointVector& qDot0) + { + Factor(q); + return inverse.Solve(xDot) + inverse.Project(qDot0); + } + + template + math::Matrix RedundancyResolution::DampedPseudoInverse(const JointVector& q) + { + Factor(q); + return inverse.Inverse(); + } + + template + math::SquareMatrix RedundancyResolution::NullSpaceProjector(const JointVector& q) + { + Factor(q); + return inverse.NullSpaceProjector(); + } + + template + void RedundancyResolution::Factor(const JointVector& q) + { + const bool factored{ inverse.Factor(jacobian.Jacobian(q)) }; + really_assert(factored); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class RedundancyResolution; + extern template class RedundancyResolution; +#endif +} diff --git a/robotics/kinematics/test/CMakeLists.txt b/robotics/kinematics/test/CMakeLists.txt index bd734e4..a3f5bc2 100644 --- a/robotics/kinematics/test/CMakeLists.txt +++ b/robotics/kinematics/test/CMakeLists.txt @@ -5,6 +5,7 @@ emil_add_test(robotics.kinematics_test) target_link_libraries(robotics.kinematics_test PUBLIC gmock_main robotics.kinematics + robotics.kinematics_test_doubles ) target_sources(robotics.kinematics_test PRIVATE @@ -13,4 +14,6 @@ target_sources(robotics.kinematics_test PRIVATE TestForwardKinematics.cpp TestGeometricJacobian.cpp TestInverseKinematics.cpp + TestManipulabilityIndex.cpp + TestRedundancyResolution.cpp ) diff --git a/robotics/kinematics/test/TestManipulabilityIndex.cpp b/robotics/kinematics/test/TestManipulabilityIndex.cpp new file mode 100644 index 0000000..3bffc61 --- /dev/null +++ b/robotics/kinematics/test/TestManipulabilityIndex.cpp @@ -0,0 +1,109 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/kinematics/ChainTaskJacobian.hpp" +#include "robotics/kinematics/ManipulabilityIndex.hpp" +#include "robotics/kinematics/test_doubles/MockJacobianProvider.hpp" +#include +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Link = dynamics::RevoluteJointLink; + using Transform = math::SE3Transform; + using Planar = math::Vector; + + constexpr float halfPi{ std::numbers::pi_v / 2.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeLink(const Vector3& parentToJoint) + { + return Link{ 1.0f, Matrix3::Identity(), zAxis, parentToJoint, Vector3{} }; + } + + class TestManipulabilityIndex + : public ::testing::Test + { + protected: + std::array planar{ MakeLink(Vector3{}), MakeLink(Vector3{ 1.0f, 0.0f, 0.0f }) }; + Transform tool{ Matrix3::Identity(), Vector3{ 1.0f, 0.0f, 0.0f } }; + kinematics::ChainTaskJacobian position{ planar, tool }; + kinematics::ChainTaskJacobian full{ planar, tool }; + kinematics::ManipulabilityIndex w{ position }; + testing::StrictMock> planarTask; + kinematics::ManipulabilityIndex wRedundant{ planarTask }; + }; +} + +TEST_F(TestManipulabilityIndex, position_manipulability_is_abs_sin_elbow) +{ + for (const float elbow : { 0.3f, 1.2f, 2.5f }) + EXPECT_NEAR(w.Compute(Planar{ 0.0f, elbow }), std::abs(std::sin(elbow)), math::Tolerance()); +} + +TEST_F(TestManipulabilityIndex, right_angle_elbow_is_most_dexterous) +{ + const float best{ w.Compute(Planar{ 0.0f, halfPi }) }; + + EXPECT_NEAR(best, 1.0f, math::Tolerance()); + EXPECT_GT(best, w.Compute(Planar{ 0.0f, halfPi + 0.1f })); + EXPECT_GT(best, w.Compute(Planar{ 0.0f, halfPi - 0.1f })); +} + +TEST_F(TestManipulabilityIndex, stretched_and_folded_arm_are_singular) +{ + EXPECT_NEAR(w.Compute(Planar{ 0.0f, 0.0f }), 0.0f, math::Tolerance()); + EXPECT_NEAR(w.Compute(Planar{ 0.0f, std::numbers::pi_v }), 0.0f, math::Tolerance()); +} + +TEST_F(TestManipulabilityIndex, redundant_task_uses_j_jt_branch) +{ + const math::Vector q{ 0.0f, halfPi, halfPi }; + EXPECT_CALL(planarTask, Jacobian(testing::_)).Times(2).WillRepeatedly(testing::Return(math::Matrix{ { -1.0f, -1.0f, 0.0f }, { 0.0f, -1.0f, -1.0f } })); + + const float value{ wRedundant.Compute(q) }; + const auto axes{ wRedundant.EllipsoidAxes(q) }; + + EXPECT_NEAR(value, std::sqrt(3.0f), math::Tolerance()); + EXPECT_NEAR(axes[0], std::sqrt(3.0f), math::Tolerance()); + EXPECT_NEAR(axes[1], 1.0f, math::Tolerance()); +} + +TEST_F(TestManipulabilityIndex, six_row_task_measures_units_separately) +{ + const kinematics::ManipulabilityIndex sixRow{ full }; + + EXPECT_NEAR(sixRow.Compute(Planar{ 0.0f, 1.2f }), 0.932039f, math::Tolerance()); + EXPECT_NEAR(sixRow.ComputeRotational(Planar{ 0.0f, 1.2f }), 0.0f, math::Tolerance()); +} + +TEST_F(TestManipulabilityIndex, ellipsoid_axes_are_sorted_singular_values) +{ + const auto axes{ w.EllipsoidAxes(Planar{ 0.0f, halfPi }) }; + + EXPECT_NEAR(axes[0], 1.618034f, math::Tolerance()); + EXPECT_NEAR(axes[1], 0.618034f, math::Tolerance()); + EXPECT_NEAR(axes[0] * axes[1], w.Compute(Planar{ 0.0f, halfPi }), math::Tolerance()); +} + +TEST_F(TestManipulabilityIndex, condition_number_grows_near_singularity) +{ + EXPECT_NEAR(w.ConditionNumber(Planar{ 0.0f, halfPi }), 2.618034f, 2.618034f * 1e-3f); + EXPECT_NEAR(w.ConditionNumber(Planar{ 0.0f, 0.1f }), 49.963f, 49.963f * 1e-3f); + EXPECT_NEAR(w.ConditionNumber(Planar{ 0.0f, 0.01f }), 499.996f, 499.996f * 1e-3f); +} + +TEST_F(TestManipulabilityIndex, exactly_singular_condition_number_is_infinite) +{ + const float condition{ w.ConditionNumber(Planar{ 0.0f, 0.0f }) }; + + EXPECT_TRUE(std::isinf(condition) && condition > 0.0f); +} + +TEST_F(TestManipulabilityIndex, near_singular_flag_trips) +{ + EXPECT_TRUE(w.NearSingular(Planar{ 0.0f, 0.01f }, 0.05f)); + EXPECT_FALSE(w.NearSingular(Planar{ 0.0f, halfPi }, 0.05f)); +} diff --git a/robotics/kinematics/test/TestRedundancyResolution.cpp b/robotics/kinematics/test/TestRedundancyResolution.cpp new file mode 100644 index 0000000..6c61275 --- /dev/null +++ b/robotics/kinematics/test/TestRedundancyResolution.cpp @@ -0,0 +1,124 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/kinematics/DenavitHartenberg.hpp" +#include "robotics/kinematics/DhTaskJacobian.hpp" +#include "robotics/kinematics/RedundancyResolution.hpp" +#include +#include +#include + +namespace +{ + using DhLink = kinematics::DhLink; + using JointVector = math::Vector; + using PoseVector = math::Vector; + + constexpr float halfPi{ std::numbers::pi_v / 2.0f }; + constexpr float exactTolerance{ 1e-5f }; + + class TestRedundancyResolution + : public ::testing::Test + { + protected: + template + void ExpectNearZero(const math::Matrix& vector, float tolerance) const + { + for (std::size_t i = 0; i < Rows; ++i) + EXPECT_NEAR(vector[i], 0.0f, tolerance); + } + + template + void ExpectNearZero(const math::Matrix& matrix, float tolerance) const + { + for (std::size_t r = 0; r < Rows; ++r) + for (std::size_t c = 0; c < 7; ++c) + EXPECT_NEAR(matrix.at(r, c), 0.0f, tolerance); + } + + static float Trace(const math::SquareMatrix& matrix) + { + float trace{ 0.0f }; + for (std::size_t i = 0; i < 7; ++i) + trace += matrix.at(i, i); + return trace; + } + + kinematics::DenavitHartenberg arm{ { DhLink{ 0.0f, -halfPi, 0.34f, 0.0f }, DhLink{ 0.0f, halfPi, 0.0f, 0.0f }, DhLink{ 0.0f, halfPi, 0.4f, 0.0f }, + DhLink{ 0.0f, -halfPi, 0.0f, 0.0f }, DhLink{ 0.0f, -halfPi, 0.4f, 0.0f }, DhLink{ 0.0f, halfPi, 0.0f, 0.0f }, + DhLink{ 0.0f, 0.0f, 0.126f, 0.0f } }, + kinematics::DhConvention::Standard }; + kinematics::DhTaskJacobian pose{ arm }; + kinematics::DhTaskJacobian position{ arm }; + JointVector q{ 0.1f, 0.5f, -0.3f, -1.2f, 0.4f, 0.8f, -0.2f }; + PoseVector xDot{ 0.1f, -0.05f, 0.02f, 0.1f, 0.2f, -0.1f }; + JointVector qDot0{ 1.0f, -1.0f, 0.5f, 0.3f, -0.2f, 0.7f, -0.4f }; + }; +} + +TEST_F(TestRedundancyResolution, undamped_primary_task_is_exact) +{ + kinematics::RedundancyResolution resolution{ pose, 0.0f }; + + const auto qDot{ resolution.Resolve(q, xDot, qDot0) }; + const auto rightInverse{ pose.Jacobian(q) * resolution.DampedPseudoInverse(q) }; + + ExpectNearZero(PoseVector{ pose.Jacobian(q) * qDot - xDot }, exactTolerance); + for (std::size_t r = 0; r < 6; ++r) + for (std::size_t c = 0; c < 6; ++c) + EXPECT_NEAR(rightInverse.at(r, c), r == c ? 1.0f : 0.0f, math::Tolerance()); +} + +TEST_F(TestRedundancyResolution, damped_resolution_matches_reference) +{ + kinematics::RedundancyResolution resolution{ pose, 0.01f }; + const JointVector expected{ -0.093934f, 0.325324f, -0.092001f, 0.551001f, 0.088089f, 0.448837f, 0.140035f }; + + const auto qDot{ resolution.Resolve(q, xDot, qDot0) }; + + for (std::size_t i = 0; i < 7; ++i) + EXPECT_NEAR(qDot[i], expected[i], math::Tolerance()); +} + +TEST_F(TestRedundancyResolution, null_space_motion_is_task_invisible_despite_damping) +{ + kinematics::RedundancyResolution resolution{ pose, 0.01f }; + + const auto projector{ resolution.NullSpaceProjector(q) }; + + ExpectNearZero(PoseVector{ pose.Jacobian(q) * (projector * qDot0) }, exactTolerance); +} + +TEST_F(TestRedundancyResolution, secondary_objective_reduces_cost_without_moving_the_tool) +{ + kinematics::RedundancyResolution resolution{ pose, 0.0f }; + const JointVector gradient{ q * (1.0f / (2.9f * 2.9f)) }; + + const auto qDot{ resolution.Resolve(q, PoseVector{}, JointVector{ gradient * -1.0f }) }; + + EXPECT_NEAR((gradient.Transpose() * qDot).at(0, 0), -0.0051809f, 1e-5f); + ExpectNearZero(PoseVector{ pose.Jacobian(q) * qDot }, exactTolerance); +} + +TEST_F(TestRedundancyResolution, position_task_has_four_dimensional_null_space) +{ + kinematics::RedundancyResolution resolution{ position, 0.0f }; + + const auto projector{ resolution.NullSpaceProjector(q) }; + + EXPECT_NEAR(Trace(projector), 4.0f, math::Tolerance()); + ExpectNearZero(math::Matrix{ position.Jacobian(q) * projector }, exactTolerance); +} + +TEST_F(TestRedundancyResolution, rank_loss_grows_the_null_space) +{ + const JointVector stretched{ 0.1f, 0.0f, -0.3f, 0.0f, 0.4f, 0.0f, -0.2f }; + kinematics::RedundancyResolution undamped{ pose, 0.0f }; + kinematics::RedundancyResolution damped{ pose, 0.01f }; + + const auto projector{ undamped.NullSpaceProjector(stretched) }; + const auto qDot{ damped.Resolve(stretched, xDot, qDot0) }; + + EXPECT_NEAR(Trace(projector), 3.0f, math::Tolerance()); + ExpectNearZero(math::Matrix{ pose.Jacobian(stretched) * projector }, exactTolerance); + for (std::size_t i = 0; i < 7; ++i) + EXPECT_TRUE(std::isfinite(qDot[i])); +} diff --git a/robotics/kinematics/test_doubles/CMakeLists.txt b/robotics/kinematics/test_doubles/CMakeLists.txt new file mode 100644 index 0000000..2a60a79 --- /dev/null +++ b/robotics/kinematics/test_doubles/CMakeLists.txt @@ -0,0 +1,11 @@ +add_library(robotics.kinematics_test_doubles INTERFACE) +emil_build_for(robotics.kinematics_test_doubles BOOL ROBOTICS_TOOLBOX_BUILD_TESTS) + +target_link_libraries(robotics.kinematics_test_doubles INTERFACE + gmock + robotics.kinematics +) + +target_sources(robotics.kinematics_test_doubles PRIVATE + MockJacobianProvider.hpp +) diff --git a/robotics/kinematics/test_doubles/MockJacobianProvider.hpp b/robotics/kinematics/test_doubles/MockJacobianProvider.hpp new file mode 100644 index 0000000..ce2550e --- /dev/null +++ b/robotics/kinematics/test_doubles/MockJacobianProvider.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include "robotics/kinematics/JacobianProvider.hpp" +#include + +namespace kinematics +{ + template + class MockJacobianProvider + : public JacobianProvider + { + public: + using typename JacobianProvider::JointVector; + using typename JacobianProvider::TaskJacobian; + using typename JacobianProvider::TaskVector; + + MOCK_METHOD(TaskJacobian, Jacobian, (const JointVector& q), (const, override)); + MOCK_METHOD(TaskVector, BiasAcceleration, (const JointVector& q, const JointVector& qDot), (const, override)); + MOCK_METHOD(math::SE3Transform, ToolPose, (const JointVector& q), (const, override)); + }; +} diff --git a/robotics/planning/CMakeLists.txt b/robotics/planning/CMakeLists.txt index 7b172cf..f33f168 100644 --- a/robotics/planning/CMakeLists.txt +++ b/robotics/planning/CMakeLists.txt @@ -11,10 +11,12 @@ target_link_libraries(robotics.planning ${ROBOTICS_VISIBILITY} ) target_sources(robotics.planning PRIVATE + PotentialField.hpp PrimitiveDistance.hpp ) robotics_add_coverage_sources(robotics.planning + PotentialField.cpp PrimitiveDistance.cpp ) diff --git a/robotics/planning/PotentialField.cpp b/robotics/planning/PotentialField.cpp new file mode 100644 index 0000000..2db7ee5 --- /dev/null +++ b/robotics/planning/PotentialField.cpp @@ -0,0 +1,6 @@ +#include "robotics/planning/PotentialField.hpp" + +namespace planning +{ + template class PotentialField; +} diff --git a/robotics/planning/PotentialField.hpp b/robotics/planning/PotentialField.hpp new file mode 100644 index 0000000..d26077a --- /dev/null +++ b/robotics/planning/PotentialField.hpp @@ -0,0 +1,127 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Matrix.hpp" +#include +#include +#include + +namespace planning +{ + template + struct PotentialFieldParameters + { + static_assert(std::is_floating_point_v, "PotentialFieldParameters supports floating-point types"); + + T attractiveGain{}; + T conicDistance{}; + T repulsiveGain{}; + T influenceDistance{}; + T maxSpeed{}; + T stallSpeed{}; + T goalTolerance{}; + }; + + template + struct ObstacleProximity + { + static_assert(std::is_floating_point_v, "ObstacleProximity supports floating-point types"); + + math::Vector3 direction{}; + T distance{}; + }; + + enum class FieldStatus : std::uint8_t + { + Moving, + Reached, + Stalled + }; + + template + struct FieldCommand + { + static_assert(std::is_floating_point_v, "FieldCommand supports floating-point types"); + + math::Vector3 velocity{}; + T attractiveMagnitude{}; + T repulsiveMagnitude{}; + FieldStatus status{ FieldStatus::Moving }; + }; + + template + class PotentialField + { + static_assert(std::is_floating_point_v, "PotentialField supports floating-point types"); + + public: + using Vector3 = math::Vector3; + + explicit PotentialField(const PotentialFieldParameters& parameters); + + OPTIMIZE_FOR_SPEED FieldCommand Compute(const Vector3& position, const Vector3& goal, std::span> obstacles) const; + + private: + Vector3 Attraction(const Vector3& error, T distance) const; + Vector3 Repulsion(std::span> obstacles) const; + FieldStatus Status(T goalDistance, T forceMagnitude) const; + + PotentialFieldParameters parameters; + }; + + template + PotentialField::PotentialField(const PotentialFieldParameters& parameters) + : parameters{ parameters } + {} + + template + OPTIMIZE_FOR_SPEED FieldCommand PotentialField::Compute(const Vector3& position, const Vector3& goal, std::span> obstacles) const + { + const Vector3 error{ goal - position }; + const T goalDistance{ math::VectorNorm(error) }; + const Vector3 attraction{ Attraction(error, goalDistance) }; + const Vector3 repulsion{ Repulsion(obstacles) }; + const Vector3 force{ attraction + repulsion }; + const T magnitude{ math::VectorNorm(force) }; + + const Vector3 velocity{ magnitude <= parameters.maxSpeed ? force : Vector3{ force * (parameters.maxSpeed / magnitude) } }; + return { velocity, math::VectorNorm(attraction), math::VectorNorm(repulsion), Status(goalDistance, magnitude) }; + } + + template + typename PotentialField::Vector3 PotentialField::Attraction(const Vector3& error, T distance) const + { + if (distance <= parameters.conicDistance) + return error * parameters.attractiveGain; + + return error * (parameters.attractiveGain * parameters.conicDistance / distance); + } + + template + typename PotentialField::Vector3 PotentialField::Repulsion(std::span> obstacles) const + { + Vector3 repulsion{}; + for (const auto& obstacle : obstacles) + { + const T rho{ obstacle.distance }; + if (rho < parameters.influenceDistance) + repulsion = repulsion + obstacle.direction * (parameters.repulsiveGain * (T{ 1 } / rho - T{ 1 } / parameters.influenceDistance) / (rho * rho)); + } + + return repulsion; + } + + template + FieldStatus PotentialField::Status(T goalDistance, T forceMagnitude) const + { + if (goalDistance <= parameters.goalTolerance) + return FieldStatus::Reached; + + return forceMagnitude < parameters.stallSpeed ? FieldStatus::Stalled : FieldStatus::Moving; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class PotentialField; +#endif +} diff --git a/robotics/planning/test/CMakeLists.txt b/robotics/planning/test/CMakeLists.txt index 327bebd..47028c6 100644 --- a/robotics/planning/test/CMakeLists.txt +++ b/robotics/planning/test/CMakeLists.txt @@ -8,5 +8,6 @@ target_link_libraries(robotics.planning_test PUBLIC ) target_sources(robotics.planning_test PRIVATE + TestPotentialField.cpp TestPrimitiveDistance.cpp ) diff --git a/robotics/planning/test/TestPotentialField.cpp b/robotics/planning/test/TestPotentialField.cpp new file mode 100644 index 0000000..b72337f --- /dev/null +++ b/robotics/planning/test/TestPotentialField.cpp @@ -0,0 +1,87 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Tolerance.hpp" +#include "robotics/planning/PotentialField.hpp" +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Obstacle = planning::ObstacleProximity; + using planning::FieldStatus; + + class TestPotentialField + : public ::testing::Test + { + protected: + void ExpectNear(const Vector3& actual, const Vector3& expected) const + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], math::Tolerance()); + } + + planning::PotentialFieldParameters parameters{ 1.0f, 1.0f, 0.1f, 1.0f, 0.5f, 1e-3f, 0.05f }; + planning::PotentialField field{ parameters }; + Vector3 origin{}; + Vector3 farGoal{ 3.0f, 4.0f, 0.0f }; + }; +} + +TEST_F(TestPotentialField, near_goal_the_pull_is_quadratic) +{ + const auto command{ field.Compute(origin, Vector3{ 0.4f, 0.3f, 0.0f }, {}) }; + + ExpectNear(command.velocity, Vector3{ 0.4f, 0.3f, 0.0f }); + EXPECT_NEAR(command.attractiveMagnitude, 0.5f, math::Tolerance()); + EXPECT_EQ(command.status, FieldStatus::Moving); +} + +TEST_F(TestPotentialField, far_goal_the_pull_is_conic_and_clamped) +{ + const auto command{ field.Compute(origin, farGoal, {}) }; + + ExpectNear(command.velocity, Vector3{ 0.3f, 0.4f, 0.0f }); + EXPECT_NEAR(command.attractiveMagnitude, 1.0f, math::Tolerance()); +} + +TEST_F(TestPotentialField, obstacle_inside_the_influence_distance_repels) +{ + const std::array obstacles{ Obstacle{ Vector3{ 0.0f, -1.0f, 0.0f }, 0.5f } }; + + const auto command{ field.Compute(origin, farGoal, obstacles) }; + + EXPECT_NEAR(command.repulsiveMagnitude, 0.4f, math::Tolerance()); + ExpectNear(command.velocity, Vector3{ 0.416025f, 0.277350f, 0.0f }); +} + +TEST_F(TestPotentialField, obstacle_outside_the_influence_distance_is_ignored) +{ + const std::array obstacles{ Obstacle{ Vector3{ 0.0f, -1.0f, 0.0f }, 1.2f } }; + + const auto command{ field.Compute(origin, farGoal, obstacles) }; + + EXPECT_NEAR(command.repulsiveMagnitude, 0.0f, math::Tolerance()); + ExpectNear(command.velocity, Vector3{ 0.3f, 0.4f, 0.0f }); +} + +TEST_F(TestPotentialField, goal_within_tolerance_is_reached) +{ + EXPECT_EQ(field.Compute(origin, Vector3{ 0.03f, 0.0f, 0.0f }, {}).status, FieldStatus::Reached); +} + +TEST_F(TestPotentialField, opposing_obstacle_creates_a_detected_local_minimum) +{ + const Vector3 goal{ 3.0f, 0.0f, 0.0f }; + const std::array balanced{ Obstacle{ Vector3{ -1.0f, 0.0f, 0.0f }, 0.393003f } }; + const std::array closer{ Obstacle{ Vector3{ -1.0f, 0.0f, 0.0f }, 0.3f } }; + + const auto stalled{ field.Compute(origin, goal, balanced) }; + const auto pushed{ field.Compute(origin, goal, closer) }; + + EXPECT_NEAR(stalled.attractiveMagnitude, 1.0f, math::Tolerance()); + EXPECT_NEAR(stalled.repulsiveMagnitude, 1.0f, math::Tolerance()); + EXPECT_LT(math::VectorNorm(stalled.velocity), 1e-3f); + EXPECT_EQ(stalled.status, FieldStatus::Stalled); + EXPECT_EQ(pushed.status, FieldStatus::Moving); + ExpectNear(pushed.velocity, Vector3{ -0.5f, 0.0f, 0.0f }); +} diff --git a/robotics/trajectory/CMakeLists.txt b/robotics/trajectory/CMakeLists.txt index a025661..c5174fc 100644 --- a/robotics/trajectory/CMakeLists.txt +++ b/robotics/trajectory/CMakeLists.txt @@ -8,14 +8,17 @@ target_include_directories(robotics.trajectory ${ROBOTICS_VISIBILITY} target_link_libraries(robotics.trajectory ${ROBOTICS_VISIBILITY} infra.util numerical.math + numerical.motion_profiles ) target_sources(robotics.trajectory PRIVATE + CartesianSlerpInterpolation.hpp CubicSplineTrajectory.hpp TrajectoryTypes.hpp ) robotics_add_coverage_sources(robotics.trajectory + CartesianSlerpInterpolation.cpp CubicSplineTrajectory.cpp ) diff --git a/robotics/trajectory/CartesianSlerpInterpolation.cpp b/robotics/trajectory/CartesianSlerpInterpolation.cpp new file mode 100644 index 0000000..8a73c44 --- /dev/null +++ b/robotics/trajectory/CartesianSlerpInterpolation.cpp @@ -0,0 +1,7 @@ +#include "robotics/trajectory/CartesianSlerpInterpolation.hpp" +#include "numerical/motion_profiles/PolynomialProfile.hpp" + +namespace trajectory +{ + template class CartesianSlerpInterpolation>; +} diff --git a/robotics/trajectory/CartesianSlerpInterpolation.hpp b/robotics/trajectory/CartesianSlerpInterpolation.hpp new file mode 100644 index 0000000..926b4b1 --- /dev/null +++ b/robotics/trajectory/CartesianSlerpInterpolation.hpp @@ -0,0 +1,98 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/Quaternion.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "numerical/motion_profiles/PolynomialProfile.hpp" +#include +#include +#include + +namespace trajectory +{ + template + struct CartesianState + { + static_assert(std::is_floating_point_v, "CartesianState supports floating-point types"); + + math::SE3Transform pose{}; + math::Vector6 twist{}; + }; + + template + class CartesianSlerpInterpolation + { + static_assert(std::is_floating_point_v, "CartesianSlerpInterpolation supports floating-point types"); + + public: + using Transform = math::SE3Transform; + using Vector3 = math::Vector3; + using Quaternion = math::Quaternion; + + CartesianSlerpInterpolation(const Transform& start, const Transform& goal, const TimeLaw& timeLaw); + + OPTIMIZE_FOR_SPEED CartesianState Sample(T t) const; + T Duration() const; + + private: + static Quaternion ShortestArcGoal(const Quaternion& from, const Quaternion& to); + static Vector3 RotationVector(const Quaternion& delta); + + Vector3 p0; + Vector3 deltaP; + Quaternion q0; + Quaternion q1; + Vector3 rotationVector; + TimeLaw timeLaw; + }; + + template + CartesianSlerpInterpolation::CartesianSlerpInterpolation(const Transform& start, const Transform& goal, const TimeLaw& timeLaw) + : p0{ start.translation } + , deltaP{ goal.translation - start.translation } + , q0{ Quaternion::FromRotationMatrix(start.rotation) } + , q1{ ShortestArcGoal(q0, Quaternion::FromRotationMatrix(goal.rotation)) } + , rotationVector{ RotationVector(q1 * q0.Conjugate()) } + , timeLaw{ timeLaw } + {} + + template + OPTIMIZE_FOR_SPEED CartesianState CartesianSlerpInterpolation::Sample(T t) const + { + const auto law{ timeLaw.Sample(std::clamp(t, T{}, Duration())) }; + const T s{ std::clamp(law.position, T{}, T{ 1 }) }; + + CartesianState state{ Transform{ Quaternion::Slerp(q0, q1, s).ToRotationMatrix(), p0 + deltaP * s }, math::Vector6{} }; + state.twist.SetBlock(Vector3{ deltaP * law.velocity }, 0, 0); + state.twist.SetBlock(Vector3{ rotationVector * law.velocity }, 3, 0); + return state; + } + + template + T CartesianSlerpInterpolation::Duration() const + { + return timeLaw.Duration(); + } + + template + typename CartesianSlerpInterpolation::Quaternion CartesianSlerpInterpolation::ShortestArcGoal(const Quaternion& from, const Quaternion& to) + { + const T dot{ from.w * to.w + from.x * to.x + from.y * to.y + from.z * to.z }; + return dot < T{} ? -to : to; + } + + template + typename CartesianSlerpInterpolation::Vector3 CartesianSlerpInterpolation::RotationVector(const Quaternion& delta) + { + const Vector3 axis{ delta.x, delta.y, delta.z }; + const T sinHalfAngle{ math::Sqrt(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z) }; + const T scale{ sinHalfAngle > std::numeric_limits::epsilon() ? T{ 2 } * math::Atan2(sinHalfAngle, delta.w) / sinHalfAngle : T{ 2 } }; + return axis * scale; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class CartesianSlerpInterpolation>; +#endif +} diff --git a/robotics/trajectory/test/CMakeLists.txt b/robotics/trajectory/test/CMakeLists.txt index 83c2395..2ae0194 100644 --- a/robotics/trajectory/test/CMakeLists.txt +++ b/robotics/trajectory/test/CMakeLists.txt @@ -9,5 +9,6 @@ target_link_libraries(robotics.trajectory_test PUBLIC ) target_sources(robotics.trajectory_test PRIVATE + TestCartesianSlerpInterpolation.cpp TestCubicSplineTrajectory.cpp ) diff --git a/robotics/trajectory/test/TestCartesianSlerpInterpolation.cpp b/robotics/trajectory/test/TestCartesianSlerpInterpolation.cpp new file mode 100644 index 0000000..3ee3768 --- /dev/null +++ b/robotics/trajectory/test/TestCartesianSlerpInterpolation.cpp @@ -0,0 +1,136 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Tolerance.hpp" +#include "numerical/motion_profiles/PolynomialProfile.hpp" +#include "robotics/trajectory/CartesianSlerpInterpolation.hpp" +#include +#include +#include +#include + +namespace +{ + using Law = motion_profiles::PolynomialProfile; + using Path = trajectory::CartesianSlerpInterpolation; + using Transform = math::SE3Transform; + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + + constexpr float pi{ std::numbers::pi_v }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Matrix3 Rz(float angle) + { + return math::RotationAboutAxis(zAxis, angle); + } + + float AngleBetween(const Matrix3& a, const Matrix3& b) + { + const Matrix3 relative{ a.Transpose() * b }; + return std::acos(std::clamp((relative.Trace() - 1.0f) / 2.0f, -1.0f, 1.0f)); + } + + class TestCartesianSlerp + : public ::testing::Test + { + protected: + void ExpectNear(const Matrix3& actual, const Matrix3& expected) const + { + for (std::size_t r = 0; r < 3; ++r) + for (std::size_t c = 0; c < 3; ++c) + EXPECT_NEAR(actual.at(r, c), expected.at(r, c), math::Tolerance()); + } + + void ExpectNear(const Vector3& actual, const Vector3& expected) const + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], math::Tolerance()); + } + + void ExpectPose(const Transform& actual, const Transform& expected) const + { + ExpectNear(actual.rotation, expected.rotation); + ExpectNear(actual.translation, expected.translation); + } + + Law linearLaw{ { 0.0f, 1.0f, 1.0f, 1.0f }, 1.0f, motion_profiles::Degree::Cubic }; + Transform start{ Matrix3::Identity(), Vector3{} }; + Transform goal{ Rz(pi / 2.0f), Vector3{ 1.0f, 0.0f, 0.0f } }; + Path path{ start, goal, linearLaw }; + }; +} + +TEST_F(TestCartesianSlerp, endpoints_match_start_and_goal) +{ + ExpectPose(path.Sample(0.0f).pose, start); + ExpectPose(path.Sample(path.Duration()).pose, goal); +} + +TEST_F(TestCartesianSlerp, position_is_straight_line) +{ + ExpectNear(path.Sample(0.5f).pose.translation, Vector3{ 0.5f, 0.0f, 0.0f }); +} + +TEST_F(TestCartesianSlerp, rotation_stays_orthonormal) +{ + const Matrix3 rotation{ path.Sample(0.5f).pose.rotation }; + + ExpectNear(Matrix3{ rotation * rotation.Transpose() }, Matrix3::Identity()); + const float determinant{ rotation.at(0, 0) * (rotation.at(1, 1) * rotation.at(2, 2) - rotation.at(1, 2) * rotation.at(2, 1)) - rotation.at(0, 1) * (rotation.at(1, 0) * rotation.at(2, 2) - rotation.at(1, 2) * rotation.at(2, 0)) + rotation.at(0, 2) * (rotation.at(1, 0) * rotation.at(2, 1) - rotation.at(1, 1) * rotation.at(2, 0)) }; + EXPECT_NEAR(determinant, 1.0f, math::Tolerance()); +} + +TEST_F(TestCartesianSlerp, slerp_midpoint_is_half_angle) +{ + ExpectNear(path.Sample(0.5f).pose.rotation, Rz(pi / 4.0f)); +} + +TEST_F(TestCartesianSlerp, shortest_path_takes_short_arc) +{ + const Path reflex{ start, Transform{ Rz(3.0f * pi / 2.0f), goal.translation }, linearLaw }; + + const auto state{ reflex.Sample(0.5f) }; + + ExpectNear(state.pose.rotation, Rz(-pi / 4.0f)); + ExpectNear(state.twist.GetBlock<3, 1>(3, 0), Vector3{ 0.0f, 0.0f, -pi / 2.0f }); +} + +TEST_F(TestCartesianSlerp, near_parallel_orientation_is_finite) +{ + const float angle{ 0.01f * pi / 180.0f }; + const Path tiny{ start, Transform{ Rz(angle), goal.translation }, linearLaw }; + + const auto state{ tiny.Sample(0.5f) }; + + for (std::size_t i = 0; i < 6; ++i) + EXPECT_TRUE(std::isfinite(state.twist[i])); + for (std::size_t r = 0; r < 3; ++r) + for (std::size_t c = 0; c < 3; ++c) + EXPECT_TRUE(std::isfinite(state.pose.rotation.at(r, c))); + ExpectNear(state.pose.rotation, Matrix3::Identity()); + EXPECT_NEAR(state.twist[5], angle, 1e-6f); +} + +TEST_F(TestCartesianSlerp, constant_angular_rate_for_linear_time_law) +{ + for (const float t : { 0.25f, 0.5f, 0.75f }) + EXPECT_NEAR(AngleBetween(start.rotation, path.Sample(t).pose.rotation), t * pi / 2.0f, math::Tolerance()); +} + +TEST_F(TestCartesianSlerp, twist_is_sdot_times_path_tangent) +{ + const Path quintic{ start, goal, Law{ { 0.0f, 1.0f }, 2.0f, motion_profiles::Degree::Quintic } }; + + const auto middle{ quintic.Sample(1.0f).twist }; + const auto atRest{ quintic.Sample(0.0f).twist }; + + ExpectNear(middle.GetBlock<3, 1>(0, 0), Vector3{ 0.9375f, 0.0f, 0.0f }); + ExpectNear(middle.GetBlock<3, 1>(3, 0), Vector3{ 0.0f, 0.0f, 1.472622f }); + for (std::size_t i = 0; i < 6; ++i) + EXPECT_NEAR(atRest[i], 0.0f, math::Tolerance()); +} + +TEST_F(TestCartesianSlerp, sample_clamps_outside_domain) +{ + ExpectPose(path.Sample(-1.0f).pose, start); + ExpectPose(path.Sample(path.Duration() + 1.0f).pose, goal); +}