From 26ddc257f08ff01706a3d3f0e64a3e6f9de3d68d Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 11:12:10 +0000 Subject: [PATCH 1/4] feat(dynamics): add generic JointLink with prismatic joints, limits and armature (M1) JointLink replaces the revolute-only link (RevoluteJointLink stays as an alias, so existing aggregate initializers compile unchanged). FK, IK, RNEA, ABA and CRBA now work through each joint's motion subspace, so revolute and prismatic joints share one code path; armature adds to the joint-space inertia. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 14 +-- ROADMAP.md | 12 +- doc/dynamics/ArticulatedBodyAlgorithm.md | 5 + doc/dynamics/CompositeRigidBodyAlgorithm.md | 6 +- doc/dynamics/JointLink.md | 109 ++++++++++++++++++ doc/dynamics/README.md | 1 + doc/dynamics/RecursiveNewtonEuler.md | 10 +- doc/kinematics/ForwardKinematics.md | 5 +- roadmap/README.md | 2 +- .../dynamics/GenericJointLink/explanation.md | 32 ----- .../GenericJointLink/implementation.md | 86 -------------- roadmap/dynamics/GenericJointLink/tests.md | 47 -------- .../ChainPoseKinematics/implementation.md | 4 +- .../dynamics/ArticulatedBodyAlgorithm.hpp | 87 +++++++------- robotics/dynamics/CMakeLists.txt | 2 + .../dynamics/CompositeRigidBodyAlgorithm.hpp | 42 ++++--- .../dynamics/ForwardDynamicsIntegrator.hpp | 6 +- robotics/dynamics/JointLink.cpp | 6 + robotics/dynamics/JointLink.hpp | 83 +++++++++++++ robotics/dynamics/RecursiveNewtonEuler.hpp | 86 +++++--------- robotics/dynamics/RevoluteJointLink.hpp | 27 +---- robotics/dynamics/SpatialInertia.hpp | 4 +- robotics/dynamics/test/CMakeLists.txt | 1 + .../test/TestArticulatedBodyAlgorithm.cpp | 25 ++++ robotics/dynamics/test/TestJointLink.cpp | 100 ++++++++++++++++ .../test/TestRecursiveNewtonEuler.cpp | 31 +++++ robotics/kinematics/ForwardKinematics.hpp | 16 +-- robotics/kinematics/InverseKinematics.hpp | 9 +- 28 files changed, 507 insertions(+), 351 deletions(-) create mode 100644 doc/dynamics/JointLink.md delete mode 100644 roadmap/dynamics/GenericJointLink/explanation.md delete mode 100644 roadmap/dynamics/GenericJointLink/implementation.md delete mode 100644 roadmap/dynamics/GenericJointLink/tests.md create mode 100644 robotics/dynamics/JointLink.cpp create mode 100644 robotics/dynamics/JointLink.hpp create mode 100644 robotics/dynamics/test/TestJointLink.cpp diff --git a/README.md b/README.md index d8f185a..f1fbf7b 100644 --- a/README.md +++ b/README.md @@ -35,13 +35,13 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Composite Rigid Body Algorithm (CRBA), Forward-Dynamics Integration | -| [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 | +| Category | Description | +|--------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | +| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), 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 | 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 fe5d651..3dafae9 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -98,10 +98,10 @@ tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematic the **full-pose kinematics, model plumbing, planning and control layer** that turns those models into a usable manipulator stack. Two current limitations gate many items below: -> **Position-only, revolute-only.** [ForwardKinematics.hpp](robotics/kinematics/ForwardKinematics.hpp) -> returns joint and tool *positions* (a 3×N Jacobian lives privately inside IK), and only -> [RevoluteJointLink](robotics/dynamics/RevoluteJointLink.hpp) exists. Items **M1** (prismatic joints, -> limits, armature), **M30** (full-pose kinematics) and **M8** (6×N Jacobian) lift these limits. +> **Position-only.** [ForwardKinematics.hpp](robotics/kinematics/ForwardKinematics.hpp) returns joint +> and tool *positions* (a 3×N Jacobian lives privately inside IK). Links are generic +> ([JointLink](robotics/dynamics/JointLink.hpp): revolute or prismatic, limits, armature). Items **M30** +> (full-pose kinematics) and **M8** (6×N Jacobian) lift the position-only limit. **Conventions (normative for every item):** 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches `(f; n)` — as defined by upstream `math::SE3Transform` (**NT53**); the geometric Jacobian @@ -117,7 +117,6 @@ existing `dynamics/` convention. | # | Component | Target module | Depends on | Difficulty | |-----|-----------------------------------------------------------------|---------------------------|----------------|------------| -| M1 | Generic joint link (prismatic, limits, armature) | `dynamics` + `kinematics` | — | ★☆☆☆☆ | | M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | | M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | | M30 | Chain pose kinematics (full-pose FK, frame chain) | `kinematics` | NT53 | ★★☆☆☆ | @@ -149,9 +148,6 @@ retired here. ### Tier 1 — Trivial ★☆☆☆☆ -**M1. Generic joint link.** Evolve `RevoluteJointLink` (compatibly, via trailing defaulted fields) into a link with a joint type (revolute/prismatic), joint limits and armature, and add the prismatic branches to FK, RNEA, ABA, CRBA and the Jacobian. -- *Algorithm / paper:* J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 and 6; Featherstone (2008), Ch. 4. - **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). diff --git a/doc/dynamics/ArticulatedBodyAlgorithm.md b/doc/dynamics/ArticulatedBodyAlgorithm.md index 8eff629..584de50 100644 --- a/doc/dynamics/ArticulatedBodyAlgorithm.md +++ b/doc/dynamics/ArticulatedBodyAlgorithm.md @@ -52,6 +52,11 @@ $$p^a_i = p^A_i + \hat{I}^A_i c_i + U_i \frac{u_i}{D_i}, \qquad p^A_{\lambda(i)} where $p^A_i$ starts as the rigid-body bias force $v_i \times^* I_i v_i$ (gyroscopic and centripetal terms). +### Prismatic Joints and Armature + +With the motion subspace $S_i$ — $(\hat{z}_i; 0)$ for a revolute joint, $(0; \hat{z}_i)$ for a prismatic one — the projections are $U_i = I^A_i S_i$, $D_i = S_i^T U_i + J_{a,i}$ and $u_i = \tau_i - S_i^T p^A_i$. The velocity-product term is $c_i = v_i \times S_i \dot{q}_i$. +Every transform to the parent uses the joint offset at the current displacement, and the armature $J_{a,i}$ (reflected rotor inertia) enters only through $D_i$. + ## Complexity Analysis | Case | Time | Space | Notes | diff --git a/doc/dynamics/CompositeRigidBodyAlgorithm.md b/doc/dynamics/CompositeRigidBodyAlgorithm.md index 06a0d65..f8b1f38 100644 --- a/doc/dynamics/CompositeRigidBodyAlgorithm.md +++ b/doc/dynamics/CompositeRigidBodyAlgorithm.md @@ -49,6 +49,10 @@ $$f \leftarrow R_j f, \qquad n \leftarrow R_j n + r_j \times f$$ projected on each ancestor axis gives $M_{ji} = M_{ij} = \hat{z}_j^T n$ for $j < i$. Each column costs one projection per ancestor, which gives the triangular operation count. +### Prismatic Joints and Armature + +For a prismatic joint the unit motion is $(0; \hat{z}_i)$, so the column wrench is $I^c_i (0; \hat{z}_i)$ and each projection uses the force part. With the motion subspace $S_j$ both cases read $M_{ji} = S_j^T (n; f)$. Transforms use the joint offsets at the current displacements. Reflected actuator inertia $J_{a,i} = k_r^2 I_{\text{rotor}}$ adds to the diagonal entry $M_{ii}$ only. + ### Properties - $M$ is symmetric, and positive definite whenever every composite body has non-zero inertia about its joint axis. @@ -86,8 +90,6 @@ These are the textbook closed-form entries of the planar two-link arm. ## Variants & Generalizations -- **Armature and rotor inertia**: add $k_r^2 I_{\text{rotor}}$ to each diagonal entry. -- **Prismatic joints**: the unit motion is $(0; \hat{z}_i)$, so the column wrench becomes $I^c_i (0; \hat{z}_i)$ and the projection uses the force part. - **Floating base**: the base becomes a 6-DOF joint and $M$ gains a $6 \times 6$ block and coupling rows (Featherstone, Ch. 9). - **Branched trees**: the ancestor loop follows the parent array instead of $j - 1$. Sparsity can then be exploited in the factorization. - **Column-by-column RNEA**: $M e_j = \mathrm{ID}(q, 0, e_j)$ with gravity off. Simpler to derive and useful as a test oracle, but $n$ times the cost of one inverse-dynamics pass. diff --git a/doc/dynamics/JointLink.md b/doc/dynamics/JointLink.md new file mode 100644 index 0000000..302d6ac --- /dev/null +++ b/doc/dynamics/JointLink.md @@ -0,0 +1,109 @@ +# Generic Joint Link + +## Overview & Motivation + +A serial manipulator is a chain of rigid links joined by one-degree-of-freedom joints. Most textbook derivations assume every joint is **revolute**, but real machines are rarely all-revolute. SCARA arms, gantries, linear axes, telescoping booms and hydraulic rams contain **prismatic** (sliding) joints. +Two other per-joint quantities matter as much as the geometry: + +- **Joint limits** bound position, velocity and effort. Every planner, trajectory generator and inverse-kinematics solver needs them. +- **Armature** is the actuator inertia reflected through the gearbox, $k_r^2 I_{\text{rotor}}$. On small geared arms it often dominates the effective inertia of the proximal joints, and ignoring it makes model-based control markedly worse. + +The generic joint link carries the joint type, its limits and its armature next to the usual mass, inertia and geometry. Every kinematic and dynamic recursion then treats the joint through one abstraction, its **motion subspace**. + +## Mathematical Theory + +### Link Description + +Link $i$ carries: + +- mass $m_i$, rotational inertia $\bar{I}_i$ about its centre of mass, and centre of mass $c_i$, all in the link frame; +- a unit joint axis $\hat{z}_i$: the rotation axis of a revolute joint, or the slide direction of a prismatic one; +- the joint origin $r_i$ at $q_i = 0$, expressed in the parent frame; +- the joint type, limits $[q_{\min}, q_{\max}]$, $|\dot{q}| \le \dot{q}_{\max}$, $|\tau| \le \tau_{\max}$ (unbounded by default), and armature $J_{a,i}$. + +### Joint Transform + +The pose of the link frame relative to its parent depends on the joint variable: + +$$\text{revolute: } \left(R(\hat{z}_i, q_i),\; r_i\right), \qquad \text{prismatic: } \left(\mathbf{I}_3,\; r_i + \hat{z}_i\, q_i\right)$$ + +A revolute joint rotates the frame about a fixed origin. A prismatic joint translates the origin along a fixed direction without rotating the frame, so its axis may be read in either the parent or the link frame. + +### Motion Subspace + +In spatial notation (angular part first), a one-degree-of-freedom joint moves along + +$$S_i = \begin{bmatrix} s_{\omega,i} \\ s_{v,i} \end{bmatrix} = \begin{cases} (\hat{z}_i;\ 0) & \text{revolute} \\ (0;\ \hat{z}_i) & \text{prismatic} \end{cases}$$ + +The joint contributes the velocity $S_i \dot{q}_i$ to its link, and the actuator torque or force is the projection of the transmitted wrench onto the same subspace: + +$$\tau_i = s_{\omega,i}^T n_i + s_{v,i}^T f_i + J_{a,i}\,\ddot{q}_i$$ + +For revolute joints this is the familiar axis–torque projection; for prismatic joints it becomes an axis–force projection. Writing the recursions in terms of $S_i$ makes one implementation serve both joint types without branching. + +### What Changes in the Recursions + +- **Velocity**: $\omega_i = R_i^T \omega_{i-1} + s_{\omega,i}\dot{q}_i$ and $v_i = R_i^T(v_{i-1} + \omega_{i-1} \times r_i(q)) + s_{v,i}\dot{q}_i$. +- **Acceleration of the joint origin**: a prismatic joint on a rotating link adds the Coriolis term $2\,\omega_i \times (\hat{z}_i \dot{q}_i)$ and the relative acceleration $\hat{z}_i \ddot{q}_i$. +- **Offsets**: every transform to the parent uses $r_i(q)$, which grows with a prismatic joint's displacement. +- **Inertia**: the armature adds only to the joint's own diagonal entry of the mass matrix and to the articulated inertia $D_i = S_i^T I^A_i S_i + J_{a,i}$. It changes neither kinematics nor gravity. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|------------------|--------|--------|-------------------------------------------------------| +| Joint transform | $O(1)$ | $O(1)$ | One axis–angle rotation (revolute) or one scaled add | +| Motion subspace | $O(1)$ | $O(1)$ | Selects the angular or linear channel | +| Per-link storage | — | $O(1)$ | Type byte, four limit scalars and one armature scalar | + +The recursions keep their order: $O(n)$ for forward kinematics, inverse dynamics and forward dynamics, and $O(n^2)$ for the mass matrix. + +## Step-by-Step Walkthrough + +**Vertical slider.** A $2$ kg carriage slides along $+z$ under gravity $9.81$ m/s² and accelerates upwards at $1$ m/s². Its joint origin accelerates at $a = g + \ddot{q}$ upwards relative to the free-fall frame, and the actuator force is the projection of $m a$ on the slide axis: + +$$\tau = m(g + \ddot{q}) = 2 \cdot (9.81 + 1) = 21.62\ \text{N}$$ + +**Slider on a turntable.** A turntable spins about $z$ at $\omega = 1.5$ rad/s with $\dot{\omega} = 0.4$ rad/s². A $2$ kg point mass sits on a radial slider at $r = 0.5$ m, extending at $\dot{r} = 0.3$ m/s with $\ddot{r} = 0.2$ m/s². The base torque is + +$$\tau_0 = m r^2 \dot{\omega} + 2 m r \dot{r} \omega = 0.2 + 0.9 = 1.1\ \text{N·m}$$ + +where the second term is the Coriolis contribution. The slider force is $\tau_1 = m(\ddot{r} - r\omega^2) = -1.85$ N, which is the centripetal pull. + +**Armature.** A uniform $1$ kg, $1$ m rod with $0.2$ kg·m² of armature has effective inertia $\tfrac13 + 0.2 = 0.533$ kg·m² about its joint, in the inverse dynamics, the forward dynamics and the mass matrix alike. + +## Pitfalls & Edge Cases + +- **Forgetting the Coriolis term**: a slider riding on a rotating link feels $2\omega \times \hat{z}\dot{q}$. Revolute-only code that is "extended" by swapping the projection silently drops it. +- **Projecting the wrong channel**: prismatic joints must project the force, not the moment. A torque projection on a prismatic joint returns a meaningless value with the right units. +- **Non-unit axes**: a non-unit axis scales the rotation angle of a revolute joint and the displacement of a prismatic one. Normalize axes when the model is built. +- **Limits are not enforced by the dynamics**: limits are data for planners, inverse kinematics and safety layers. Inverse and forward dynamics never clamp. +- **Armature units**: kg·m² for revolute joints and kg for prismatic joints. Reflected inertia scales with the square of the gear ratio, so a small rotor behind a 100:1 gearbox easily outweighs a light link. +- **Singular models**: a prismatic joint whose outboard mass is zero and has no armature gives $D_i = 0$; forward dynamics through it is undefined. + +## Variants & Generalizations + +- **Helical (screw) joints**: $S = (\hat{z};\ p\,\hat{z})$ with pitch $p$ couples rotation and translation in one degree of freedom. +- **Multi-degree-of-freedom joints**: spherical, planar and floating joints have motion subspaces with several columns. They need the matrix form of the projections and the articulated-inertia update. +- **Denavit–Hartenberg descriptions**: the same joint types appear as variable $\theta_i$ (revolute) or variable $d_i$ (prismatic) in DH tables. +- **Motor models**: armature generalizes to a full rotor model with gyroscopic coupling for high-speed joints, and friction enters as an extra joint torque. + +## Applications + +- SCARA arms, Cartesian gantries and pick-and-place machines with linear axes. +- Telescopic manipulators, linear actuators and hydraulic cylinders modelled as prismatic joints. +- Accurate inverse dynamics and computed-torque control of geared arms, where armature dominates. +- Limit-aware planning, inverse kinematics and trajectory time-scaling. + +## Connections to Other Algorithms + +- [Forward Kinematics](../kinematics/ForwardKinematics.md): composes the joint transforms along the chain. +- [Recursive Newton-Euler](RecursiveNewtonEuler.md): propagates velocities and accelerations through each joint's motion subspace and projects wrenches back onto it. +- [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md): projects articulated inertias onto the motion subspace, with the armature added to $D_i$. +- [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md): builds mass-matrix columns from the composite inertia acting on the motion subspace, plus the armature on the diagonal. + +## References & Further Reading + +- Craig, J.J. (2005). *Introduction to Robotics: Mechanics and Control*. 3rd ed. Pearson. Chapters 3 and 6. +- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. Chapter 4. +- Spong, M.W., Hutchinson, S. and Vidyasagar, M. (2020). *Robot Modeling and Control*. 2nd ed. Wiley. Chapter 6. diff --git a/doc/dynamics/README.md b/doc/dynamics/README.md index e10761a..a1a75b2 100644 --- a/doc/dynamics/README.md +++ b/doc/dynamics/README.md @@ -10,5 +10,6 @@ Rigid-body dynamics algorithms for robotics and mechanical systems. | [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 | diff --git a/doc/dynamics/RecursiveNewtonEuler.md b/doc/dynamics/RecursiveNewtonEuler.md index 0cf1407..87f0501 100644 --- a/doc/dynamics/RecursiveNewtonEuler.md +++ b/doc/dynamics/RecursiveNewtonEuler.md @@ -18,7 +18,7 @@ The joint torque at each joint is then the projection of the net torque onto the ### Kinematic Chain Model -A serial chain of $n$ rigid bodies (links) connected by revolute joints. Each link $i$ is described by: +A serial chain of $n$ rigid bodies (links) connected by revolute or prismatic joints. Each link $i$ is described by: - $m_i$ — mass - $I_i \in \mathbb{R}^{3 \times 3}$ — inertia tensor at the center of mass, in the link frame @@ -59,6 +59,14 @@ $$\tau_i = I_i \dot{\omega}_i + \omega_i \times (I_i \omega_i) + r_{j \to c}^{(i **Joint torque:** $$\tau_{q_i} = \hat{z}_i^T \tau_i$$ +### Prismatic Joints and Armature + +A prismatic joint slides along $\hat{z}_i$ instead of rotating about it. Its frame keeps the parent's orientation ($R_i = \mathbf{I}_3$), its offset becomes $r_{p \to j}^{(i)} + \hat{z}_i q_i$, and it adds no angular velocity. The joint-origin acceleration gains a Coriolis and a relative term: + +$$a_{J_i} = R_i^T \left( a_{J_{i-1}} + \dot{\omega}_{i-1} \times r^{(i)} + \omega_{i-1} \times (\omega_{i-1} \times r^{(i)}) \right) + 2\,\omega_i \times (\dot{q}_i \hat{z}_i) + \ddot{q}_i \hat{z}_i$$ + +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. + ### 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/ForwardKinematics.md b/doc/kinematics/ForwardKinematics.md index 9511e8f..f5f5d85 100644 --- a/doc/kinematics/ForwardKinematics.md +++ b/doc/kinematics/ForwardKinematics.md @@ -29,6 +29,10 @@ $$R(\hat{u}, \theta) = \cos\theta\,\mathbf{I}_3 + (1 - \cos\theta)\,\hat{u}\hat{ The product is taken left to right because every axis is expressed in its own link frame: $^{0}R_{i}$ maps vectors written in link $i$ coordinates into base coordinates. Rotations are right-handed: a positive angle about $\hat{y}$ carries $\hat{x}$ towards $-\hat{z}$. +### Prismatic Joints + +A prismatic joint slides its link along $\hat{z}_k$ instead of rotating it: its factor in the orientation product is $\mathbf{I}_3$, and its offset becomes $r_k + \hat{z}_k q_k$. The recursions below use the offset at the current joint value, so revolute and prismatic joints mix freely. + ### Position Recursion $$p_0 = r_0, \qquad p_{i+1} = p_i + {}^{0}R_{i}\, r_{i+1} \quad (0 \le i < n - 1), \qquad p_{\text{tool}} = p_{n-1} + {}^{0}R_{n-1}\, t$$ @@ -62,7 +66,6 @@ Consider a 2-link arm with $y$-axis joints, joint 2 located $1$ along $\hat{x}$ - **Planar vs. spatial chains**: in purely planar manipulators all axes are parallel and the rotations reduce to 2D trigonometry, but the algorithmic structure (one pass accumulating transforms) is the same. - **Full pose**: returning $^{0}R_{n-1}$ together with $p_{\text{tool}}$ gives the complete tool pose needed for orientation control. -- **Prismatic joints**: $R_k$ stays constant while the joint offset becomes a function of the joint displacement; the same forward sweep applies. - **Different parameterizations**: Denavit–Hartenberg, modified DH, or product-of-exponentials formulations all recursively compose rigid transforms along the chain. - **Multiple end-effectors**: for branched chains the routine runs per branch, reusing shared prefixes. diff --git a/roadmap/README.md b/roadmap/README.md index 4769531..1410080 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -92,7 +92,7 @@ prerequisites. `DenavitHartenberg` (M7) · `GeometricJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) · `ChainPoseKinematics` (M30) ### `dynamics` -`GenericJointLink` (M1) · `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) +`MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) ### `trajectory` `CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) diff --git a/roadmap/dynamics/GenericJointLink/explanation.md b/roadmap/dynamics/GenericJointLink/explanation.md deleted file mode 100644 index 118ff62..0000000 --- a/roadmap/dynamics/GenericJointLink/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# Generic Joint Link — Overview - -## What it is -One link description that covers both revolute (rotating) and prismatic (sliding) joints and also -carries joint limits and the actuator's reflected inertia (armature). - -## Why it matters (embedded) -Real machines are rarely all-revolute: SCARA arms, gantries, linear axes and hydraulic rams contain -sliding joints. Limits are needed by every planner and inverse-kinematics solver, and the armature of -geared actuators often dominates the effective inertia of small arms — ignoring it makes -model-based control markedly worse. - -## How it works (intuition) -Every one-degree-of-freedom joint moves along a screw axis: a revolute joint spends its motion in -the angular channel, a prismatic joint in the linear channel. The kinematic and dynamic recursions -only need to know which channel the joint drives; for prismatic joints the torque projection becomes -a force projection and an extra Coriolis term appears when the slider rides on a rotating link. -Armature adds a rotor inertia that only the joint itself feels. - -## Key parameters -- **type** — revolute or prismatic. -- **axis** — unit rotation axis or slide direction. -- **limits** — position, velocity and effort bounds. -- **armature** — reflected actuator inertia. - -## Reference -J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 and 6; -R. Featherstone, *Rigid Body Dynamics Algorithms* (2008), Ch. 4 (joint models). - -## See also -`RecursiveNewtonEuler`, `ArticulatedBodyAlgorithm`, `CompositeRigidBodyAlgorithm` (M28), -`ChainPoseKinematics` (M30), `DenavitHartenberg` (M7). diff --git a/roadmap/dynamics/GenericJointLink/implementation.md b/roadmap/dynamics/GenericJointLink/implementation.md deleted file mode 100644 index e4ac05f..0000000 --- a/roadmap/dynamics/GenericJointLink/implementation.md +++ /dev/null @@ -1,86 +0,0 @@ -# Generic (Revolute / Prismatic) Joint Link with Limits and Armature — Implementation Pseudocode - -> Roadmap ref: #M1 (Tier 1) · Target: `robotics/dynamics` (+ changes in `kinematics`) · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -enum class JointType : uint8_t { Revolute, Prismatic } # shared with M7 / M30 - -template # static_assert(std::is_floating_point_v); instantiated for float -struct JointLimits: - T positionMin = −∞, positionMax = +∞ # rad or m - T velocityMax = +∞ # rad/s or m/s - T effortMax = +∞ # N·m or N - -template -struct JointLink: # evolves the shipped RevoluteJointLink - T mass - math::SquareMatrix inertia # at the CoM, link frame - math::Vector jointAxis # unit, link frame (rotation axis or slide direction) - math::Vector parentToJoint # joint origin at q = 0, parent frame - math::Vector jointToCoM # link frame - JointType type = JointType::Revolute # new fields are trailing and defaulted, - JointLimits limits = {} # so every existing aggregate - T armature = 0 # initializer keeps compiling - -template using RevoluteJointLink = JointLink # migration alias -``` - -`armature` is the reflected actuator inertia (`G²·I_rotor`, kg·m² for revolute, kg for prismatic). - -## Interface - -```text -(Matrix3 R, Vector3 offset) JointTransform(T q) const # link frame relative to parent; hot path -Vector3 AngularAxis() const # axis if Revolute else 0 # motion subspace S = (AngularAxis; LinearAxis) -Vector3 LinearAxis() const # axis if Prismatic else 0 # in the dynamics' (ω; v) ordering -``` - -## Algorithm (pseudocode) - -```cpp -function JointTransform(q): # OPTIMIZE_FOR_SPEED - if type == Revolute: return (RotationAboutAxis(jointAxis, q), parentToJoint) - else: return (Identity, parentToJoint + jointAxis·q) - -# Required changes in the shipped algorithms (per joint, link frame = parent rotated by R, offset r): -FK / ChainPoseKinematics (M30): origin_i = origin_{i−1} + R_{i−1}·r_i ; R_i = R_{i−1}·R(q_i) - Jacobian column (M8): revolute (z × (p − o); z), prismatic (z; 0) -RNEA forward pass, prismatic i: ω_i = Rᵀω_{i−1} = ω_{i−1}; ω̇_i = ω̇_{i−1} - a_i = a_{i−1} + ω̇_{i−1}×r_i + ω_{i−1}×(ω_{i−1}×r_i) + 2·ω_i×(z·q̇_i) + z·q̈_i -RNEA backward pass: child offset is r_{i+1} (= parentToJoint + z·q for a prismatic child) - τ_i = z·n_i (revolute) | τ_i = z·f_i (prismatic) - τ_i += armature_i·q̈_i -ABA (Featherstone, S = (0; z) for prismatic): U = I^A·S, D = SᵀU + armature, u = τ − Sᵀp^A, - c_i = v_i × S·q̇_i, transforms use r_i(q) -CRBA (M28): S as above; M_ii += armature_i -Limits: consumed by IK clamping (M13), trajectory planners (NT55/NT56/NT60, M33), TOPP (M27) -``` - -## Complexity & memory - -- `JointTransform`: `O(1)`. The algorithm changes keep every pass `O(N)` (`O(N²)` for CRBA). -- Memory: one enum byte, four limit scalars and the armature per link. - -## Numerical / embedded notes - -- **Migration:** keep the aggregate order of the shipped struct and append defaulted fields; alias - `RevoluteJointLink` so FK/IK/RNEA/ABA and their tests compile unchanged, then add the prismatic - branches behind `type`. -- The prismatic `2ω×(z·q̇)` Coriolis term and the `z·f` projection are the two places a revolute-only - implementation silently goes wrong — both are pinned by tests. -- Keep `jointAxis` unit-normalized (assert at construction); a non-unit axis rescales both the angle - and the slide. -- Armature adds to the joint-space inertia only; it does not change the kinematics or gravity. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/dynamics/JointLink.hpp` (and `RevoluteJointLink.hpp` becomes the alias) — - `#pragma once`, `extern template struct JointLink;` - under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/dynamics/JointLink.cpp` → `template struct JointLink;` -- Tests: `robotics/dynamics/test/TestJointLink.cpp` plus prismatic cases in the RNEA/ABA/FK tests. -- Doc: `doc/dynamics/JointLink.md` (per `doc/TEMPLATE.md`) and updates to the RNEA/ABA/FK docs. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/GenericJointLink/tests.md b/roadmap/dynamics/GenericJointLink/tests.md deleted file mode 100644 index 166ff27..0000000 --- a/roadmap/dynamics/GenericJointLink/tests.md +++ /dev/null @@ -1,47 +0,0 @@ -# Generic Joint Link — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestJointLink : public ::testing::Test: - Vector3 x = {1, 0, 0}, z = {0, 0, 1} - JointLink MakeRevolute(axis, parentToJoint), MakePrismatic(axis, parentToJoint) -# each case below is a TEST_F(TestJointLink, ) (algorithm cases live in the RNEA/ABA/FK tests) -``` - -## Test cases (Arrange / Act / Assert) - -```text -existing_aggregate_initializers_default_to_revolute: - Arrange: RevoluteJointLink{ m, I, axis, offset, com } - Assert: type == Revolute, armature == 0, limits unbounded -revolute_transform_rotates_about_axis: - Assert: JointTransform(π/2) about z maps x → y, offset == parentToJoint -prismatic_transform_slides_along_axis: - Assert: JointTransform(0.3) along x == (I, parentToJoint + (0.3, 0, 0)) -motion_subspace_selects_one_channel: - Assert: revolute → (axis; 0), prismatic → (0; axis) -vertical_slider_needs_weight_plus_inertial_force: # RNEA test file - Arrange: prismatic along z, mass m, gravity (0, 0, −g), q̈ = a - Assert: τ = m·(g + a) -prismatic_coriolis_term_on_rotating_base: # RNEA test file - Arrange: revolute base about z spinning at ω, prismatic slider along x at q = r, q̇ = v - Assert: base torque = 2·m·r·v·ω (Coriolis) + inertial terms per closed form -mixed_chain_round_trips_through_aba: # ABA test file - Assert: ABA(RNEA(q̈)) ≈ q̈ for a revolute–prismatic–revolute chain under gravity -armature_adds_to_diagonal_inertia: - Assert: single rod: τ = (m l²/3 + armature)·q̈ in RNEA; ABA inverts it; CRBA M = m l²/3 + armature -``` - -## Reference vectors - -- Slider: `m = 2`, `a = 1`: `τ = 2·(9.81 + 1) = 21.62 N`. -- Rotating slider (point mass at `r` on an arm spinning at `ω`, extending at `v`): Coriolis torque - `2 m r v ω` about the base axis. - -## Edge cases - -- Negative `q` retracts the slider / reverses the rotation. -- Full `2π` revolute wrap returns to identity. diff --git a/roadmap/kinematics/ChainPoseKinematics/implementation.md b/roadmap/kinematics/ChainPoseKinematics/implementation.md index 4248510..61ffb6f 100644 --- a/roadmap/kinematics/ChainPoseKinematics/implementation.md +++ b/roadmap/kinematics/ChainPoseKinematics/implementation.md @@ -5,7 +5,7 @@ The shipped `ForwardKinematics` returns joint origins and the tool point only. Pose-level algorithms (M8, M13, M17–M19) also need the tool **orientation** and every joint **axis** in the base frame. This spec extends the same recursion over the existing link model (`RevoluteJointLink`, and -`GenericJointLink` once M1 lands) and produces a model-independent `FrameChain` — the single input +`JointLink` since M1) and produces a model-independent `FrameChain` — the single input the geometric Jacobian (M8) consumes, whether the frames come from this link model, from DH (M7) or from PoE (M15). @@ -43,7 +43,7 @@ function Compute(links, q): # OPTIMIZE_FOR_SPEED if i > 0: o = o + R · links[i].parentToJoint frames.jointOrigins[i] = o frames.jointAxes[i] = R · links[i].jointAxis # axis is invariant under its own rotation - frames.jointTypes[i] = Revolute # (GenericJointLink: links[i].type) + frames.jointTypes[i] = Revolute # (JointLink: links[i].type) R = R · RotationAboutAxis(links[i].jointAxis, q[i]) # prismatic (M1): R unchanged, o += axis·q frames.tool = SE3Transform{ R, o } * toolInLastLink return frames diff --git a/robotics/dynamics/ArticulatedBodyAlgorithm.hpp b/robotics/dynamics/ArticulatedBodyAlgorithm.hpp index 8b834f6..65173c4 100644 --- a/robotics/dynamics/ArticulatedBodyAlgorithm.hpp +++ b/robotics/dynamics/ArticulatedBodyAlgorithm.hpp @@ -4,7 +4,7 @@ #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" -#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/dynamics/JointLink.hpp" #include "robotics/dynamics/SpatialInertia.hpp" #include #include @@ -22,7 +22,7 @@ namespace dynamics using Vector3 = math::Vector; using Matrix3 = math::SquareMatrix; using JointVector = math::Vector; - using LinkArray = std::array, NumLinks>; + using LinkArray = std::array, NumLinks>; ArticulatedBodyAlgorithm() = default; @@ -36,6 +36,7 @@ namespace dynamics Vector3 omega; Vector3 linVelJ; Matrix3 R; + Vector3 offset; Vector3 cJ; Vector3 cJLin; }; @@ -56,14 +57,14 @@ namespace dynamics static void PropagateKinematicsFromParent( LinkKinematics& current, const LinkKinematics& parent, - const RevoluteJointLink& link, T qDot_i); + const JointLink& link, T qDot_i); static BiasWrench ComputeBiasWrench( - const RevoluteJointLink& link, + const JointLink& link, const LinkKinematics& kin); static JointProjection ComputeJointProjection( - const Vector3& axis, const SpatialInertia& Ia, + const JointLink& link, const SpatialInertia& Ia, const BiasWrench& pA, T tau); static SpatialInertia ArticulatedBodyDowndate( @@ -100,18 +101,17 @@ namespace dynamics template void ArticulatedBodyAlgorithm::PropagateKinematicsFromParent( LinkKinematics& current, const LinkKinematics& parent, - const RevoluteJointLink& link, T qDot_i) + const JointLink& link, T qDot_i) { - auto Rt = current.R.Transpose(); - auto qDotAxis = link.jointAxis * qDot_i; + const auto Rt = current.R.Transpose(); + const Vector3 angularRate{ link.AngularAxis() * qDot_i }; + const Vector3 linearRate{ link.LinearAxis() * qDot_i }; - current.omega = Rt * parent.omega + qDotAxis; + current.omega = Rt * parent.omega + angularRate; + current.linVelJ = Rt * (parent.linVelJ + math::CrossProduct(parent.omega, current.offset)) + linearRate; - auto parentLinVelAtJoint = parent.linVelJ + math::CrossProduct(parent.omega, link.parentToJoint); - current.linVelJ = Rt * parentLinVelAtJoint; - - current.cJ = math::CrossProduct(current.omega, qDotAxis); - current.cJLin = math::CrossProduct(current.linVelJ, qDotAxis); + current.cJ = math::CrossProduct(current.omega, angularRate); + current.cJLin = math::CrossProduct(current.linVelJ, angularRate) + math::CrossProduct(current.omega, linearRate); } template @@ -120,16 +120,16 @@ namespace dynamics const LinkArray& links, const JointVector& q, const JointVector& qDot) { std::array kin{}; + const LinkKinematics base{}; for (std::size_t i = 0; i < NumLinks; ++i) { assert(HasUnitJointAxis(links[i])); - kin[i].R = math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); + const auto transform{ links[i].JointTransform(q.at(i, 0)) }; + kin[i].R = transform.rotation; + kin[i].offset = transform.translation; - if (i == 0) - kin[i].omega = links[i].jointAxis * qDot.at(i, 0); - else - PropagateKinematicsFromParent(kin[i], kin[i - 1], links[i], qDot.at(i, 0)); + PropagateKinematicsFromParent(kin[i], i == 0 ? base : kin[i - 1], links[i], qDot.at(i, 0)); } return kin; @@ -138,7 +138,7 @@ namespace dynamics template typename ArticulatedBodyAlgorithm::BiasWrench ArticulatedBodyAlgorithm::ComputeBiasWrench( - const RevoluteJointLink& link, + const JointLink& link, const LinkKinematics& kin) { auto rCoM = link.jointToCoM; @@ -156,17 +156,20 @@ namespace dynamics template typename ArticulatedBodyAlgorithm::JointProjection ArticulatedBodyAlgorithm::ComputeJointProjection( - const Vector3& axis, const SpatialInertia& Ia, + const JointLink& link, const SpatialInertia& Ia, const BiasWrench& pA, T tau) { + const Vector3 angular{ link.AngularAxis() }; + const Vector3 linear{ link.LinearAxis() }; + JointProjection proj{}; - proj.Ua = Ia.rot * axis; - proj.UaLin = Ia.cross.Transpose() * axis; + proj.Ua = Ia.rot * angular + Ia.cross * linear; + proj.UaLin = Ia.cross.Transpose() * angular + Ia.lin * linear; - proj.D = math::DotProduct(axis, proj.Ua); + proj.D = math::DotProduct(angular, proj.Ua) + math::DotProduct(linear, proj.UaLin) + link.armature; really_assert(proj.D > T(0)); - proj.u = tau - math::DotProduct(axis, pA.torque); + proj.u = tau - math::DotProduct(angular, pA.torque) - math::DotProduct(linear, pA.force); return proj; } @@ -220,8 +223,8 @@ namespace dynamics auto IaA = ArticulatedBodyDowndate(Ia[childIndex], proj[childIndex]); auto pAA = ComputeArticulatedBiasWrench(pA[childIndex], IaA, kin[childIndex], proj[childIndex]); - auto IaParent = TransformInertiaToParent(IaA, kin[childIndex].R, links[childIndex].parentToJoint); - auto pAParent = TransformWrenchToParent(pAA, kin[childIndex].R, links[childIndex].parentToJoint); + auto IaParent = TransformInertiaToParent(IaA, kin[childIndex].R, kin[childIndex].offset); + auto pAParent = TransformWrenchToParent(pAA, kin[childIndex].R, kin[childIndex].offset); Ia[parentIndex] += IaParent; pA[parentIndex].force = pA[parentIndex].force + pAParent.force; @@ -245,7 +248,7 @@ namespace dynamics for (std::size_t j = NumLinks; j > 0; --j) { std::size_t i = j - 1; - proj[i] = ComputeJointProjection(links[i].jointAxis, Ia[i], pA[i], tau.at(i, 0)); + proj[i] = ComputeJointProjection(links[i], Ia[i], pA[i], tau.at(i, 0)); if (i > 0) AccumulateChildContribution(links, i, Ia, pA, kin, proj); @@ -265,29 +268,17 @@ namespace dynamics for (std::size_t i = 0; i < NumLinks; ++i) { - auto Rt = kin[i].R.Transpose(); - Vector3 aPrimeAng; - Vector3 aPrimeLin; - - if (i == 0) - { - aPrimeAng = Vector3{}; - aPrimeLin = Rt * (gravity * T(-1)); - } - else - { - auto rToJoint = links[i].parentToJoint; - aPrimeAng = Rt * angAcc[i - 1]; - aPrimeLin = Rt * (linAcc[i - 1] + math::CrossProduct(angAcc[i - 1], rToJoint)); - } - - auto aHatAng = aPrimeAng + kin[i].cJ; - auto aHatLin = aPrimeLin + kin[i].cJLin; + const auto Rt = kin[i].R.Transpose(); + const Vector3 parentAng{ i == 0 ? Vector3{} : angAcc[i - 1] }; + const Vector3 parentLin{ i == 0 ? Vector3{ gravity * T(-1) } : linAcc[i - 1] }; + + const Vector3 aHatAng{ Rt * parentAng + kin[i].cJ }; + const Vector3 aHatLin{ Rt * (parentLin + math::CrossProduct(parentAng, kin[i].offset)) + kin[i].cJLin }; qDDot.at(i, 0) = ComputeJointAcceleration(proj[i], aHatAng, aHatLin); - angAcc[i] = aHatAng + links[i].jointAxis * qDDot.at(i, 0); - linAcc[i] = aHatLin; + angAcc[i] = aHatAng + links[i].AngularAxis() * qDDot.at(i, 0); + linAcc[i] = aHatLin + links[i].LinearAxis() * qDDot.at(i, 0); } return qDDot; diff --git a/robotics/dynamics/CMakeLists.txt b/robotics/dynamics/CMakeLists.txt index 15ae812..9546bdf 100644 --- a/robotics/dynamics/CMakeLists.txt +++ b/robotics/dynamics/CMakeLists.txt @@ -17,6 +17,7 @@ target_sources(robotics.dynamics PRIVATE EulerLagrangeDynamics.hpp EulerLagrangeSolver.hpp ForwardDynamicsIntegrator.hpp + JointLink.hpp NewtonEulerBody.hpp NewtonEulerSolver.hpp RecursiveNewtonEuler.hpp @@ -29,6 +30,7 @@ robotics_add_coverage_sources(robotics.dynamics CompositeRigidBodyAlgorithm.cpp EulerLagrangeSolver.cpp ForwardDynamicsIntegrator.cpp + JointLink.cpp NewtonEulerSolver.cpp RecursiveNewtonEuler.cpp ) diff --git a/robotics/dynamics/CompositeRigidBodyAlgorithm.hpp b/robotics/dynamics/CompositeRigidBodyAlgorithm.hpp index 9b1dd58..2f12281 100644 --- a/robotics/dynamics/CompositeRigidBodyAlgorithm.hpp +++ b/robotics/dynamics/CompositeRigidBodyAlgorithm.hpp @@ -3,7 +3,7 @@ #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" -#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/dynamics/JointLink.hpp" #include "robotics/dynamics/SpatialInertia.hpp" #include #include @@ -22,66 +22,74 @@ namespace dynamics using Matrix3 = math::SquareMatrix; using JointVector = math::Vector; using MassMatrix = math::SquareMatrix; - using LinkArray = std::array, NumLinks>; + using LinkArray = std::array, NumLinks>; OPTIMIZE_FOR_SPEED MassMatrix Compute(const LinkArray& links, const JointVector& q) const; private: - using Rotations = std::array; + using Transforms = std::array, NumLinks>; using CompositeInertias = std::array, NumLinks>; - static CompositeInertias ComputeCompositeInertias(const LinkArray& links, const Rotations& R); - static void FillColumn(MassMatrix& M, std::size_t column, const LinkArray& links, const Rotations& R, const SpatialInertia& composite); + static CompositeInertias ComputeCompositeInertias(const LinkArray& links, const Transforms& transforms); + static void FillColumn(MassMatrix& M, std::size_t column, const LinkArray& links, const Transforms& transforms, const SpatialInertia& composite); + static T Project(const JointLink& link, const SpatialWrench& wrench); }; template OPTIMIZE_FOR_SPEED typename CompositeRigidBodyAlgorithm::MassMatrix CompositeRigidBodyAlgorithm::Compute(const LinkArray& links, const JointVector& q) const { - Rotations R{}; + Transforms transforms{}; for (std::size_t i = 0; i < NumLinks; ++i) { assert(HasUnitJointAxis(links[i])); - R[i] = math::RotationAboutAxis(links[i].jointAxis, q[i]); + transforms[i] = links[i].JointTransform(q[i]); } - const auto composite{ ComputeCompositeInertias(links, R) }; + const auto composite{ ComputeCompositeInertias(links, transforms) }; MassMatrix M{}; for (std::size_t i = 0; i < NumLinks; ++i) - FillColumn(M, i, links, R, composite[i]); + FillColumn(M, i, links, transforms, composite[i]); return M; } template - typename CompositeRigidBodyAlgorithm::CompositeInertias CompositeRigidBodyAlgorithm::ComputeCompositeInertias(const LinkArray& links, const Rotations& R) + typename CompositeRigidBodyAlgorithm::CompositeInertias CompositeRigidBodyAlgorithm::ComputeCompositeInertias(const LinkArray& links, const Transforms& transforms) { CompositeInertias composite{}; for (std::size_t i = 0; i < NumLinks; ++i) composite[i] = RigidBodyInertia(links[i]); for (std::size_t i = NumLinks - 1; i > 0; --i) - composite[i - 1] += TransformInertiaToParent(composite[i], R[i], links[i].parentToJoint); + composite[i - 1] += TransformInertiaToParent(composite[i], transforms[i].rotation, transforms[i].translation); return composite; } template - void CompositeRigidBodyAlgorithm::FillColumn(MassMatrix& M, std::size_t column, const LinkArray& links, const Rotations& R, const SpatialInertia& composite) + void CompositeRigidBodyAlgorithm::FillColumn(MassMatrix& M, std::size_t column, const LinkArray& links, const Transforms& transforms, const SpatialInertia& composite) { - const auto& axis{ links[column].jointAxis }; - SpatialWrench wrench{ composite.cross.Transpose() * axis, composite.rot * axis }; - M.at(column, column) = math::DotProduct(axis, wrench.torque); + const auto angular{ links[column].AngularAxis() }; + const auto linear{ links[column].LinearAxis() }; + SpatialWrench wrench{ composite.cross.Transpose() * angular + composite.lin * linear, composite.rot * angular + composite.cross * linear }; + M.at(column, column) = Project(links[column], wrench) + links[column].armature; for (std::size_t j = column; j > 0; --j) { - wrench = TransformWrenchToParent(wrench, R[j], links[j].parentToJoint); - const T entry{ math::DotProduct(links[j - 1].jointAxis, wrench.torque) }; + wrench = TransformWrenchToParent(wrench, transforms[j].rotation, transforms[j].translation); + const T entry{ Project(links[j - 1], wrench) }; M.at(column, j - 1) = entry; M.at(j - 1, column) = entry; } } + template + T CompositeRigidBodyAlgorithm::Project(const JointLink& link, const SpatialWrench& wrench) + { + return math::DotProduct(link.AngularAxis(), wrench.torque) + math::DotProduct(link.LinearAxis(), wrench.force); + } + #ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD extern template class CompositeRigidBodyAlgorithm; extern template class CompositeRigidBodyAlgorithm; diff --git a/robotics/dynamics/ForwardDynamicsIntegrator.hpp b/robotics/dynamics/ForwardDynamicsIntegrator.hpp index bd3deab..06edd1c 100644 --- a/robotics/dynamics/ForwardDynamicsIntegrator.hpp +++ b/robotics/dynamics/ForwardDynamicsIntegrator.hpp @@ -7,7 +7,7 @@ #include "numerical/solvers/SecondOrderSystem.hpp" #include "numerical/solvers/SymplecticIntegrators.hpp" #include "robotics/dynamics/ArticulatedBodyAlgorithm.hpp" -#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/dynamics/JointLink.hpp" #include #include #include @@ -42,7 +42,7 @@ namespace dynamics using Vector3 = math::Vector; using JointVector = math::Vector; using StateVector = math::Vector; - using LinkArray = std::array, NumLinks>; + using LinkArray = std::array, NumLinks>; ChainAccelerationSystem(const LinkArray& links, const Vector3& gravity); @@ -63,7 +63,7 @@ namespace dynamics public: using Vector3 = math::Vector; using JointVector = math::Vector; - using LinkArray = std::array, NumLinks>; + using LinkArray = std::array, NumLinks>; using State = ChainState; ForwardDynamicsIntegrator(const LinkArray& links, const Vector3& gravity, T dt); diff --git a/robotics/dynamics/JointLink.cpp b/robotics/dynamics/JointLink.cpp new file mode 100644 index 0000000..f86cc31 --- /dev/null +++ b/robotics/dynamics/JointLink.cpp @@ -0,0 +1,6 @@ +#include "robotics/dynamics/JointLink.hpp" + +namespace dynamics +{ + template struct JointLink; +} diff --git a/robotics/dynamics/JointLink.hpp b/robotics/dynamics/JointLink.hpp new file mode 100644 index 0000000..89a317c --- /dev/null +++ b/robotics/dynamics/JointLink.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include +#include +#include + +namespace dynamics +{ + enum class JointType : std::uint8_t + { + Revolute, + Prismatic + }; + + template + struct JointLimits + { + static_assert(std::is_floating_point_v, "JointLimits supports floating-point types"); + + T positionMin{ -std::numeric_limits::infinity() }; + T positionMax{ std::numeric_limits::infinity() }; + T velocityMax{ std::numeric_limits::infinity() }; + T effortMax{ std::numeric_limits::infinity() }; + }; + + template + struct JointLink + { + static_assert(std::is_floating_point_v, "JointLink supports floating-point types"); + + T mass; + math::SquareMatrix inertia; + math::Vector jointAxis; + math::Vector parentToJoint; + math::Vector jointToCoM; + JointType type{ JointType::Revolute }; + JointLimits limits{}; + T armature{}; + + OPTIMIZE_FOR_SPEED math::SE3Transform JointTransform(T q) const; + math::Vector AngularAxis() const; + math::Vector LinearAxis() const; + }; + + template + using RevoluteJointLink = JointLink; + + template + OPTIMIZE_FOR_SPEED math::SE3Transform JointLink::JointTransform(T q) const + { + if (type == JointType::Revolute) + return { math::RotationAboutAxis(jointAxis, q), parentToJoint }; + + return { math::SquareMatrix::Identity(), parentToJoint + jointAxis * q }; + } + + template + math::Vector JointLink::AngularAxis() const + { + return type == JointType::Revolute ? jointAxis : math::Vector{}; + } + + template + math::Vector JointLink::LinearAxis() const + { + return type == JointType::Prismatic ? jointAxis : math::Vector{}; + } + + template + bool HasUnitJointAxis(const JointLink& link) + { + return math::Abs(math::DotProduct(link.jointAxis, link.jointAxis) - T(1)) < T(1e-3); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template struct JointLink; +#endif +} diff --git a/robotics/dynamics/RecursiveNewtonEuler.hpp b/robotics/dynamics/RecursiveNewtonEuler.hpp index f5639f4..0263cf7 100644 --- a/robotics/dynamics/RecursiveNewtonEuler.hpp +++ b/robotics/dynamics/RecursiveNewtonEuler.hpp @@ -3,7 +3,7 @@ #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" -#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/dynamics/JointLink.hpp" #include #include @@ -20,7 +20,7 @@ namespace dynamics using Vector3 = math::Vector; using Matrix3 = math::SquareMatrix; using JointVector = math::Vector; - using LinkArray = std::array, NumLinks>; + using LinkArray = std::array, NumLinks>; RecursiveNewtonEuler() = default; @@ -33,18 +33,14 @@ namespace dynamics { Vector3 omega; Vector3 omegaDot; - Vector3 linAccCoM; + Vector3 linAccJoint; Matrix3 R; + Vector3 offset; }; - static void ComputeBaseLinkState( - LinkState& state, const RevoluteJointLink& link, - T qDot_i, T qDDot_i, const Vector3& gravity); - static void PropagateStateFromParent( LinkState& state, const LinkState& parent, - const RevoluteJointLink& link, const RevoluteJointLink& parentLink, - T qDot_i, T qDDot_i); + const JointLink& link, T qDot_i, T qDDot_i); static std::array ComputeForwardPass( const LinkArray& links, const JointVector& q, @@ -53,7 +49,7 @@ namespace dynamics static void ComputeLinkWrench( Vector3& force, Vector3& torque, - const RevoluteJointLink& link, const LinkState& state); + const JointLink& link, const LinkState& state); static void AccumulateChildWrench( Vector3& parentForce, Vector3& parentTorque, @@ -61,47 +57,24 @@ namespace dynamics const Matrix3& childR, const Vector3& rToChild); static JointVector ComputeBackwardPass( - const LinkArray& links, const std::array& states); + const LinkArray& links, const std::array& states, + const JointVector& qDDot); }; - template - void RecursiveNewtonEuler::ComputeBaseLinkState( - LinkState& state, const RevoluteJointLink& link, - T qDot_i, T qDDot_i, const Vector3& gravity) - { - auto Rt = state.R.Transpose(); - - state.omega = link.jointAxis * qDot_i; - - auto qDotAxis = link.jointAxis * qDot_i; - state.omegaDot = link.jointAxis * qDDot_i + math::CrossProduct(state.omega, qDotAxis); - - auto baseLinAcc = Rt * (gravity * T(-1)); - auto rCoM = link.jointToCoM; - state.linAccCoM = baseLinAcc + math::CrossProduct(state.omegaDot, rCoM) + math::CrossProduct(state.omega, math::CrossProduct(state.omega, rCoM)); - } - template void RecursiveNewtonEuler::PropagateStateFromParent( LinkState& state, const LinkState& parent, - const RevoluteJointLink& link, const RevoluteJointLink& parentLink, - T qDot_i, T qDDot_i) + const JointLink& link, T qDot_i, T qDDot_i) { - auto Rt = state.R.Transpose(); - auto qDotAxis = link.jointAxis * qDot_i; - - state.omega = Rt * parent.omega + qDotAxis; - state.omegaDot = Rt * parent.omegaDot + math::CrossProduct(state.omega, qDotAxis) + link.jointAxis * qDDot_i; - - auto rParentCoM = parentLink.jointToCoM; - auto parentJointAcc = parent.linAccCoM - math::CrossProduct(parent.omegaDot, rParentCoM) - math::CrossProduct(parent.omega, math::CrossProduct(parent.omega, rParentCoM)); + const auto Rt = state.R.Transpose(); + const Vector3 angularRate{ link.AngularAxis() * qDot_i }; + const Vector3 linearRate{ link.LinearAxis() * qDot_i }; - auto rToJoint = link.parentToJoint; - auto accAtParentEnd = parentJointAcc + math::CrossProduct(parent.omegaDot, rToJoint) + math::CrossProduct(parent.omega, math::CrossProduct(parent.omega, rToJoint)); + state.omega = Rt * parent.omega + angularRate; + state.omegaDot = Rt * parent.omegaDot + math::CrossProduct(state.omega, angularRate) + link.AngularAxis() * qDDot_i; - auto accJointInLink = Rt * accAtParentEnd; - auto rCoM = link.jointToCoM; - state.linAccCoM = accJointInLink + math::CrossProduct(state.omegaDot, rCoM) + math::CrossProduct(state.omega, math::CrossProduct(state.omega, rCoM)); + const Vector3 accAtJoint{ parent.linAccJoint + math::CrossProduct(parent.omegaDot, state.offset) + math::CrossProduct(parent.omega, math::CrossProduct(parent.omega, state.offset)) }; + state.linAccJoint = Rt * accAtJoint + math::CrossProduct(state.omega, linearRate) * T(2) + link.LinearAxis() * qDDot_i; } template @@ -112,16 +85,16 @@ namespace dynamics const Vector3& gravity) { std::array states{}; + const LinkState base{ Vector3{}, Vector3{}, gravity * T(-1), Matrix3::Identity(), Vector3{} }; for (std::size_t i = 0; i < NumLinks; ++i) { assert(HasUnitJointAxis(links[i])); - states[i].R = math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); + const auto transform{ links[i].JointTransform(q.at(i, 0)) }; + states[i].R = transform.rotation; + states[i].offset = transform.translation; - if (i == 0) - ComputeBaseLinkState(states[i], links[i], qDot.at(i, 0), qDDot.at(i, 0), gravity); - else - PropagateStateFromParent(states[i], states[i - 1], links[i], links[i - 1], qDot.at(i, 0), qDDot.at(i, 0)); + PropagateStateFromParent(states[i], i == 0 ? base : states[i - 1], links[i], qDot.at(i, 0), qDDot.at(i, 0)); } return states; @@ -130,14 +103,16 @@ namespace dynamics template void RecursiveNewtonEuler::ComputeLinkWrench( Vector3& force, Vector3& torque, - const RevoluteJointLink& link, const LinkState& state) + const JointLink& link, const LinkState& state) { - force = state.linAccCoM * link.mass; + const Vector3 rCoM{ link.jointToCoM }; + const Vector3 linAccCoM{ state.linAccJoint + math::CrossProduct(state.omegaDot, rCoM) + math::CrossProduct(state.omega, math::CrossProduct(state.omega, rCoM)) }; + force = linAccCoM * link.mass; auto Iw = link.inertia * state.omega; torque = link.inertia * state.omegaDot + math::CrossProduct(state.omega, Iw); - torque = torque + math::CrossProduct(link.jointToCoM, force); + torque = torque + math::CrossProduct(rCoM, force); } template @@ -156,7 +131,8 @@ namespace dynamics template typename RecursiveNewtonEuler::JointVector RecursiveNewtonEuler::ComputeBackwardPass( - const LinkArray& links, const std::array& states) + const LinkArray& links, const std::array& states, + const JointVector& qDDot) { std::array force; std::array torque; @@ -169,12 +145,12 @@ namespace dynamics if (i + 1 < NumLinks) AccumulateChildWrench(force[i], torque[i], force[i + 1], torque[i + 1], - states[i + 1].R, links[i + 1].parentToJoint); + states[i + 1].R, states[i + 1].offset); } JointVector tau{}; for (std::size_t i = 0; i < NumLinks; ++i) - tau.at(i, 0) = math::DotProduct(links[i].jointAxis, torque[i]); + tau.at(i, 0) = math::DotProduct(links[i].AngularAxis(), torque[i]) + math::DotProduct(links[i].LinearAxis(), force[i]) + links[i].armature * qDDot.at(i, 0); return tau; } @@ -187,7 +163,7 @@ namespace dynamics const Vector3& gravity) const { auto states = ComputeForwardPass(links, q, qDot, qDDot, gravity); - return ComputeBackwardPass(links, states); + return ComputeBackwardPass(links, states, qDDot); } #ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD diff --git a/robotics/dynamics/RevoluteJointLink.hpp b/robotics/dynamics/RevoluteJointLink.hpp index 3719d53..921192a 100644 --- a/robotics/dynamics/RevoluteJointLink.hpp +++ b/robotics/dynamics/RevoluteJointLink.hpp @@ -1,28 +1,3 @@ #pragma once -#include "numerical/math/Geometry3D.hpp" -#include "numerical/math/Matrix.hpp" -#include -#include - -namespace dynamics -{ - template - struct RevoluteJointLink - { - static_assert(std::is_floating_point_v, - "RevoluteJointLink only supports floating-point types"); - - T mass; - math::SquareMatrix inertia; - math::Vector jointAxis; - math::Vector parentToJoint; - math::Vector jointToCoM; - }; - - template - bool HasUnitJointAxis(const RevoluteJointLink& link) - { - return std::abs(math::DotProduct(link.jointAxis, link.jointAxis) - T(1)) < T(1e-3); - } -} +#include "robotics/dynamics/JointLink.hpp" diff --git a/robotics/dynamics/SpatialInertia.hpp b/robotics/dynamics/SpatialInertia.hpp index bb9e9fb..77d64b1 100644 --- a/robotics/dynamics/SpatialInertia.hpp +++ b/robotics/dynamics/SpatialInertia.hpp @@ -2,7 +2,7 @@ #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" -#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/dynamics/JointLink.hpp" #include namespace dynamics @@ -35,7 +35,7 @@ namespace dynamics }; template - SpatialInertia RigidBodyInertia(const RevoluteJointLink& link) + SpatialInertia RigidBodyInertia(const JointLink& link) { const auto rSkew{ math::SkewSymmetric(link.jointToCoM) }; diff --git a/robotics/dynamics/test/CMakeLists.txt b/robotics/dynamics/test/CMakeLists.txt index 6f11a03..112d5b4 100644 --- a/robotics/dynamics/test/CMakeLists.txt +++ b/robotics/dynamics/test/CMakeLists.txt @@ -12,6 +12,7 @@ target_sources(robotics.dynamics_test PRIVATE TestCompositeRigidBodyAlgorithm.cpp TestEulerLagrangeSolver.cpp TestForwardDynamicsIntegrator.cpp + TestJointLink.cpp TestNewtonEulerSolver.cpp TestRecursiveNewtonEuler.cpp ) diff --git a/robotics/dynamics/test/TestArticulatedBodyAlgorithm.cpp b/robotics/dynamics/test/TestArticulatedBodyAlgorithm.cpp index ab71bb9..566450f 100644 --- a/robotics/dynamics/test/TestArticulatedBodyAlgorithm.cpp +++ b/robotics/dynamics/test/TestArticulatedBodyAlgorithm.cpp @@ -134,3 +134,28 @@ TEST_F(TestArticulatedBodyAlgorithm, spatial_chain_inverts_recursive_newton_eule EXPECT_NEAR(qDDot.at(1, 0), expected.at(1, 0), math::Tolerance()); EXPECT_NEAR(qDDot.at(2, 0), expected.at(2, 0), math::Tolerance()); } + +TEST_F(TestArticulatedBodyAlgorithm, mixed_chain_round_trips_through_aba) +{ + const math::SquareMatrix inertia{ + { 0.02f, 0.001f, 0.0f }, + { 0.001f, 0.03f, 0.002f }, + { 0.0f, 0.002f, 0.025f } + }; + std::array links{ + Link{ 1.2f, inertia, zAxis, Vector3{ 0.0f, 0.0f, 0.1f }, Vector3{ 0.05f, 0.02f, 0.1f } }, + Link{ 0.9f, inertia * 0.6f, Normalized(1.0f, 0.2f, 0.3f), Vector3{ 0.1f, 0.0f, 0.2f }, Vector3{ 0.15f, -0.03f, 0.02f }, dynamics::JointType::Prismatic }, + Link{ 0.6f, inertia * 0.3f, Normalized(1.0f, 0.0f, 1.0f), Vector3{ 0.3f, 0.05f, 0.0f }, Vector3{ 0.1f, 0.04f, -0.05f } } + }; + dynamics::RecursiveNewtonEuler rnea; + math::Vector q{ 0.4f, 0.25f, 1.1f }; + math::Vector qDot{ 0.8f, -0.6f, 2.1f }; + math::Vector expected{ 0.3f, -1.2f, 0.7f }; + + auto tau = rnea.InverseDynamics(links, q, qDot, expected, gravityVector); + auto qDDot = aba3.ForwardDynamics(links, q, qDot, tau, gravityVector); + + EXPECT_NEAR(qDDot.at(0, 0), expected.at(0, 0), math::Tolerance()); + EXPECT_NEAR(qDDot.at(1, 0), expected.at(1, 0), math::Tolerance()); + EXPECT_NEAR(qDDot.at(2, 0), expected.at(2, 0), math::Tolerance()); +} diff --git a/robotics/dynamics/test/TestJointLink.cpp b/robotics/dynamics/test/TestJointLink.cpp new file mode 100644 index 0000000..f50f1b9 --- /dev/null +++ b/robotics/dynamics/test/TestJointLink.cpp @@ -0,0 +1,100 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/dynamics/ArticulatedBodyAlgorithm.hpp" +#include "robotics/dynamics/CompositeRigidBodyAlgorithm.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include "robotics/dynamics/RecursiveNewtonEuler.hpp" +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector; + using Matrix3 = math::SquareMatrix; + using Link = dynamics::JointLink; + + class TestJointLink + : public ::testing::Test + { + protected: + Link MakeRevolute(const Vector3& axis, const Vector3& parentToJoint) const + { + return Link{ 1.0f, Matrix3::Identity(), axis, parentToJoint, Vector3{} }; + } + + Link MakePrismatic(const Vector3& axis, const Vector3& parentToJoint) const + { + return Link{ 1.0f, Matrix3::Identity(), axis, parentToJoint, Vector3{}, dynamics::JointType::Prismatic }; + } + + 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()); + } + + Vector3 x{ 1.0f, 0.0f, 0.0f }; + Vector3 z{ 0.0f, 0.0f, 1.0f }; + Vector3 offset{ 0.1f, -0.2f, 0.3f }; + }; +} + +TEST_F(TestJointLink, existing_aggregate_initializers_default_to_revolute) +{ + const dynamics::RevoluteJointLink link{ 2.0f, Matrix3::Identity(), z, offset, x }; + + EXPECT_EQ(link.type, dynamics::JointType::Revolute); + EXPECT_EQ(link.armature, 0.0f); + EXPECT_TRUE(std::isinf(link.limits.positionMin) && link.limits.positionMin < 0.0f); + EXPECT_TRUE(std::isinf(link.limits.positionMax) && link.limits.positionMax > 0.0f); + EXPECT_TRUE(std::isinf(link.limits.velocityMax)); + EXPECT_TRUE(std::isinf(link.limits.effortMax)); +} + +TEST_F(TestJointLink, revolute_transform_rotates_about_axis) +{ + const auto transform{ MakeRevolute(z, offset).JointTransform(std::numbers::pi_v / 2.0f) }; + + ExpectNear(transform.rotation * x, Vector3{ 0.0f, 1.0f, 0.0f }); + ExpectNear(transform.translation, offset); +} + +TEST_F(TestJointLink, prismatic_transform_slides_along_axis) +{ + const auto transform{ MakePrismatic(x, offset).JointTransform(0.3f) }; + + for (std::size_t r = 0; r < 3; ++r) + for (std::size_t c = 0; c < 3; ++c) + EXPECT_NEAR(transform.rotation.at(r, c), r == c ? 1.0f : 0.0f, math::Tolerance()); + ExpectNear(transform.translation, Vector3{ 0.4f, -0.2f, 0.3f }); +} + +TEST_F(TestJointLink, motion_subspace_selects_one_channel) +{ + const Link revolute{ MakeRevolute(z, offset) }; + const Link prismatic{ MakePrismatic(z, offset) }; + + ExpectNear(revolute.AngularAxis(), z); + ExpectNear(revolute.LinearAxis(), Vector3{}); + ExpectNear(prismatic.AngularAxis(), Vector3{}); + ExpectNear(prismatic.LinearAxis(), z); +} + +TEST_F(TestJointLink, armature_adds_to_diagonal_inertia) +{ + const float armature{ 0.2f }; + const float rodInertia{ 1.0f / 3.0f }; + const float qDDot{ 1.5f }; + Link rod{ 1.0f, Matrix3{ { 0.0f, 0.0f, 0.0f }, { 0.0f, 1.0f / 12.0f, 0.0f }, { 0.0f, 0.0f, 1.0f / 12.0f } }, z, Vector3{}, Vector3{ 0.5f, 0.0f, 0.0f } }; + rod.armature = armature; + const std::array links{ rod }; + const math::Vector q{ 0.4f }; + + const auto tau{ dynamics::RecursiveNewtonEuler{}.InverseDynamics(links, q, math::Vector{}, math::Vector{ qDDot }, Vector3{}) }; + const auto acceleration{ dynamics::ArticulatedBodyAlgorithm{}.ForwardDynamics(links, q, math::Vector{}, tau, Vector3{}) }; + const auto M{ dynamics::CompositeRigidBodyAlgorithm{}.Compute(links, q) }; + + EXPECT_NEAR(tau[0], (rodInertia + armature) * qDDot, math::Tolerance()); + EXPECT_NEAR(acceleration[0], qDDot, math::Tolerance()); + EXPECT_NEAR(M.at(0, 0), rodInertia + armature, math::Tolerance()); +} diff --git a/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp b/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp index e33a09d..1d8ddbe 100644 --- a/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp +++ b/robotics/dynamics/test/TestRecursiveNewtonEuler.cpp @@ -161,3 +161,34 @@ TEST_F(TestRecursiveNewtonEuler, matches_euler_lagrange_model_under_gravity_and_ EXPECT_NEAR(tau.at(0, 0), reference.at(0, 0), math::Tolerance()); EXPECT_NEAR(tau.at(1, 0), reference.at(1, 0), math::Tolerance()); } + +TEST_F(TestRecursiveNewtonEuler, vertical_slider_needs_weight_plus_inertial_force) +{ + const float mass{ 2.0f }; + const float acceleration{ 1.0f }; + std::array links{ Link{ mass, math::SquareMatrix{}, zAxis, Vector3{}, Vector3{}, dynamics::JointType::Prismatic } }; + + auto tau = rnea1.InverseDynamics(links, math::Vector{ 0.3f }, math::Vector{ 0.5f }, math::Vector{ acceleration }, gravityVector); + + EXPECT_NEAR(tau.at(0, 0), mass * (gravity + acceleration), math::Tolerance()); +} + +TEST_F(TestRecursiveNewtonEuler, prismatic_coriolis_term_on_rotating_base) +{ + const float mass{ 2.0f }; + const float radius{ 0.5f }; + const float extensionRate{ 0.3f }; + const float spinRate{ 1.5f }; + const float spinAcceleration{ 0.4f }; + const float extensionAcceleration{ 0.2f }; + std::array links{ + Link{ 0.0f, math::SquareMatrix{}, zAxis, Vector3{}, Vector3{} }, + Link{ mass, math::SquareMatrix{}, Vector3{ 1.0f, 0.0f, 0.0f }, Vector3{}, Vector3{}, dynamics::JointType::Prismatic } + }; + + auto tau = rnea2.InverseDynamics(links, math::Vector{ 0.7f, radius }, math::Vector{ spinRate, extensionRate }, + math::Vector{ spinAcceleration, extensionAcceleration }, Vector3{}); + + 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()); +} diff --git a/robotics/kinematics/ForwardKinematics.hpp b/robotics/kinematics/ForwardKinematics.hpp index 743391f..cc5ddcb 100644 --- a/robotics/kinematics/ForwardKinematics.hpp +++ b/robotics/kinematics/ForwardKinematics.hpp @@ -3,7 +3,7 @@ #include "numerical/math/CompilerOptimizations.hpp" #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" -#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/dynamics/JointLink.hpp" #include #include @@ -20,7 +20,7 @@ namespace kinematics using Vector3 = math::Vector; using Matrix3 = math::SquareMatrix; using JointVector = math::Vector; - using LinkArray = std::array, NumLinks>; + using LinkArray = std::array, NumLinks>; using PositionArray = std::array; explicit ForwardKinematics(const Vector3& toolOffset); @@ -46,19 +46,19 @@ namespace kinematics const JointVector& q) const { PositionArray positions{}; - positions[0] = links[0].parentToJoint; - + Vector3 origin{}; auto R = Matrix3::Identity(); for (std::size_t i = 0; i < NumLinks; ++i) { assert(dynamics::HasUnitJointAxis(links[i])); - R = R * math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); - - const Vector3& offsetToNext = (i + 1 < NumLinks) ? links[i + 1].parentToJoint : toolOffset; - positions[i + 1] = positions[i] + R * offsetToNext; + const auto transform{ links[i].JointTransform(q.at(i, 0)) }; + origin = origin + R * transform.translation; + R = R * transform.rotation; + positions[i] = origin; } + positions[NumLinks] = origin + R * toolOffset; return positions; } diff --git a/robotics/kinematics/InverseKinematics.hpp b/robotics/kinematics/InverseKinematics.hpp index a2a0719..ef689f8 100644 --- a/robotics/kinematics/InverseKinematics.hpp +++ b/robotics/kinematics/InverseKinematics.hpp @@ -5,7 +5,7 @@ #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" #include "numerical/solvers/GaussianElimination.hpp" -#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/dynamics/JointLink.hpp" #include "robotics/kinematics/ForwardKinematics.hpp" #include #include @@ -44,7 +44,7 @@ namespace kinematics using Matrix3 = math::Matrix3; using JointVector = math::Vector; using Jacobian = math::Matrix; - using LinkArray = std::array, NumLinks>; + using LinkArray = std::array, NumLinks>; using PositionArray = std::array; explicit InverseKinematics(const Vector3& toolOffset, InverseKinematicsConfig cfg = {}); @@ -117,14 +117,13 @@ namespace kinematics for (std::size_t i = 0; i < NumLinks; ++i) { - const Vector3 worldAxis{ R * links[i].jointAxis }; - const Vector3 column{ math::CrossProduct(worldAxis, Vector3{ toolPosition - positions[i] }) }; + const Vector3 column{ math::CrossProduct(R * links[i].AngularAxis(), Vector3{ toolPosition - positions[i] }) + R * links[i].LinearAxis() }; J.at(0, i) = column.at(0, 0); J.at(1, i) = column.at(1, 0); J.at(2, i) = column.at(2, 0); - R = R * math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); + R = R * links[i].JointTransform(q.at(i, 0)).rotation; } return J; From d4106f4c45c9bec3254eca68b80acb19bcef8002 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 11:14:17 +0000 Subject: [PATCH 2/4] feat(kinematics): add ChainPoseKinematics and FrameChain (M30) Full tool pose on SE(3) plus every joint origin, axis and type in the base frame, in one O(n) pass over the generic link model. The frame chain is the model-independent input for the geometric Jacobian. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 4 - doc/kinematics/ChainPoseKinematics.md | 91 +++++++++++++++++++ doc/kinematics/README.md | 9 +- roadmap/README.md | 2 +- .../ChainPoseKinematics/explanation.md | 29 ------ .../ChainPoseKinematics/implementation.md | 75 --------------- .../kinematics/ChainPoseKinematics/tests.md | 40 -------- robotics/kinematics/CMakeLists.txt | 2 + robotics/kinematics/ChainPoseKinematics.cpp | 8 ++ robotics/kinematics/ChainPoseKinematics.hpp | 81 +++++++++++++++++ robotics/kinematics/test/CMakeLists.txt | 1 + .../test/TestChainPoseKinematics.cpp | 90 ++++++++++++++++++ 13 files changed, 280 insertions(+), 154 deletions(-) create mode 100644 doc/kinematics/ChainPoseKinematics.md delete mode 100644 roadmap/kinematics/ChainPoseKinematics/explanation.md delete mode 100644 roadmap/kinematics/ChainPoseKinematics/implementation.md delete mode 100644 roadmap/kinematics/ChainPoseKinematics/tests.md create mode 100644 robotics/kinematics/ChainPoseKinematics.cpp create mode 100644 robotics/kinematics/ChainPoseKinematics.hpp create mode 100644 robotics/kinematics/test/TestChainPoseKinematics.cpp diff --git a/README.md b/README.md index f1fbf7b..dee5075 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. | Category | Description | |--------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Inverse Kinematics (Damped Least Squares) | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), 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 | diff --git a/ROADMAP.md b/ROADMAP.md index 3dafae9..866da48 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -119,7 +119,6 @@ existing `dynamics/` convention. |-----|-----------------------------------------------------------------|---------------------------|----------------|------------| | M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | | M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | -| M30 | Chain pose kinematics (full-pose FK, frame chain) | `kinematics` | NT53 | ★★☆☆☆ | | M7 | Denavit-Hartenberg parameters (standard + modified) | `kinematics` | NT53, M30 | ★★☆☆☆ | | M8 | Geometric Jacobian (6×N), bias term, Jacobian provider | `kinematics` | NT53, M30 | ★★☆☆☆ | | M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | NT53, NT55 | ★★☆☆☆ | @@ -156,9 +155,6 @@ retired here. **M29. Chain dynamics model.** Adapter implementing `EulerLagrangeDynamics` (M via CRBA, `Cq̇` and `g` via RNEA) and a one-call `InverseDynamicsModel` for the controllers. - *Reuses / builds on:* RNEA, M28; optional friction model from NT57. -**M30. Chain pose kinematics.** Full tool pose and a model-independent frame chain (joint origins/axes in the base frame) for the link model. -- *Algorithm / paper:* Siciliano et al. (2009), Ch. 2. - **M7. Denavit-Hartenberg parameters.** Standard and modified `(a, α, d, θ)` descriptions with joint offsets, producing the M30 frame chain. - *Algorithm / paper:* Craig, *Introduction to Robotics*, Ch. 3. diff --git a/doc/kinematics/ChainPoseKinematics.md b/doc/kinematics/ChainPoseKinematics.md new file mode 100644 index 0000000..f981a48 --- /dev/null +++ b/doc/kinematics/ChainPoseKinematics.md @@ -0,0 +1,91 @@ +# Chain Pose Kinematics + +## Overview & Motivation + +Position-only forward kinematics answers "where is the tool point?". That is enough for reaching a point, but grasping, welding, screwing or keeping a camera level also needs the tool **orientation**. +Every Jacobian-based algorithm also needs each joint's **axis** and **origin** in one common frame: + +- inverse kinematics; +- impedance and operational-space control; +- manipulability analysis. + +Chain pose kinematics computes all of this in the same single pass over the chain. It returns the full tool pose, a rigid transform in SE(3), together with a **frame chain**: the origin, axis and type of every joint in the base frame. +The frame chain is model-independent, so a Jacobian routine consumes it without knowing whether the arm was described link by link, by Denavit–Hartenberg parameters or by screw axes. + +## Mathematical Theory + +### Rigid Transforms + +A pose is a pair $(R, p)$ with $R \in SO(3)$ and $p \in \mathbb{R}^3$, composed as + +$$(R_a, p_a)\,(R_b, p_b) = (R_a R_b,\; R_a p_b + p_a)$$ + +Each joint contributes a transform from its link frame to the parent frame, $X_i(q_i) = (R_i(q_i), r_i(q_i))$. A revolute joint rotates about its axis $\hat{z}_i$ at the fixed offset $r_i$. A prismatic joint keeps $R_i = \mathbf{I}_3$ and moves its offset to $r_i + \hat{z}_i q_i$. + +### Frame Chain Recursion + +Starting from the base frame $(R, o) = (\mathbf{I}_3, 0)$, for $i = 0, \dots, n-1$: + +$$a_i = R\,\hat{z}_i, \qquad (R, o) \leftarrow (R, o)\,X_i(q_i), \qquad o_i = o$$ + +The axis is taken **before** folding in the joint's own rotation. This makes no difference for a revolute joint, since a rotation leaves its own axis unchanged. For a prismatic joint it is the slide direction in the parent's orientation. The origin $o_i$ is the joint position after its own offset. + +After the last joint, the constant tool frame $X_t$, the tool pose in the last link's frame, is appended: + +$$T_{\text{tool}} = X_0(q_0)\,X_1(q_1)\cdots X_{n-1}(q_{n-1})\,X_t$$ + +### Consistency with Position-Only Kinematics + +The origins $o_i$ are exactly the joint positions of position-only forward kinematics. With $X_t = (\mathbf{I}_3, t)$, the tool translation equals its tool point. The tool frame's rotation is applied last, so it changes the reported orientation but never the position of a tool offset along the last link. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|------|--------|--------|-----------------------------------------------------------------------------------| +| All | $O(n)$ | $O(n)$ | One axis–angle rotation, two $3 \times 3$ products per joint, $2n$ vectors stored | + +The single pass costs the same order as position-only kinematics; the extra work is one matrix–vector product per joint for the axis. + +## Step-by-Step Walkthrough + +An arm with a $z$-axis base joint raised $0.2$ m, then two $y$-axis joints offset $0.5$ m up and $0.4$ m along $x$, and a tool $0.3$ m along the last link's $x$. Take $q = (\pi/2, \pi/2, 0)$: + +1. Joint 0: axis $a_0 = \hat{z}$, origin $o_0 = (0, 0, 0.2)$. Fold in $R_z(\pi/2)$. +2. Joint 1: axis $a_1 = R_z(\pi/2)\,\hat{y} = (-1, 0, 0)$, origin $o_1 = o_0 + R_z(\pi/2)(0, 0, 0.5) = (0, 0, 0.7)$. Fold in $R_y(\pi/2)$. +3. Joint 2: running rotation $R_z(\pi/2) R_y(\pi/2)$ maps $\hat{x}$ to $(0, 0, -1)$, so $o_2 = (0, 0, 0.7) + (0, 0, -0.4) = (0, 0, 0.3)$. +4. Tool: $p_{\text{tool}} = o_2 + (0, 0, -0.3) = (0, 0, 0)$, with orientation $R_z(\pi/2) R_y(\pi/2)$: the arm folds straight down to the base plane. + +## Pitfalls & Edge Cases + +- **Axis frame**: joint axes are written in each link's own frame. Taking the axis after the joint's own rotation is harmless for revolute joints but wrong for prismatic joints mounted on rotated frames. +- **Tool frame order**: the tool transform is multiplied on the right. Pre-multiplying it would rotate the whole arm instead of the tool. +- **Base offset**: the first joint's offset is the mounting offset of the arm. It moves every origin and the tool, and must not be folded into the tool frame. +- **Drift of composed rotations**: repeated products of rotation matrices drift slowly from orthonormality in single precision. Each evaluation starts from the identity here, so the drift never accumulates across calls. +- **Zero-length links**: coincident origins are valid; they only make some Jacobian columns linearly dependent. + +## Variants & Generalizations + +- **Denavit–Hartenberg**: four parameters per joint produce the same frame chain with fewer numbers, at the price of convention-dependent frames. +- **Product of exponentials**: screw axes in the base frame give $T = e^{[S_0]q_0} \cdots e^{[S_{n-1}]q_{n-1}} M$, with no per-link frames at all. +- **Per-link poses**: storing every intermediate $(R, o)$ instead of only axes and origins supports collision checking against link geometry. +- **Branched chains**: trees repeat the recursion per branch, sharing prefixes. + +## Applications + +- Full-pose inverse kinematics and Cartesian trajectory tracking. +- The geometric Jacobian, manipulability measures and task-space control, all built from the frame chain. +- Tool-frame calibration and visualization of joint frames. +- Self-collision models whose capsules run between consecutive joint origins. + +## Connections to Other Algorithms + +- [Forward Kinematics](ForwardKinematics.md): the position-only special case; its joint positions equal the frame chain's origins. +- [Inverse Kinematics](InverseKinematics.md): iterates on forward kinematics; a pose-level variant needs the orientation computed here. +- [Generic Joint Link](../dynamics/JointLink.md): supplies the per-joint transform for revolute and prismatic joints. +- The SE(3) rigid transform comes from the shared primitives of [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Siciliano, B., Sciavicco, L., Villani, L. and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 2. +- Lynch, K.M. and Park, F.C. (2017). *Modern Robotics*. Cambridge University Press. Chapter 4. +- Craig, J.J. (2005). *Introduction to Robotics: Mechanics and Control*. 3rd ed. Pearson. Chapter 3. diff --git a/doc/kinematics/README.md b/doc/kinematics/README.md index be323f0..668b82f 100644 --- a/doc/kinematics/README.md +++ b/doc/kinematics/README.md @@ -4,7 +4,8 @@ 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 | -| [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)$ | +| [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/roadmap/README.md b/roadmap/README.md index 1410080..14d9e1e 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -89,7 +89,7 @@ prerequisites. ## Index ### `kinematics` -`DenavitHartenberg` (M7) · `GeometricJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) · `ChainPoseKinematics` (M30) +`DenavitHartenberg` (M7) · `GeometricJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) ### `dynamics` `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) diff --git a/roadmap/kinematics/ChainPoseKinematics/explanation.md b/roadmap/kinematics/ChainPoseKinematics/explanation.md deleted file mode 100644 index 3386321..0000000 --- a/roadmap/kinematics/ChainPoseKinematics/explanation.md +++ /dev/null @@ -1,29 +0,0 @@ -# Chain Pose Kinematics — Overview - -## What it is -Forward kinematics that returns the full tool pose (position and orientation) of a serial arm -described link by link, together with every joint's axis and origin in the base frame. - -## Why it matters (embedded) -Position-only kinematics is enough for reaching a point, but grasping, welding or keeping a camera -level needs orientation too, and every Jacobian-based algorithm needs the joint axes in one common -frame. Producing them in the same single pass that computes the positions keeps the cost linear in -the number of joints. - -## How it works (intuition) -Walk the chain from the base: each joint's origin is the previous origin plus the previous -orientation applied to the link offset; each joint's axis is the previous orientation applied to the -axis as written in the link; then the joint's own rotation is folded into the running orientation. -After the last joint the constant tool frame is appended. The collected origins and axes form a -"frame chain" that any Jacobian routine can consume without knowing how the arm was described. - -## Key parameters -- **Link offsets and joint axes** — the chain description. -- **Tool frame** — constant pose of the tool in the last link's frame. - -## Reference -B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, *Robotics: Modelling, Planning and Control* (2009), -Ch. 2 (direct kinematics). - -## See also -`ForwardKinematics` (shipped, position-only), `math::SE3Transform` (NT53), `GeometricJacobian` (M8). diff --git a/roadmap/kinematics/ChainPoseKinematics/implementation.md b/roadmap/kinematics/ChainPoseKinematics/implementation.md deleted file mode 100644 index 61ffb6f..0000000 --- a/roadmap/kinematics/ChainPoseKinematics/implementation.md +++ /dev/null @@ -1,75 +0,0 @@ -# Chain Pose Kinematics (Full-Pose FK + Frame Chain) — Implementation Pseudocode - -> Roadmap ref: #M30 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -The shipped `ForwardKinematics` returns joint origins and the tool point only. Pose-level algorithms -(M8, M13, M17–M19) also need the tool **orientation** and every joint **axis** in the base frame. This -spec extends the same recursion over the existing link model (`RevoluteJointLink`, and -`JointLink` since M1) and produces a model-independent `FrameChain` — the single input -the geometric Jacobian (M8) consumes, whether the frames come from this link model, from DH (M7) or -from PoE (M15). - -## Data structures - -```cpp -enum class JointType : uint8_t { Revolute, Prismatic } # shared with M1 / M7 - -template # static_assert(std::is_floating_point_v); instantiated for float -struct FrameChain: - std::array, N> jointOrigins # base frame - std::array, N> jointAxes # unit, base frame - std::array jointTypes - SE3Transform tool # tool pose in the base frame (NT53) - -template -class ChainPoseKinematics: - SE3Transform toolInLastLink # constant tool frame, injected -``` - -## Interface - -```text -explicit ChainPoseKinematics(const SE3Transform& toolInLastLink) -FrameChain Compute(const LinkArray& links, const JointVector& q) const # hot path -SE3Transform ToolPose(const LinkArray& links, const JointVector& q) const -``` - -## Algorithm (pseudocode) - -```text -function Compute(links, q): # OPTIMIZE_FOR_SPEED - R = I; o = links[0].parentToJoint - for i in 0..N-1: - if i > 0: o = o + R · links[i].parentToJoint - frames.jointOrigins[i] = o - frames.jointAxes[i] = R · links[i].jointAxis # axis is invariant under its own rotation - frames.jointTypes[i] = Revolute # (JointLink: links[i].type) - R = R · RotationAboutAxis(links[i].jointAxis, q[i]) # prismatic (M1): R unchanged, o += axis·q - frames.tool = SE3Transform{ R, o } * toolInLastLink - return frames -``` - -## Complexity & memory - -- `Compute`: `O(N)` — one Rodrigues rotation and two 3×3 products per joint. -- Memory: `2N` vectors + one transform; stack only. - -## Numerical / embedded notes - -- Consistent with the shipped `ForwardKinematics`: `jointOrigins[i]` equals its `positions[i]`, and - `tool.p` equals its last entry when `toolInLastLink = { I, toolOffset }` — assert this in a test. -- The base mounting offset is `links[0].parentToJoint`; the tool is a kinematic parameter, never - derived from a center of mass. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/ChainPoseKinematics.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class ChainPoseKinematics;` / `` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/ChainPoseKinematics.cpp` → the same three instantiations. -- Test: `robotics/kinematics/test/TestChainPoseKinematics.cpp` -- Doc: `doc/kinematics/ChainPoseKinematics.md` (or extend `ForwardKinematics.md`) -- Depends on: NT53 (`math::SE3Transform`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ChainPoseKinematics/tests.md b/roadmap/kinematics/ChainPoseKinematics/tests.md deleted file mode 100644 index dcfe8b4..0000000 --- a/roadmap/kinematics/ChainPoseKinematics/tests.md +++ /dev/null @@ -1,40 +0,0 @@ -# Chain Pose Kinematics — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestChainPoseKinematics : public ::testing::Test: - # 3-link arm: z-axis base joint raised 0.2, then two y-axis joints (0.5 up, 0.4 along x) - std::array, 3> arm{ ... } - ChainPoseKinematics kinematics{ SE3Transform{ I, (0.3, 0, 0) } } -# each case below is a TEST_F(TestChainPoseKinematics, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -origins_and_tool_match_shipped_forward_kinematics: - Assert: jointOrigins[i] and tool.p equal ForwardKinematics{ (0.3,0,0) }.Compute positions -axes_are_expressed_in_the_base_frame: - Arrange: q = (π/2, 0, 0) - Assert: jointAxes[1] ≈ (−1, 0, 0) (y rotated by +90° about z) -tool_orientation_composes_joint_rotations: - Arrange: q = (π/2, π/2, 0) - Assert: tool.R ≈ Rz(π/2)·Ry(π/2) -tool_frame_rotation_is_applied_last: - Arrange: toolInLastLink = { Rx(π/2), 0 } - Assert: tool.R ≈ Rz(q1)·Ry(q2)·Ry(q3)·Rx(π/2) -base_offset_moves_every_origin: - Assert: jointOrigins[0] == links[0].parentToJoint for all q -``` - -## Reference vectors - -- `Rz(+90°)·ŷ = −x̂`; `Rz(90°)Ry(90°)·x̂ = (0, 0, −1)`. - -## Edge cases - -- Zero-length links (coincident origins) — valid, origins repeat. -- Non-unit axis — asserted at construction of the link (M1), not here. diff --git a/robotics/kinematics/CMakeLists.txt b/robotics/kinematics/CMakeLists.txt index 25c4499..ad8413e 100644 --- a/robotics/kinematics/CMakeLists.txt +++ b/robotics/kinematics/CMakeLists.txt @@ -12,11 +12,13 @@ target_link_libraries(robotics.kinematics ${ROBOTICS_VISIBILITY} ) target_sources(robotics.kinematics PRIVATE + ChainPoseKinematics.hpp ForwardKinematics.hpp InverseKinematics.hpp ) robotics_add_coverage_sources(robotics.kinematics + ChainPoseKinematics.cpp ForwardKinematics.cpp InverseKinematics.cpp ) diff --git a/robotics/kinematics/ChainPoseKinematics.cpp b/robotics/kinematics/ChainPoseKinematics.cpp new file mode 100644 index 0000000..41d4225 --- /dev/null +++ b/robotics/kinematics/ChainPoseKinematics.cpp @@ -0,0 +1,8 @@ +#include "robotics/kinematics/ChainPoseKinematics.hpp" + +namespace kinematics +{ + template class ChainPoseKinematics; + template class ChainPoseKinematics; + template class ChainPoseKinematics; +} diff --git a/robotics/kinematics/ChainPoseKinematics.hpp b/robotics/kinematics/ChainPoseKinematics.hpp new file mode 100644 index 0000000..53e6b44 --- /dev/null +++ b/robotics/kinematics/ChainPoseKinematics.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include +#include +#include +#include + +namespace kinematics +{ + template + struct FrameChain + { + static_assert(std::is_floating_point_v, "FrameChain supports floating-point types"); + + std::array, N> jointOrigins{}; + std::array, N> jointAxes{}; + std::array jointTypes{}; + math::SE3Transform tool{}; + }; + + template + class ChainPoseKinematics + { + static_assert(std::is_floating_point_v, "ChainPoseKinematics supports floating-point types"); + static_assert(N > 0, "Number of links must be positive"); + + public: + using JointVector = math::Vector; + using LinkArray = std::array, N>; + using Transform = math::SE3Transform; + + explicit ChainPoseKinematics(const Transform& toolInLastLink); + + OPTIMIZE_FOR_SPEED FrameChain Compute(const LinkArray& links, const JointVector& q) const; + Transform ToolPose(const LinkArray& links, const JointVector& q) const; + + private: + Transform toolInLastLink; + }; + + template + ChainPoseKinematics::ChainPoseKinematics(const Transform& toolInLastLink) + : toolInLastLink{ toolInLastLink } + {} + + template + OPTIMIZE_FOR_SPEED FrameChain ChainPoseKinematics::Compute(const LinkArray& links, const JointVector& q) const + { + FrameChain frames{}; + Transform frame{}; + + for (std::size_t i = 0; i < N; ++i) + { + assert(dynamics::HasUnitJointAxis(links[i])); + const auto joint{ links[i].JointTransform(q[i]) }; + frames.jointAxes[i] = frame.rotation * links[i].jointAxis; + frames.jointTypes[i] = links[i].type; + frame = frame * joint; + frames.jointOrigins[i] = frame.translation; + } + + frames.tool = frame * toolInLastLink; + return frames; + } + + template + typename ChainPoseKinematics::Transform ChainPoseKinematics::ToolPose(const LinkArray& links, const JointVector& q) const + { + return Compute(links, q).tool; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ChainPoseKinematics; + extern template class ChainPoseKinematics; + extern template class ChainPoseKinematics; +#endif +} diff --git a/robotics/kinematics/test/CMakeLists.txt b/robotics/kinematics/test/CMakeLists.txt index 52967e6..4268fbe 100644 --- a/robotics/kinematics/test/CMakeLists.txt +++ b/robotics/kinematics/test/CMakeLists.txt @@ -8,6 +8,7 @@ target_link_libraries(robotics.kinematics_test PUBLIC ) target_sources(robotics.kinematics_test PRIVATE + TestChainPoseKinematics.cpp TestForwardKinematics.cpp TestInverseKinematics.cpp ) diff --git a/robotics/kinematics/test/TestChainPoseKinematics.cpp b/robotics/kinematics/test/TestChainPoseKinematics.cpp new file mode 100644 index 0000000..e05ab70 --- /dev/null +++ b/robotics/kinematics/test/TestChainPoseKinematics.cpp @@ -0,0 +1,90 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Tolerance.hpp" +#include "robotics/kinematics/ChainPoseKinematics.hpp" +#include "robotics/kinematics/ForwardKinematics.hpp" +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Link = dynamics::RevoluteJointLink; + using Transform = math::SE3Transform; + + constexpr float halfPi{ std::numbers::pi_v / 2.0f }; + const Vector3 xAxis{ 1.0f, 0.0f, 0.0f }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeLink(const Vector3& axis, const Vector3& parentToJoint) + { + return Link{ 1.0f, Matrix3::Identity(), axis, parentToJoint, Vector3{} }; + } + + class TestChainPoseKinematics + : 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()); + } + + 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()); + } + + std::array arm{ MakeLink(zAxis, Vector3{ 0.0f, 0.0f, 0.2f }), MakeLink(yAxis, Vector3{ 0.0f, 0.0f, 0.5f }), MakeLink(yAxis, Vector3{ 0.4f, 0.0f, 0.0f }) }; + kinematics::ChainPoseKinematics kinematics{ Transform{ Matrix3::Identity(), Vector3{ 0.3f, 0.0f, 0.0f } } }; + }; +} + +TEST_F(TestChainPoseKinematics, origins_and_tool_match_shipped_forward_kinematics) +{ + const math::Vector q{ 0.3f, -0.7f, 1.1f }; + const kinematics::ForwardKinematics fk{ Vector3{ 0.3f, 0.0f, 0.0f } }; + + const auto frames{ kinematics.Compute(arm, q) }; + const auto positions{ fk.Compute(arm, q) }; + + for (std::size_t i = 0; i < 3; ++i) + ExpectNear(frames.jointOrigins[i], positions[i]); + ExpectNear(frames.tool.translation, positions[3]); +} + +TEST_F(TestChainPoseKinematics, axes_are_expressed_in_the_base_frame) +{ + const auto frames{ kinematics.Compute(arm, math::Vector{ halfPi, 0.0f, 0.0f }) }; + + ExpectNear(frames.jointAxes[1], Vector3{ -1.0f, 0.0f, 0.0f }); +} + +TEST_F(TestChainPoseKinematics, tool_orientation_composes_joint_rotations) +{ + const auto frames{ kinematics.Compute(arm, math::Vector{ halfPi, halfPi, 0.0f }) }; + + ExpectNear(frames.tool.rotation, Matrix3{ math::RotationAboutAxis(zAxis, halfPi) * math::RotationAboutAxis(yAxis, halfPi) }); + ExpectNear(frames.tool.rotation * xAxis, Vector3{ 0.0f, 0.0f, -1.0f }); +} + +TEST_F(TestChainPoseKinematics, tool_frame_rotation_is_applied_last) +{ + const kinematics::ChainPoseKinematics rotatedTool{ Transform{ math::RotationAboutAxis(xAxis, halfPi), Vector3{} } }; + const math::Vector q{ 0.4f, -0.3f, 0.9f }; + + const auto tool{ rotatedTool.ToolPose(arm, q) }; + + const Matrix3 expected{ math::RotationAboutAxis(zAxis, q[0]) * math::RotationAboutAxis(yAxis, q[1]) * math::RotationAboutAxis(yAxis, q[2]) * math::RotationAboutAxis(xAxis, halfPi) }; + ExpectNear(tool.rotation, expected); +} + +TEST_F(TestChainPoseKinematics, base_offset_moves_every_origin) +{ + for (const auto& q : { math::Vector{}, math::Vector{ 1.2f, -0.4f, 2.0f }, math::Vector{ -2.5f, 0.8f, -1.1f } }) + ExpectNear(kinematics.Compute(arm, q).jointOrigins[0], arm[0].parentToJoint); +} From 9acd4cc42f4c88f53e5912b26ca4e60d36c5f254 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 11:21:00 +0000 Subject: [PATCH 3/4] feat(kinematics): add GeometricJacobian and DenavitHartenberg (M8, M7) GeometricJacobian builds the 6xN Jacobian, the bias acceleration and the statics map from any FrameChain, with revolute and prismatic columns. JacobianProvider is the seam for task-space consumers; ChainTaskJacobian and DhTaskJacobian implement it for link models and DH tables. DenavitHartenberg covers standard and modified conventions with constant joint offsets. InverseKinematics now uses the shared Jacobian instead of its private 3xN one. M7 and M8 ship together because each one's tests need the other. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 23 +-- doc/kinematics/DenavitHartenberg.md | 96 ++++++++++ doc/kinematics/GeometricJacobian.md | 101 +++++++++++ doc/kinematics/InverseKinematics.md | 3 +- doc/kinematics/README.md | 12 +- roadmap/README.md | 4 +- .../DenavitHartenberg/explanation.md | 40 ----- .../DenavitHartenberg/implementation.md | 118 ------------- roadmap/kinematics/DenavitHartenberg/tests.md | 64 ------- .../GeometricJacobian/explanation.md | 32 ---- .../GeometricJacobian/implementation.md | 106 ----------- roadmap/kinematics/GeometricJacobian/tests.md | 49 ------ robotics/kinematics/CMakeLists.txt | 7 + robotics/kinematics/ChainTaskJacobian.hpp | 65 +++++++ robotics/kinematics/DenavitHartenberg.cpp | 10 ++ robotics/kinematics/DenavitHartenberg.hpp | 150 ++++++++++++++++ robotics/kinematics/DhTaskJacobian.hpp | 58 +++++++ robotics/kinematics/GeometricJacobian.cpp | 12 ++ robotics/kinematics/GeometricJacobian.hpp | 135 ++++++++++++++ robotics/kinematics/InverseKinematics.hpp | 49 +----- robotics/kinematics/JacobianProvider.hpp | 27 +++ robotics/kinematics/test/CMakeLists.txt | 2 + .../kinematics/test/TestDenavitHartenberg.cpp | 151 ++++++++++++++++ .../kinematics/test/TestGeometricJacobian.cpp | 164 ++++++++++++++++++ 25 files changed, 1007 insertions(+), 473 deletions(-) create mode 100644 doc/kinematics/DenavitHartenberg.md create mode 100644 doc/kinematics/GeometricJacobian.md delete mode 100644 roadmap/kinematics/DenavitHartenberg/explanation.md delete mode 100644 roadmap/kinematics/DenavitHartenberg/implementation.md delete mode 100644 roadmap/kinematics/DenavitHartenberg/tests.md delete mode 100644 roadmap/kinematics/GeometricJacobian/explanation.md delete mode 100644 roadmap/kinematics/GeometricJacobian/implementation.md delete mode 100644 roadmap/kinematics/GeometricJacobian/tests.md create mode 100644 robotics/kinematics/ChainTaskJacobian.hpp create mode 100644 robotics/kinematics/DenavitHartenberg.cpp create mode 100644 robotics/kinematics/DenavitHartenberg.hpp create mode 100644 robotics/kinematics/DhTaskJacobian.hpp create mode 100644 robotics/kinematics/GeometricJacobian.cpp create mode 100644 robotics/kinematics/GeometricJacobian.hpp create mode 100644 robotics/kinematics/JacobianProvider.hpp create mode 100644 robotics/kinematics/test/TestDenavitHartenberg.cpp create mode 100644 robotics/kinematics/test/TestGeometricJacobian.cpp diff --git a/README.md b/README.md index dee5075..100c0df 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. | Category | Description | |--------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Inverse Kinematics (Damped Least Squares) | +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Inverse Kinematics (Damped Least Squares) | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), 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 | diff --git a/ROADMAP.md b/ROADMAP.md index 866da48..e7861d1 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -94,14 +94,15 @@ The library already has the core of manipulator **modelling**: forward/inverse d [Euler-Lagrange](robotics/dynamics/EulerLagrangeSolver.hpp)), the joint-space mass matrix ([CRBA](robotics/dynamics/CompositeRigidBodyAlgorithm.hpp)), a fixed-step simulation step ([ForwardDynamicsIntegrator](robotics/dynamics/ForwardDynamicsIntegrator.hpp)), position forward kinematics with base and -tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). What is missing is -the **full-pose kinematics, model plumbing, planning and control layer** that turns those models into a -usable manipulator stack. Two current limitations gate many items below: +tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). Full-pose +kinematics and the geometric Jacobian have shipped (see the note below); what is missing is the +**model plumbing, planning and control layer** that turns those models into a usable manipulator stack. -> **Position-only.** [ForwardKinematics.hpp](robotics/kinematics/ForwardKinematics.hpp) returns joint -> and tool *positions* (a 3×N Jacobian lives privately inside IK). Links are generic -> ([JointLink](robotics/dynamics/JointLink.hpp): revolute or prismatic, limits, armature). Items **M30** -> (full-pose kinematics) and **M8** (6×N Jacobian) lift the position-only limit. +> **Full-pose kinematics shipped.** Links are generic ([JointLink](robotics/dynamics/JointLink.hpp): +> revolute or prismatic, limits, armature); [ChainPoseKinematics](robotics/kinematics/ChainPoseKinematics.hpp) +> and [DenavitHartenberg](robotics/kinematics/DenavitHartenberg.hpp) produce a frame chain, and +> [GeometricJacobian](robotics/kinematics/GeometricJacobian.hpp) builds the 6×N Jacobian from it behind +> the [JacobianProvider](robotics/kinematics/JacobianProvider.hpp) seam. **Conventions (normative for every item):** 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches `(f; n)` — as defined by upstream `math::SE3Transform` (**NT53**); the geometric Jacobian @@ -119,8 +120,6 @@ existing `dynamics/` convention. |-----|-----------------------------------------------------------------|---------------------------|----------------|------------| | M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | | M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | -| M7 | Denavit-Hartenberg parameters (standard + modified) | `kinematics` | NT53, M30 | ★★☆☆☆ | -| M8 | Geometric Jacobian (6×N), bias term, Jacobian provider | `kinematics` | NT53, M30 | ★★☆☆☆ | | 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 | ★★☆☆☆ | @@ -155,12 +154,6 @@ retired here. **M29. Chain dynamics model.** Adapter implementing `EulerLagrangeDynamics` (M via CRBA, `Cq̇` and `g` via RNEA) and a one-call `InverseDynamicsModel` for the controllers. - *Reuses / builds on:* RNEA, M28; optional friction model from NT57. -**M7. Denavit-Hartenberg parameters.** Standard and modified `(a, α, d, θ)` descriptions with joint offsets, producing the M30 frame chain. -- *Algorithm / paper:* Craig, *Introduction to Robotics*, Ch. 3. - -**M8. Geometric Jacobian (6×N).** Jacobian and `J̇q̇` from any frame chain, statics `τ = Jᵀw`, and the `JacobianProvider` seam used by IK and the task-space controllers; replaces IK's private 3×N Jacobian. -- *Algorithm / paper:* Siciliano et al. (2009), Ch. 3; Lynch & Park (2017), Ch. 5. - **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. diff --git a/doc/kinematics/DenavitHartenberg.md b/doc/kinematics/DenavitHartenberg.md new file mode 100644 index 0000000..2b2f84d --- /dev/null +++ b/doc/kinematics/DenavitHartenberg.md @@ -0,0 +1,96 @@ +# Denavit-Hartenberg Parameters + +## Overview & Motivation + +A serial arm can be described link by link with arbitrary offsets and axes. The **Denavit–Hartenberg (DH)** convention instead attaches frames to the links by a fixed rule, so every joint–link pair is captured by just **four numbers** $(a, \alpha, d, \theta)$. +DH tables are the lingua franca of industrial robotics: manufacturers publish them, textbooks tabulate classic arms such as the PUMA-560, and calibration methods identify their errors. One of the four numbers is the joint variable: $\theta$ for a revolute joint, $d$ for a prismatic one. + +From a DH table the model produces the tool pose and the frame chain (joint origins, axes and types in the base frame), so the same geometric Jacobian and every task-space algorithm built on it work on DH-described arms unchanged. + +## Mathematical Theory + +### Parameters + +For link $i$: + +- $a$ — link length, the distance between consecutive $z$ axes along their common normal $x$; +- $\alpha$ — link twist, the angle between consecutive $z$ axes about $x$; +- $d$ — link offset along $z$ (variable for a prismatic joint); +- $\theta$ — joint angle about $z$ (variable for a revolute joint). + +Real tables carry constant offsets on the variable parameter as well. The joint variable is **added**, $\theta = \theta_0 + q$ or $d = d_0 + q$, never substituted. + +### Standard (Distal) Convention + +Frame $i$ sits at the distal end of link $i$, and joint $i$ acts about $z_{i-1}$: + +$$A_i = \mathrm{Rot}_z(\theta)\,\mathrm{Trans}_z(d)\,\mathrm{Trans}_x(a)\,\mathrm{Rot}_x(\alpha) = \begin{bmatrix} c_\theta & -s_\theta c_\alpha & s_\theta s_\alpha & a c_\theta \\ s_\theta & c_\theta c_\alpha & -c_\theta s_\alpha & a s_\theta \\ 0 & s_\alpha & c_\alpha & d \\ 0 & 0 & 0 & 1 \end{bmatrix}$$ + +### Modified (Proximal, Craig) Convention + +Frame $i$ sits at the proximal end, and joint $i$ acts about $z_i$. Row $i$ holds the twist and length of the **previous** link: + +$$A_i = \mathrm{Rot}_x(\alpha_{i-1})\,\mathrm{Trans}_x(a_{i-1})\,\mathrm{Rot}_z(\theta)\,\mathrm{Trans}_z(d) = \begin{bmatrix} c_\theta & -s_\theta & 0 & a_{i-1} \\ s_\theta c_\alpha & c_\theta c_\alpha & -s_\alpha & -s_\alpha d \\ s_\theta s_\alpha & c_\theta s_\alpha & c_\alpha & c_\alpha d \\ 0 & 0 & 0 & 1 \end{bmatrix}$$ + +The conventions differ in row layout, not just factor order. The last standard $(a_n, \alpha_n)$ moves into the constant tool transform of the modified table. Described consistently, both yield the same tool pose and the same frame chain. + +### Tool Pose and Frame Chain + +$$T_{\text{tool}} = A_1(q_1)\,A_2(q_2) \cdots A_n(q_n)\,T_t$$ + +with $T_t$ the constant tool transform in the last DH frame. The frame chain records, for each joint, the origin and $z$ axis of the frame it acts about. In the standard convention that is the running product **before** $A_i$; in the modified convention, **after** it. +For revolute joints, any point on the axis is a valid origin. The modified origin, which includes $d_i$, therefore gives the same Jacobian column. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|----------------|--------|--------|-------------------------------------------------------------------| +| Link transform | $O(1)$ | $O(1)$ | One sine–cosine pair; the twist's sine and cosine are precomputed | +| Tool pose | $O(n)$ | $O(1)$ | $n$ rigid-transform products | +| Frame chain | $O(n)$ | $O(n)$ | Same products, plus $2n$ stored vectors | + +## Step-by-Step Walkthrough + +A planar two-link arm with unit links, standard table $(a, \alpha, d, \theta) = (1, 0, 0, q_1)$, $(1, 0, 0, q_2)$, at $q = (0, \pi/2)$: + +1. $A_1(0)$: no rotation, translation $(1, 0, 0)$. +2. $A_2(\pi/2)$: rotation $\mathrm{Rot}_z(\pi/2)$, translation $(a c_\theta, a s_\theta, 0) = (0, 1, 0)$. +3. $T = A_1 A_2$: position $(1, 0, 0) + (0, 1, 0) = (1, 1, 0)$, the elbow bent upwards. +4. The modified table $(0, 0, 0, q_1)$, $(1, 0, 0, q_2)$ with tool translation $(1, 0, 0)$ gives $\mathrm{Rot}_z(q_1) \cdot (\mathrm{Trans}_x(1)\,\mathrm{Rot}_z(q_2)) \cdot \mathrm{Trans}_x(1)$, which is the same pose. +5. With a constant offset $\theta_{0,1} = \pi/2$ the stretched configuration points along $y$: $q = (0, 0)$ gives $(0, 2, 0)$. + +## Pitfalls & Edge Cases + +- **Mixing conventions**: a table written for one convention and evaluated with the other gives plausible but wrong poses. Check which one a datasheet uses, and remember the modified table's shifted $(a, \alpha)$. +- **Dropping offsets**: published tables often have constant $\theta$ offsets (e.g. $-\pi/2$ on a shoulder) so that the zero configuration matches the robot's home pose. Substituting $q$ instead of adding it moves the whole arm. +- **Parallel consecutive axes**: the common normal is not unique, and small manufacturing errors cause large parameter jumps. DH is ill-conditioned for calibration there; modified DH or extra parameters (Hayati) fix it. +- **Twist sign**: $\alpha = +\pi/2$ maps $z_{i}$ onto $-y_{i-1}$ in the standard form. A sign slip flips the following axes. +- **Prismatic joints**: the variable is $d$; $\theta$ stays constant and must still include any offset. + +## Variants & Generalizations + +- **Hayati–Mirmirani parameters**: an extra rotation about $y$ handles nearly parallel axes for calibration. +- **Product of exponentials**: screw axes in the base frame avoid per-link frame rules entirely and degrade gracefully near parallel axes. +- **Sheth–Uicker two-frame convention**: separates joint and link frames for general mechanisms. +- **Branched and closed chains**: DH applies per serial branch; closed loops add constraint equations. + +## Applications + +- Modelling industrial arms directly from manufacturer datasheets. +- Kinematic calibration, which identifies DH parameter errors from measured tool poses. +- Classical closed-form inverse kinematics for arms with spherical wrists, derived in DH frames. +- Teaching and verification against textbook tables (PUMA-560, Stanford arm, SCARA). + +## Connections to Other Algorithms + +- [Geometric Jacobian](GeometricJacobian.md): consumes the frame chain produced here; DH-described arms share its columns, statics and bias acceleration. +- [Chain Pose Kinematics](ChainPoseKinematics.md): the link-by-link alternative that produces the same frame chain. +- [Forward Kinematics](ForwardKinematics.md): the position-only special case for link-by-link models. +- Rigid transforms and their products come from the SE(3) primitives of [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Denavit, J. and Hartenberg, R.S. (1955). "A kinematic notation for lower-pair mechanisms based on matrices." *ASME Journal of Applied Mechanics*, 22, 215–221. +- Craig, J.J. (2005). *Introduction to Robotics: Mechanics and Control*. 3rd ed. Pearson. Chapter 3. +- Spong, M.W., Hutchinson, S. and Vidyasagar, M. (2020). *Robot Modeling and Control*. 2nd ed. Wiley. Chapter 3. +- Corke, P.I. (2007). "A simple and systematic approach to assigning Denavit–Hartenberg parameters." *IEEE Transactions on Robotics*, 23(3), 590–594. diff --git a/doc/kinematics/GeometricJacobian.md b/doc/kinematics/GeometricJacobian.md new file mode 100644 index 0000000..91ee1f9 --- /dev/null +++ b/doc/kinematics/GeometricJacobian.md @@ -0,0 +1,101 @@ +# Geometric Jacobian + +## Overview & Motivation + +The geometric Jacobian $J(q) \in \mathbb{R}^{6 \times n}$ is the linear map from joint rates to the tool's spatial velocity: + +$$\begin{bmatrix} v \\ \omega \end{bmatrix} = J(q)\,\dot{q}$$ + +Here $v$ is the linear velocity of the **tool point** and $\omega$ the angular velocity of the tool frame, both in the base frame, linear part first. Nearly every task-space algorithm runs on it: + +- damped-least-squares inverse kinematics inverts it; +- manipulability analysis decomposes it; +- redundancy resolution projects into its null space; +- impedance and operational-space control use it to map wrenches back to joint torques. + +Its transpose gives the **statics** of the arm: the joint torques that balance a wrench applied at the tool are $\tau = J^T w$. + +Built from a **frame chain**, the joint origins, axes and types in the base frame, the Jacobian is independent of how the arm was described. Link-by-link models, Denavit–Hartenberg tables and screw-axis descriptions all reduce to one implementation. + +## Mathematical Theory + +### Columns + +Let $p$ be the tool point, and $o_i$, $\hat{z}_i$ the origin and unit axis of joint $i$ in the base frame. A unit rate of joint $i$ with all others locked moves the tool by + +$$J_i = \begin{cases} \begin{bmatrix} \hat{z}_i \times (p - o_i) \\ \hat{z}_i \end{bmatrix} & \text{revolute} \\[2ex] \begin{bmatrix} \hat{z}_i \\ 0 \end{bmatrix} & \text{prismatic} \end{cases}$$ + +A revolute column depends on $o_i$ only through $p - o_i$ crossed with $\hat{z}_i$, so any point on the joint axis may serve as the origin. + +### Statics + +By the principle of virtual work, a wrench $w = (f; n)$ applied at the tool point (force first, moment about the tool point) does power $w^T J \dot{q}$, which must equal $\tau^T \dot{q}$ for every $\dot{q}$: + +$$\tau = J^T w$$ + +A force whose line of action passes through every revolute axis produces no joint torque. + +### Bias Acceleration + +Differentiating $\dot{x} = J\dot{q}$ gives $\ddot{x} = J\ddot{q} + \dot{J}\dot{q}$. The **bias acceleration** $\dot{J}\dot{q}$ is the tool acceleration at zero joint acceleration, needed by operational-space control and Cartesian tracking. +Let $\omega_{i-1}$ be the angular velocity of the link that carries axis $i$; the axis then turns at $\dot{\hat{z}}_i = \omega_{i-1} \times \hat{z}_i$. Differentiating each column gives + +$$\dot{J}_i\dot{q}_i = \begin{cases} \dot{q}_i \begin{bmatrix} \dot{\hat{z}}_i \times (p - o_i) + \hat{z}_i \times (\dot{p} - \dot{o}_i) \\ \dot{\hat{z}}_i \end{bmatrix} & \text{revolute} \\[2ex] \dot{q}_i \begin{bmatrix} \dot{\hat{z}}_i \\ 0 \end{bmatrix} & \text{prismatic} \end{cases}$$ + +Here $\dot{p} = J_v\dot{q}$ is the tool-point velocity and $\dot{o}_i$ the velocity of the joint origin. Both $\omega$ and $\dot{o}$ are accumulated base to tip in the same pass: $\dot{o}_i = \dot{o}_{i-1} + \omega_{i-1} \times (o_i - o_{i-1})$, plus the sliding rate of a prismatic joint. No second derivative of the kinematics is ever formed. + +### Task Rows + +Position-only tasks use the top three rows (the tool-point linear velocity). Full-pose tasks use all six. The same rows of $\dot{J}\dot{q}$ follow. + +## Complexity Analysis + +| Quantity | Time | Space | Notes | +|-------------------|---------|--------|------------------------------------------------------| +| Jacobian | $O(n)$ | $O(n)$ | One cross product per revolute column | +| Bias acceleration | $O(n)$ | $O(1)$ | One base-to-tip pass after the $O(6n)$ tool velocity | +| Joint torques | $O(6n)$ | $O(n)$ | One transposed matrix–vector product | + +## Step-by-Step Walkthrough + +A planar two-link arm with unit links and joints about $\hat{z}$, stretched along $x$ ($q = (0, 0)$): + +1. Frame chain: $o_0 = (0,0,0)$, $o_1 = (1,0,0)$, $\hat{z}_0 = \hat{z}_1 = \hat{z}$, tool $p = (2,0,0)$. +2. Column 0: $\hat{z} \times (2,0,0) = (0,2,0)$, angular $(0,0,1)$. Column 1: $\hat{z} \times (1,0,0) = (0,1,0)$, angular $(0,0,1)$. +3. $J = \begin{bmatrix} 0 & 0 \\ 2 & 1 \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \\ 1 & 1 \end{bmatrix}$. The linear columns are parallel, so the arm is singular: it cannot move the tool along $x$. +4. A tool force $(1,0,0)$ along the links gives $\tau = J^T w = (0, 0)$: the structure carries it without any joint torque. + +## Pitfalls & Edge Cases + +- **Row ordering**: this Jacobian is linear-first, $(v;\omega)$. Featherstone's spatial vectors and many robotics libraries order angular first; mixing the two silently swaps blocks. +- **Reference point**: the linear rows are the velocity of the tool point, not of the base origin. The "space Jacobian" of the product-of-exponentials literature differs by a frame shift and is a different matrix. +- **Wrench reference**: in $\tau = J^T w$, the moment must be taken about the tool point. A moment about another point must be shifted first. +- **Singularities**: near a singularity the columns become dependent and any inverse blows up. Use damping or a pseudo-inverse with a rank threshold, and monitor manipulability. +- **Frame-chain conventions**: for a standard Denavit–Hartenberg table, joint $i$ acts about the $z$ axis of the frame **before** its link; for the modified convention, about the frame **of** its link. A wrong frame shifts every column. + +## Variants & Generalizations + +- **Analytic Jacobian**: derivative of a minimal orientation parameterization (Euler angles, quaternion). It differs from the geometric Jacobian by an orientation-dependent map that is singular for Euler angles. +- **Body Jacobian**: velocities expressed in the tool frame, obtained by rotating both blocks by $R^T$. +- **Space Jacobian**: twist coordinates about the base origin, natural for product-of-exponentials kinematics. +- **Point Jacobians**: the same column formula with $p$ replaced by any point on any link, used for contact points and collision avoidance. + +## Applications + +- Damped-least-squares and Levenberg–Marquardt inverse kinematics. +- Resolved-rate and Cartesian velocity control; null-space redundancy resolution. +- Impedance, admittance and operational-space control, which map task-space forces to joint torques. +- Force estimation from joint torques and static payload compensation. + +## Connections to Other Algorithms + +- [Chain Pose Kinematics](ChainPoseKinematics.md): produces the frame chain from a link-by-link model. +- [Denavit-Hartenberg Parameters](DenavitHartenberg.md): produces the same frame chain from a DH table. +- [Inverse Kinematics](InverseKinematics.md): uses the position rows of this Jacobian in its damped-least-squares step. +- [Generic Joint Link](../dynamics/JointLink.md): defines the revolute and prismatic joint types that select each column's form. + +## References & Further Reading + +- 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. +- Orin, D.E. and Schrader, W.W. (1984). "Efficient computation of the Jacobian for robot manipulators." *International Journal of Robotics Research*, 3(4), 66–75. diff --git a/doc/kinematics/InverseKinematics.md b/doc/kinematics/InverseKinematics.md index f09131a..a48f679 100644 --- a/doc/kinematics/InverseKinematics.md +++ b/doc/kinematics/InverseKinematics.md @@ -115,7 +115,8 @@ After 7 iterations the error drops below $10^{-4}$ and $p_{\text{ee}} \approx (0 ## Connections to Other Algorithms -- [Forward Kinematics](ForwardKinematics.md): Called internally every iteration to compute current end-effector position and joint positions for the Jacobian. +- [Chain Pose Kinematics](ChainPoseKinematics.md): Called internally every iteration to compute the current tool position and the joint origins and axes. +- [Geometric Jacobian](GeometricJacobian.md): Its position rows are the $3 \times n$ Jacobian of the damped-least-squares step; prismatic joints contribute their axis as the column. - Gaussian elimination ([numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)): Solves the $3 \times 3$ DLS system $Ay = e$ at the core of each iteration. - 3D geometry primitives (cross product, axis–angle rotation, vector norm) from numerical-toolbox-cpp build the Jacobian columns, the accumulated rotations and the convergence test. diff --git a/doc/kinematics/README.md b/doc/kinematics/README.md index 668b82f..ba855d4 100644 --- a/doc/kinematics/README.md +++ b/doc/kinematics/README.md @@ -4,8 +4,10 @@ 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)$ | -| [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)$ | +| [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/roadmap/README.md b/roadmap/README.md index 14d9e1e..92436ee 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -30,7 +30,7 @@ Conventions shared by every spec: 6-vectors are ordered linear part first — tw 3-vectors `(vx, vy, ω)` by [math/SE2Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE2Transform/implementation.md) (NT66); dynamics and Jacobians reach controllers through interfaces implemented by [dynamics/ChainDynamicsModel](dynamics/ChainDynamicsModel/implementation.md) and -[kinematics/GeometricJacobian](kinematics/GeometricJacobian/implementation.md). +[kinematics/JacobianProvider](../robotics/kinematics/JacobianProvider.hpp) (shipped). ## Deployment shape (float-only) @@ -89,7 +89,7 @@ prerequisites. ## Index ### `kinematics` -`DenavitHartenberg` (M7) · `GeometricJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) +`ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) ### `dynamics` `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) diff --git a/roadmap/kinematics/DenavitHartenberg/explanation.md b/roadmap/kinematics/DenavitHartenberg/explanation.md deleted file mode 100644 index 3ccd19c..0000000 --- a/roadmap/kinematics/DenavitHartenberg/explanation.md +++ /dev/null @@ -1,40 +0,0 @@ -# Denavit-Hartenberg Parameters — Overview - -## What it is -A minimal four-number recipe — `(a, α, d, θ)` — that describes where each link of a robot arm sits -relative to the previous one. Each row generates one rigid transform; multiplying them down the chain -gives the forward kinematics, and reading each joint's axis off the intermediate frames gives the input -the Jacobian needs. - -## Why it matters (embedded) -DH is the *lingua franca* of industrial robotics: nearly every arm's datasheet ships a DH table. -Encoding a manipulator as `N×4` constants (plus a joint-type flag) is the most compact possible model, -and the per-link transform is one sine/cosine pair plus a fixed fill — cheap enough for a servo loop on -a microcontroller. Emitting the shared frame chain lets DH-described arms reuse the library's single -Jacobian, inverse-kinematics and control code. - -## How it works (intuition) -The convention forces every joint axis onto a frame's `z`-axis and every common normal onto an -`x`-axis. That discipline collapses the six numbers of a general rigid transform down to four: two -describe the joint (`d` slides along `z`, `θ` rotates about `z`) and two describe the link (`a` slides -along `x`, `α` twists about `x`). The joint variable is *added* to the constant `θ` (revolute) or `d` -(prismatic), because real tables carry offsets. The standard (distal) convention attaches frame `i` at -the far end of link `i`, so joint `i` turns about the `z`-axis of the previous frame; the modified -(proximal, Craig) convention attaches it at the near end, so joint `i` turns about its own frame's -`z`-axis and each row carries the previous link's `a` and `α`. Both describe the same arm. - -## Key parameters -- **a (link length), α (link twist)** — fixed geometry of the link. -- **d (link offset), θ (joint angle)** — constant offsets; the joint variable adds to one of them. -- **convention** — standard (distal) or modified (proximal); rows differ between the two. -- **tool transform** — constant pose of the tool in the last DH frame. - -## Reference -J. Denavit, R. S. Hartenberg, "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices," -*ASME J. Applied Mechanics*, 1955; J. J. Craig, *Introduction to Robotics: Mechanics and Control*, -4th ed., Ch. 3 (modified convention). - -## See also -`math::SE3Transform` (NT53, the per-link transform), `ChainPoseKinematics` (M30, the `FrameChain` it emits), -`GeometricJacobian` (M8, consumes the frame chain), `ProductOfExponentials` (M15, the screw-theory -alternative). diff --git a/roadmap/kinematics/DenavitHartenberg/implementation.md b/roadmap/kinematics/DenavitHartenberg/implementation.md deleted file mode 100644 index 6984e3c..0000000 --- a/roadmap/kinematics/DenavitHartenberg/implementation.md +++ /dev/null @@ -1,118 +0,0 @@ -# Denavit-Hartenberg Parameters — Implementation Pseudocode - -> Roadmap ref: #M7 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Produces the tool pose and the M30 `FrameChain` from a DH table, so the geometric Jacobian (M8) and -everything built on `JacobianProvider` work on DH-described arms unchanged. - -## Data structures - -```cpp -enum class JointType : uint8_t { Revolute, Prismatic } # shared with M30 / M1 -enum class DhConvention : uint8_t { Standard, Modified } # distal (classic) vs proximal (Craig) - -template # static_assert(std::is_floating_point_v); instantiated for float -struct DhLink: # Standard row i: (aᵢ, αᵢ, dᵢ, θᵢ); Modified row i: (aᵢ₋₁, αᵢ₋₁, dᵢ, θᵢ) - T a # link length along x - T alpha # link twist about x - T d # constant offset along z (added to q when Prismatic) - T theta # constant offset about z (added to q when Revolute) - JointType type - -template -class DenavitHartenberg: - std::array, N> links - DhConvention convention - SE3Transform tool # constant tool pose in the last DH frame (NT53) - std::array cosAlpha, sinAlpha # precomputed at construction - -template # TaskDim ∈ {3, 6} -class DhTaskJacobian : public JacobianProvider: # M8 seam, mirrors ChainTaskJacobian - const DenavitHartenberg& model -``` - -## Interface - -```cpp -DenavitHartenberg(const std::array, N>& links, DhConvention convention, - const SE3Transform& tool = SE3Transform::Identity()) -SE3Transform LinkTransform(std::size_t i, T q) const # one Aᵢ; hot path -SE3Transform Forward(const JointVector& q) const # tool pose ⁰Tₙ·tool; hot path -FrameChain Frames(const JointVector& q) const # M30 frame chain (for M8) - -# DhTaskJacobian: Jacobian(q) = rows of GeometricJacobian::Compute(model.Frames(q)) (TaskDim = 3 → linear rows), -# BiasAcceleration(q, q̇) = rows of GeometricJacobian::BiasAcceleration(model.Frames(q), q̇), -# ToolPose(q) = model.Forward(q) -``` - -## Algorithm (pseudocode) - -```text -function LinkTransform(i, q): # OPTIMIZE_FOR_SPEED - link = links[i] - θ = link.theta + (link.type == Revolute ? q : 0) - d = link.d + (link.type == Prismatic ? q : 0) - (cθ, sθ) = (cos θ, sin θ); (cα, sα) = (cosAlpha[i], sinAlpha[i]) - if convention == Standard: # Rotz(θ)·Transz(d)·Transx(a)·Rotx(α) - R = [[ cθ, −sθ·cα, sθ·sα ], - [ sθ, cθ·cα, −cθ·sα ], - [ 0, sα, cα ]] - p = (a·cθ, a·sθ, d) - else: # Modified: Rotx(αᵢ₋₁)·Transx(aᵢ₋₁)·Rotz(θ)·Transz(d) - R = [[ cθ, −sθ, 0 ], - [ sθ·cα, cθ·cα, −sα ], - [ sθ·sα, cθ·sα, cα ]] - p = (a, −sα·d, cα·d) - return { R, p } - -function Forward(q): # OPTIMIZE_FOR_SPEED - A = Identity - for i in 0..N-1: A = A * LinkTransform(i, q[i]) - return A * tool - -function Frames(q): - A = Identity - for i in 0..N-1: - if convention == Standard: # joint i acts about z of frame i−1 (the frame before its link) - frames.jointOrigins[i] = A.p; frames.jointAxes[i] = A.R · ẑ - A = A * LinkTransform(i, q[i]) - else: # Modified: joint i acts about z of frame i - A = A * LinkTransform(i, q[i]) - frames.jointOrigins[i] = A.p; frames.jointAxes[i] = A.R · ẑ - frames.jointTypes[i] = links[i].type - frames.tool = A * tool - return frames -``` - -## Complexity & memory - -- `LinkTransform`: `O(1)` — one `sin`/`cos` pair and a fixed 3×3 fill (`α` terms precomputed). -- `Forward` / `Frames`: `O(N)` SE(3) products; `Frames` stores `2N` vectors + one transform. -- Memory: the constant link table plus `2N` precomputed trig values; stack only. - -## Numerical / embedded notes - -- **Offsets are kept:** real DH tables carry constant `θ` offsets on revolute joints and constant `d` - on prismatic ones; the joint variable is *added*, never substituted. -- **Conventions differ in the row layout, not just the factor order:** a modified row holds - `(aᵢ₋₁, αᵢ₋₁)` of the previous link, and the last standard `(aₙ, αₙ)` moves into `tool`. The same - arm described both ways yields identical `Forward` and `Frames`. -- The origin written into `FrameChain` is any point on the joint axis; for revolute columns - `z × (p − o)` is unchanged by sliding `o` along `z`, so the modified-frame origin (which includes `dᵢ`) - is valid. -- Both factored forms are exactly orthonormal — no re-orthonormalization needed. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/DenavitHartenberg.hpp` (+ `DhTaskJacobian.hpp`) — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `LinkTransform`/`Forward`, and - `extern template class DenavitHartenberg;` / `` / `` plus - `extern template class DhTaskJacobian;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/DenavitHartenberg.cpp` → the same instantiations. -- Test: `robotics/kinematics/test/TestDenavitHartenberg.cpp` -- Doc: `doc/kinematics/DenavitHartenberg.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestDenavitHartenberg.cpp` → the `_test` target. -- Depends on: NT53 (`math::SE3Transform`), M30 (`FrameChain`), M8 (`GeometricJacobian`, `JacobianProvider`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/DenavitHartenberg/tests.md b/roadmap/kinematics/DenavitHartenberg/tests.md deleted file mode 100644 index 1d86e95..0000000 --- a/roadmap/kinematics/DenavitHartenberg/tests.md +++ /dev/null @@ -1,64 +0,0 @@ -# Denavit-Hartenberg Parameters — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestDenavitHartenberg : public ::testing::Test: - # 2-link unit planar arm, both revolute (a, α, d, θ, type) - DenavitHartenberg standard{ { {1,0,0,0,Revolute}, {1,0,0,0,Revolute} }, Standard } - DenavitHartenberg modified{ { {0,0,0,0,Revolute}, {1,0,0,0,Revolute} }, Modified, - SE3Transform{ I, (1, 0, 0) } } # a₂ moves into the tool -# each case below is a TEST_F(TestDenavitHartenberg, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -zero_config_is_stretched_along_x: - Assert: standard.Forward({0, 0}).p ≈ (2, 0, 0), R ≈ I -planar_elbow_ninety_deg: - Assert: standard.Forward({0, π/2}).p ≈ (1, 1, 0) -single_link_revolute_rotates: - Arrange: 1-link standard arm {1,0,0,0,Revolute} - Assert: Forward({π/2}).p ≈ (0, 1, 0) -theta_offset_is_added_to_the_joint_variable: - Arrange: standard table with link 1 θ = π/2 - Assert: Forward({0, 0}).p ≈ (0, 2, 0); Forward({0, π/2}).p ≈ (−1, 1, 0) -prismatic_variable_adds_to_d: - Arrange: 1-link {0,0,0.2,0,Prismatic} - Assert: Forward({0.5}).p ≈ (0, 0, 0.7) -link_twist_maps_z_to_minus_y: - Arrange: link {0, π/2, 0, 0, Revolute}, both conventions - Assert: LinkTransform(0, 0).R · ẑ ≈ (0, −1, 0) -modified_link_transform_matches_elementary_product: - Arrange: link {0.3, 0.7, 0.2, 0, Revolute}, q = 1.1 - Assert: LinkTransform ≈ Rotx(0.7)·Transx(0.3)·Rotz(1.1)·Transz(0.2) - ⇒ p ≈ (0.3, −0.128844, 0.152968) -standard_and_modified_give_the_same_pose: - Assert: standard.Forward(q) ≈ modified.Forward(q) for q ∈ {(0,0), (0,π/2), (0.4,−0.7), (1.3,2.1)} -standard_and_modified_give_the_same_frame_chain: - Assert: Frames(q).jointOrigins, jointAxes, tool agree; origins = {(0,0,0), (cos q₁, sin q₁, 0)}, axes = ẑ -frame_chain_jacobian_matches_finite_difference: - Arrange: PUMA-560 standard table (below), q = (0.3,−0.4,0.5,0.6,−0.7,0.8), q̇ = (0.3,−0.2,0.5,0.1,−0.4,0.7) - Assert: GeometricJacobian::Compute(Frames(q))·q̇ ≈ (Δp; Log(ΔR)) / ε (tol 1e-3, ε = 1e-3) -``` - -## Reference vectors - -- 2-link unit arm: `q = (0, π/2)` ⇒ tip `(1, 1, 0)`; `q = (0.4, −0.7)` ⇒ `(1.876397, 0.093898, 0)`; - `q = (1.3, 2.1)` ⇒ `(−0.699299, 0.708017, 0)` — identical for both conventions. -- `θ₁` offset `π/2`: `q = (0,0)` ⇒ `(0, 2, 0)`; `q = (0, π/2)` ⇒ `(−1, 1, 0)`. -- `α = π/2` ⇒ `R·ẑ = −ŷ` (both conventions). -- PUMA-560 standard `(a, α, d)`: `(0, π/2, 0)`, `(0.4318, 0, 0)`, `(0.0203, −π/2, 0.15005)`, - `(0, π/2, 0.4318)`, `(0, −π/2, 0)`, `(0, 0, 0)`. At the test `q`: tool `p ≈ (0.402407, −0.032586, - 0.263519)`, `J·q̇ ≈ (−0.146069, 0.072514, −0.086416; −0.005657, 0.296324, 0.946824)`; forward-difference - truncation error at `ε = 1e-3` is `≈ 1e-4`. - -## Edge cases - -- `α = ±π/2` ⇒ frame axis swaps; sign of `sα` checked by `link_twist_maps_z_to_minus_y`. -- Zero-length links (`a = d = 0`) ⇒ coincident origins. -- Mixed revolute/prismatic chain ⇒ `Frames` reports the per-joint type. -- `θ = 2π` ⇒ same pose as `θ = 0`. diff --git a/roadmap/kinematics/GeometricJacobian/explanation.md b/roadmap/kinematics/GeometricJacobian/explanation.md deleted file mode 100644 index fdbfc19..0000000 --- a/roadmap/kinematics/GeometricJacobian/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# Geometric Jacobian — Overview - -## What it is -The matrix that maps joint rates to the tool's velocity: for an `N`-joint arm it is `6×N`, the top -three rows giving the linear velocity of the tool point and the bottom three its angular velocity, -both expressed in the base frame. Its transpose maps a force and moment applied at the tool to joint -torques. The companion term `J̇q̇` is the tool acceleration produced by joint velocities alone. - -## Why it matters (embedded) -It is the workhorse of manipulator control: velocity control, force control, singularity detection, -pose inverse kinematics, impedance and operational-space control all need it, and task-space -controllers additionally need `J̇q̇`. Building it from one generic "frame chain" means a single, -well-tested routine serves every kinematic description in the library. - -## How it works (intuition) -Each joint contributes one column. A revolute joint spins everything outboard about its axis `z`, so -the tool gains angular velocity `z` and linear velocity `z × r`, where `r` runs from the joint to the -tool. A prismatic joint slides the tool along `z`. `J̇q̇` follows from differentiating those columns -while the axes and lever arms are carried along by the moving links. - -## Key parameters -- **Frame chain** — joint origins, axes and types plus the tool pose, in the base frame. -- **Ordering** — `(v; ω)`, linear first, as everywhere in the library. - -## Reference -B. Siciliano et al., *Robotics: Modelling, Planning and Control* (2009), Ch. 3 (geometric Jacobian); -K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 5 (for the space/body Jacobian variants). - -## See also -`ChainPoseKinematics` (M30), `DenavitHartenberg` (M7) and `ProductOfExponentials` (M15) produce the -frame chain; `ManipulabilityIndex` (M11) scores the Jacobian; `PoseInverseKinematics` (M13), -`RedundancyResolution` (M14) and the task-space controllers (M17–M19) use it. diff --git a/roadmap/kinematics/GeometricJacobian/implementation.md b/roadmap/kinematics/GeometricJacobian/implementation.md deleted file mode 100644 index 92869e5..0000000 --- a/roadmap/kinematics/GeometricJacobian/implementation.md +++ /dev/null @@ -1,106 +0,0 @@ -# Geometric Jacobian (6×N) and Jacobian Provider — Implementation Pseudocode - -> Roadmap ref: #M8 (Tier 2) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -The **geometric** Jacobian maps joint rates to the tool's linear velocity (of the tool point) and -angular velocity, both in the base frame, ordered `(v; ω)` (NT53 convention). It is *not* the -Lynch–Park "space Jacobian" (whose linear part is the velocity of the point at the base origin); -M15 provides that one. The routine is model-agnostic: it consumes a `FrameChain` (M30), which the -link model (M30), DH (M7) and PoE (M15) all produce, so FK, IK, dynamics and controllers share a -single Jacobian implementation — replacing the private 3×N Jacobian inside the shipped IK. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class GeometricJacobian: # stateless - using Jacobian = math::Matrix - -template # DI seam used by all task-space controllers -class JacobianProvider: - protected ~JacobianProvider() = default # non-virtual; never deleted through the interface - virtual math::Matrix Jacobian(const JointVector& q) const = 0 - virtual math::Vector BiasAcceleration(const JointVector& q, const JointVector& qDot) const = 0 # J̇·q̇ - virtual SE3Transform ToolPose(const JointVector& q) const = 0 - -template # TaskDim ∈ {3, 6} -class ChainTaskJacobian : public JacobianProvider: - const LinkArray& links # link model (shipped RevoluteJointLink chain) - ChainPoseKinematics kinematics # M30 -``` - -## Interface - -```text -static Jacobian Compute(const FrameChain& frames) # hot path -static Vector6 BiasAcceleration(const FrameChain& frames, const JointVector& qDot) # J̇·q̇ -static JointVector JointTorques(const Jacobian& J, const Vector6& wrench) # Jᵀ·w - -# ChainTaskJacobian: TaskDim = 6 → all rows; TaskDim = 3 → the linear rows only (position tasks) -``` - -## Algorithm (pseudocode) - -```cpp -function Compute(frames): # OPTIMIZE_FOR_SPEED - p = frames.tool.p - for i in 0..N-1: - z = frames.jointAxes[i]; o = frames.jointOrigins[i] - if frames.jointTypes[i] == Revolute: column i = ( CrossProduct(z, p − o) ; z ) - else: column i = ( z ; 0 ) - return J - -function BiasAcceleration(frames, q̇): # O(N) recursion, q̈ = 0 - ω = 0; vOrigin = 0; oPrev = frames.jointOrigins[0] - ṗ = Compute(frames).linearRows · q̇ # tool-point velocity (or recurse, see note) - acc = 0 (6-vector) - for i in 0..N-1: - o = frames.jointOrigins[i]; z = frames.jointAxes[i] - vOrigin = vOrigin + CrossProduct(ω, o − oPrev) # velocity of joint origin i - ż = CrossProduct(ω, z) # axis carried by link i−1 - if Revolute: - acc.linear += q̇[i] · ( CrossProduct(ż, p − o) + CrossProduct(z, ṗ − vOrigin) ) - acc.angular += q̇[i] · ż - ω = ω + z · q̇[i] - else: # Prismatic - acc.linear += q̇[i] · ż - vOrigin = vOrigin + z · q̇[i] - oPrev = o - return acc - -function JointTorques(J, w): # statics: τ = Jᵀ·w, w = (f; n) at the tool point - return Transpose(J) · w -``` - -## Complexity & memory - -- `Compute`: `O(N)` — one cross product per column. -- `BiasAcceleration`: `O(N)` after the `O(6N)` tool-velocity product. -- `JointTorques`: `O(6N)`. -- Memory: one `6×N` matrix; stack only. - -## Numerical / embedded notes - -- **Ordering is normative:** rows `0–2` are linear velocity of the tool point, rows `3–5` angular - velocity, both in the base frame; wrenches passed to `JointTorques` are `(f; n)` about the tool point. -- DH frames (M7) put joint `i`'s axis on `z` of frame `i−1` (standard) or frame `i` (modified); the DH - spec is responsible for filling `FrameChain` correctly — this routine never looks at DH parameters. -- `BiasAcceleration` (`J̇q̇`) is required by operational-space control (M18) and by pose tracking; - check it against a finite difference `(J(q + εq̇) − J(q))/ε · q̇`. -- Near a **singularity** columns become dependent; detect with M11 and never invert `J` directly. -- The provider interface is a virtual seam for testability (StrictMock). Hard real-time users can - template the controllers on the concrete `ChainTaskJacobian` to avoid the virtual call. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/GeometricJacobian.hpp` (+ `JacobianProvider.hpp`, `ChainTaskJacobian.hpp`) — - `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `Compute`/`BiasAcceleration`, and `extern template class GeometricJacobian;` / `` / - `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/GeometricJacobian.cpp` → the same instantiations. -- Test: `robotics/kinematics/test/TestGeometricJacobian.cpp` -- Doc: `doc/kinematics/GeometricJacobian.md` (per `doc/TEMPLATE.md`) -- Depends on: NT53 (`math::SE3Transform`), M30 (`FrameChain`, `ChainPoseKinematics`). -- After deployment, the shipped `InverseKinematics` should build its 3×N Jacobian from this routine. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/GeometricJacobian/tests.md b/roadmap/kinematics/GeometricJacobian/tests.md deleted file mode 100644 index 222be93..0000000 --- a/roadmap/kinematics/GeometricJacobian/tests.md +++ /dev/null @@ -1,49 +0,0 @@ -# Geometric Jacobian — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestGeometricJacobian : public ::testing::Test: - # 2-link unit planar arm, z-axis joints, joint 2 at (1,0,0), tool at (1,0,0) of link 2 - std::array, 2> planar{ ... } - ChainPoseKinematics kinematics{ SE3Transform{ I, (1, 0, 0) } } -# each case below is a TEST_F(TestGeometricJacobian, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -stretched_planar_arm_columns: - Act: J = GeometricJacobian::Compute(kinematics.Compute(planar, (0, 0))) - Assert: linear rows [[0,0],[2,1],[0,0]], angular rows [[0,0],[0,0],[1,1]] -linear_rows_match_finite_difference_of_tool_position: - Arrange: q = (0.4, −0.7), q̇ = (0.3, −0.5) - Assert: J_linear·q̇ ≈ (p(q + εq̇) − p(q)) / ε -angular_rows_match_finite_difference_of_tool_rotation: - Assert: J_angular·q̇ ≈ Log(R(q + εq̇)·R(q)ᵀ).angular / ε (spatial 3-link arm, mixed axes) -bias_acceleration_matches_finite_difference: - Assert: BiasAcceleration(frames, q̇) ≈ (J(q + εq̇) − J(q)) / ε · q̇ -prismatic_column_is_pure_translation: - Arrange: FrameChain with a prismatic joint along ŷ - Assert: its column = (ŷ; 0) -wrench_along_link_line_needs_no_torque: - Assert: JointTorques(J(0,0), (x̂; 0)) ≈ (0, 0) -link_model_and_dh_frames_give_the_same_jacobian: - Arrange: same 2-link arm described by DH (M7) and by links (M30) - Assert: Compute(dhFrames) ≈ Compute(linkFrames) -task_jacobian_position_rows_match_six_dof_rows: - Assert: ChainTaskJacobian::Jacobian(q) == first three rows of the 6-row version -``` - -## Reference vectors - -- 2-link unit arm at `q = (0,0)`: linear `[[0,0],[2,1],[0,0]]`, angular `[[0,0],[0,0],[1,1]]`. -- Tool force `x̂` on the stretched arm acts along the links ⇒ joint torques `(0, 0)`. - -## Edge cases - -- Stretched arm ⇒ columns linearly dependent (singular). -- `q̇ = 0` ⇒ `BiasAcceleration = 0`. -- Single joint ⇒ `6×1`. diff --git a/robotics/kinematics/CMakeLists.txt b/robotics/kinematics/CMakeLists.txt index ad8413e..fb8017b 100644 --- a/robotics/kinematics/CMakeLists.txt +++ b/robotics/kinematics/CMakeLists.txt @@ -13,13 +13,20 @@ target_link_libraries(robotics.kinematics ${ROBOTICS_VISIBILITY} target_sources(robotics.kinematics PRIVATE ChainPoseKinematics.hpp + ChainTaskJacobian.hpp + DenavitHartenberg.hpp + DhTaskJacobian.hpp ForwardKinematics.hpp + GeometricJacobian.hpp InverseKinematics.hpp + JacobianProvider.hpp ) robotics_add_coverage_sources(robotics.kinematics ChainPoseKinematics.cpp + DenavitHartenberg.cpp ForwardKinematics.cpp + GeometricJacobian.cpp InverseKinematics.cpp ) diff --git a/robotics/kinematics/ChainTaskJacobian.hpp b/robotics/kinematics/ChainTaskJacobian.hpp new file mode 100644 index 0000000..ea3fe77 --- /dev/null +++ b/robotics/kinematics/ChainTaskJacobian.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include "robotics/kinematics/ChainPoseKinematics.hpp" +#include "robotics/kinematics/GeometricJacobian.hpp" +#include "robotics/kinematics/JacobianProvider.hpp" +#include +#include + +namespace kinematics +{ + template + class ChainTaskJacobian + : public JacobianProvider + { + public: + using typename JacobianProvider::JointVector; + using typename JacobianProvider::TaskJacobian; + using typename JacobianProvider::TaskVector; + using LinkArray = std::array, Dof>; + + ChainTaskJacobian(const LinkArray& links, const math::SE3Transform& toolInLastLink); + + TaskJacobian Jacobian(const JointVector& q) const override; + TaskVector BiasAcceleration(const JointVector& q, const JointVector& qDot) const override; + math::SE3Transform ToolPose(const JointVector& q) const override; + + private: + using Geometric = GeometricJacobian; + + const LinkArray& links; + ChainPoseKinematics kinematics; + }; + + template + ChainTaskJacobian::ChainTaskJacobian(const LinkArray& links, const math::SE3Transform& toolInLastLink) + : links{ links } + , kinematics{ toolInLastLink } + {} + + template + typename ChainTaskJacobian::TaskJacobian ChainTaskJacobian::Jacobian(const JointVector& q) const + { + return Geometric::template TaskRows(Geometric::Compute(kinematics.Compute(links, q))); + } + + template + typename ChainTaskJacobian::TaskVector ChainTaskJacobian::BiasAcceleration(const JointVector& q, const JointVector& qDot) const + { + return Geometric::template TaskTwist(Geometric::BiasAcceleration(kinematics.Compute(links, q), qDot)); + } + + template + math::SE3Transform ChainTaskJacobian::ToolPose(const JointVector& q) const + { + return kinematics.ToolPose(links, q); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ChainTaskJacobian; + extern template class ChainTaskJacobian; +#endif +} diff --git a/robotics/kinematics/DenavitHartenberg.cpp b/robotics/kinematics/DenavitHartenberg.cpp new file mode 100644 index 0000000..19cc751 --- /dev/null +++ b/robotics/kinematics/DenavitHartenberg.cpp @@ -0,0 +1,10 @@ +#include "robotics/kinematics/DenavitHartenberg.hpp" +#include "robotics/kinematics/DhTaskJacobian.hpp" + +namespace kinematics +{ + template class DenavitHartenberg; + template class DenavitHartenberg; + template class DenavitHartenberg; + template class DhTaskJacobian; +} diff --git a/robotics/kinematics/DenavitHartenberg.hpp b/robotics/kinematics/DenavitHartenberg.hpp new file mode 100644 index 0000000..c45ca7e --- /dev/null +++ b/robotics/kinematics/DenavitHartenberg.hpp @@ -0,0 +1,150 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include "robotics/kinematics/ChainPoseKinematics.hpp" +#include +#include +#include +#include + +namespace kinematics +{ + enum class DhConvention : std::uint8_t + { + Standard, + Modified + }; + + template + struct DhLink + { + static_assert(std::is_floating_point_v, "DhLink supports floating-point types"); + + T a{}; + T alpha{}; + T d{}; + T theta{}; + dynamics::JointType type{ dynamics::JointType::Revolute }; + }; + + template + class DenavitHartenberg + { + static_assert(std::is_floating_point_v, "DenavitHartenberg supports floating-point types"); + static_assert(N > 0, "Number of links must be positive"); + + public: + using JointVector = math::Vector; + using Transform = math::SE3Transform; + using LinkTable = std::array, N>; + + DenavitHartenberg(const LinkTable& links, DhConvention convention, const Transform& tool = Transform::Identity()); + + OPTIMIZE_FOR_SPEED Transform LinkTransform(std::size_t i, T q) const; + OPTIMIZE_FOR_SPEED Transform Forward(const JointVector& q) const; + FrameChain Frames(const JointVector& q) const; + + private: + Transform StandardTransform(std::size_t i, T cosTheta, T sinTheta, T d) const; + Transform ModifiedTransform(std::size_t i, T cosTheta, T sinTheta, T d) const; + + LinkTable links; + DhConvention convention; + Transform tool; + std::array cosAlpha{}; + std::array sinAlpha{}; + }; + + template + DenavitHartenberg::DenavitHartenberg(const LinkTable& links, DhConvention convention, const Transform& tool) + : links{ links } + , convention{ convention } + , tool{ tool } + { + for (std::size_t i = 0; i < N; ++i) + { + cosAlpha[i] = math::Cos(links[i].alpha); + sinAlpha[i] = math::Sin(links[i].alpha); + } + } + + template + OPTIMIZE_FOR_SPEED typename DenavitHartenberg::Transform DenavitHartenberg::LinkTransform(std::size_t i, T q) const + { + const auto& link{ links[i] }; + const bool revolute{ link.type == dynamics::JointType::Revolute }; + const T theta{ link.theta + (revolute ? q : T{}) }; + const T d{ link.d + (revolute ? T{} : q) }; + + if (convention == DhConvention::Standard) + return StandardTransform(i, math::Cos(theta), math::Sin(theta), d); + + return ModifiedTransform(i, math::Cos(theta), math::Sin(theta), d); + } + + template + OPTIMIZE_FOR_SPEED typename DenavitHartenberg::Transform DenavitHartenberg::Forward(const JointVector& q) const + { + Transform frame{}; + for (std::size_t i = 0; i < N; ++i) + frame = frame * LinkTransform(i, q[i]); + + return frame * tool; + } + + template + FrameChain DenavitHartenberg::Frames(const JointVector& q) const + { + const math::Vector3 zAxis{ T{}, T{}, T{ 1 } }; + const bool standard{ convention == DhConvention::Standard }; + + FrameChain frames{}; + Transform frame{}; + for (std::size_t i = 0; i < N; ++i) + { + if (!standard) + frame = frame * LinkTransform(i, q[i]); + + frames.jointOrigins[i] = frame.translation; + frames.jointAxes[i] = frame.rotation * zAxis; + frames.jointTypes[i] = links[i].type; + + if (standard) + frame = frame * LinkTransform(i, q[i]); + } + + frames.tool = frame * tool; + return frames; + } + + template + typename DenavitHartenberg::Transform DenavitHartenberg::StandardTransform(std::size_t i, T cosTheta, T sinTheta, T d) const + { + const T ca{ cosAlpha[i] }; + const T sa{ sinAlpha[i] }; + const T a{ links[i].a }; + + return { math::Matrix3{ { cosTheta, -sinTheta * ca, sinTheta * sa }, { sinTheta, cosTheta * ca, -cosTheta * sa }, { T{}, sa, ca } }, + math::Vector3{ a * cosTheta, a * sinTheta, d } }; + } + + template + typename DenavitHartenberg::Transform DenavitHartenberg::ModifiedTransform(std::size_t i, T cosTheta, T sinTheta, T d) const + { + const T ca{ cosAlpha[i] }; + const T sa{ sinAlpha[i] }; + + return { math::Matrix3{ { cosTheta, -sinTheta, T{} }, { sinTheta * ca, cosTheta * ca, -sa }, { sinTheta * sa, cosTheta * sa, ca } }, + math::Vector3{ links[i].a, -sa * d, ca * d } }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class DenavitHartenberg; + extern template class DenavitHartenberg; + extern template class DenavitHartenberg; +#endif +} diff --git a/robotics/kinematics/DhTaskJacobian.hpp b/robotics/kinematics/DhTaskJacobian.hpp new file mode 100644 index 0000000..2ce5841 --- /dev/null +++ b/robotics/kinematics/DhTaskJacobian.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include "numerical/math/SE3Transform.hpp" +#include "robotics/kinematics/DenavitHartenberg.hpp" +#include "robotics/kinematics/GeometricJacobian.hpp" +#include "robotics/kinematics/JacobianProvider.hpp" +#include + +namespace kinematics +{ + template + class DhTaskJacobian + : public JacobianProvider + { + public: + using typename JacobianProvider::JointVector; + using typename JacobianProvider::TaskJacobian; + using typename JacobianProvider::TaskVector; + + explicit DhTaskJacobian(const DenavitHartenberg& model); + + TaskJacobian Jacobian(const JointVector& q) const override; + TaskVector BiasAcceleration(const JointVector& q, const JointVector& qDot) const override; + math::SE3Transform ToolPose(const JointVector& q) const override; + + private: + using Geometric = GeometricJacobian; + + const DenavitHartenberg& model; + }; + + template + DhTaskJacobian::DhTaskJacobian(const DenavitHartenberg& model) + : model{ model } + {} + + template + typename DhTaskJacobian::TaskJacobian DhTaskJacobian::Jacobian(const JointVector& q) const + { + return Geometric::template TaskRows(Geometric::Compute(model.Frames(q))); + } + + template + typename DhTaskJacobian::TaskVector DhTaskJacobian::BiasAcceleration(const JointVector& q, const JointVector& qDot) const + { + return Geometric::template TaskTwist(Geometric::BiasAcceleration(model.Frames(q), qDot)); + } + + template + math::SE3Transform DhTaskJacobian::ToolPose(const JointVector& q) const + { + return model.Forward(q); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class DhTaskJacobian; +#endif +} diff --git a/robotics/kinematics/GeometricJacobian.cpp b/robotics/kinematics/GeometricJacobian.cpp new file mode 100644 index 0000000..b61da53 --- /dev/null +++ b/robotics/kinematics/GeometricJacobian.cpp @@ -0,0 +1,12 @@ +#include "robotics/kinematics/GeometricJacobian.hpp" +#include "robotics/kinematics/ChainTaskJacobian.hpp" + +namespace kinematics +{ + template class GeometricJacobian; + template class GeometricJacobian; + template class GeometricJacobian; + template class GeometricJacobian; + template class ChainTaskJacobian; + template class ChainTaskJacobian; +} diff --git a/robotics/kinematics/GeometricJacobian.hpp b/robotics/kinematics/GeometricJacobian.hpp new file mode 100644 index 0000000..07fdb6a --- /dev/null +++ b/robotics/kinematics/GeometricJacobian.hpp @@ -0,0 +1,135 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include "robotics/kinematics/ChainPoseKinematics.hpp" +#include +#include + +namespace kinematics +{ + template + class GeometricJacobian + { + static_assert(std::is_floating_point_v, "GeometricJacobian supports floating-point types"); + static_assert(N > 0, "Number of joints must be positive"); + + public: + using Jacobian = math::Matrix; + using JointVector = math::Vector; + using Twist = math::Vector6; + using Vector3 = math::Vector3; + + OPTIMIZE_FOR_SPEED static Jacobian Compute(const FrameChain& frames); + OPTIMIZE_FOR_SPEED static Twist BiasAcceleration(const FrameChain& frames, const JointVector& qDot); + static JointVector JointTorques(const Jacobian& jacobian, const Twist& wrench); + + template + static math::Matrix TaskRows(const Jacobian& jacobian); + template + static math::Vector TaskTwist(const Twist& twist); + + private: + static bool IsRevolute(const FrameChain& frames, std::size_t joint); + static void SetColumn(Jacobian& jacobian, std::size_t column, const Vector3& linear, const Vector3& angular); + }; + + template + OPTIMIZE_FOR_SPEED typename GeometricJacobian::Jacobian GeometricJacobian::Compute(const FrameChain& frames) + { + const Vector3& tool{ frames.tool.translation }; + + Jacobian jacobian{}; + for (std::size_t i = 0; i < N; ++i) + { + const Vector3& axis{ frames.jointAxes[i] }; + if (IsRevolute(frames, i)) + SetColumn(jacobian, i, math::CrossProduct(axis, Vector3{ tool - frames.jointOrigins[i] }), axis); + else + SetColumn(jacobian, i, axis, Vector3{}); + } + + return jacobian; + } + + template + OPTIMIZE_FOR_SPEED typename GeometricJacobian::Twist GeometricJacobian::BiasAcceleration(const FrameChain& frames, const JointVector& qDot) + { + const Vector3& tool{ frames.tool.translation }; + const Vector3 toolVelocity{ TaskTwist<3>(Twist{ Compute(frames) * qDot }) }; + + Vector3 omega{}; + Vector3 originVelocity{}; + Vector3 linear{}; + Vector3 angular{}; + for (std::size_t i = 0; i < N; ++i) + { + const Vector3& origin{ frames.jointOrigins[i] }; + const Vector3& axis{ frames.jointAxes[i] }; + originVelocity = originVelocity + math::CrossProduct(omega, Vector3{ origin - frames.jointOrigins[i == 0 ? 0 : i - 1] }); + const Vector3 axisRate{ math::CrossProduct(omega, axis) }; + + if (IsRevolute(frames, i)) + { + linear = linear + (math::CrossProduct(axisRate, Vector3{ tool - origin }) + math::CrossProduct(axis, Vector3{ toolVelocity - originVelocity })) * qDot[i]; + angular = angular + axisRate * qDot[i]; + omega = omega + axis * qDot[i]; + } + else + { + linear = linear + axisRate * qDot[i]; + originVelocity = originVelocity + axis * qDot[i]; + } + } + + Twist bias{}; + bias.SetBlock(linear, 0, 0); + bias.SetBlock(angular, 3, 0); + return bias; + } + + template + typename GeometricJacobian::JointVector GeometricJacobian::JointTorques(const Jacobian& jacobian, const Twist& wrench) + { + return jacobian.Transpose() * wrench; + } + + template + template + math::Matrix GeometricJacobian::TaskRows(const Jacobian& jacobian) + { + static_assert(TaskDim == 3 || TaskDim == 6, "Task dimension must be 3 (position) or 6 (pose)"); + return jacobian.template GetBlock(0, 0); + } + + template + template + math::Vector GeometricJacobian::TaskTwist(const Twist& twist) + { + static_assert(TaskDim == 3 || TaskDim == 6, "Task dimension must be 3 (position) or 6 (pose)"); + return twist.template GetBlock(0, 0); + } + + template + bool GeometricJacobian::IsRevolute(const FrameChain& frames, std::size_t joint) + { + return frames.jointTypes[joint] == dynamics::JointType::Revolute; + } + + template + void GeometricJacobian::SetColumn(Jacobian& jacobian, std::size_t column, const Vector3& linear, const Vector3& angular) + { + jacobian.SetBlock(linear, 0, column); + jacobian.SetBlock(angular, 3, column); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class GeometricJacobian; + extern template class GeometricJacobian; + extern template class GeometricJacobian; + extern template class GeometricJacobian; +#endif +} diff --git a/robotics/kinematics/InverseKinematics.hpp b/robotics/kinematics/InverseKinematics.hpp index ef689f8..f0ab116 100644 --- a/robotics/kinematics/InverseKinematics.hpp +++ b/robotics/kinematics/InverseKinematics.hpp @@ -6,7 +6,8 @@ #include "numerical/math/Matrix.hpp" #include "numerical/solvers/GaussianElimination.hpp" #include "robotics/dynamics/JointLink.hpp" -#include "robotics/kinematics/ForwardKinematics.hpp" +#include "robotics/kinematics/ChainPoseKinematics.hpp" +#include "robotics/kinematics/GeometricJacobian.hpp" #include #include @@ -45,7 +46,6 @@ namespace kinematics using JointVector = math::Vector; using Jacobian = math::Matrix; using LinkArray = std::array, NumLinks>; - using PositionArray = std::array; explicit InverseKinematics(const Vector3& toolOffset, InverseKinematicsConfig cfg = {}); @@ -55,23 +55,18 @@ namespace kinematics const JointVector& initialQ) const; private: - Jacobian ComputeJacobian( - const LinkArray& links, - const JointVector& q, - const PositionArray& positions) const; - JointVector DampedLeastSquaresStep(const Jacobian& jacobian, const Vector3& error) const; T PositionError(const LinkArray& links, const JointVector& q, const Vector3& target) const; InverseKinematicsConfig config; - ForwardKinematics fk; + ChainPoseKinematics kinematics; }; template InverseKinematics::InverseKinematics(const Vector3& toolOffset, InverseKinematicsConfig cfg) : config{ cfg } - , fk{ toolOffset } + , kinematics{ math::SE3Transform{ Matrix3::Identity(), toolOffset } } { really_assert(config.dampingFactor > T(0)); really_assert(config.tolerance > T(0)); @@ -90,45 +85,20 @@ namespace kinematics for (std::size_t iteration = 0; iteration < config.maxIterations; ++iteration) { - const auto positions = fk.Compute(links, q); - const Vector3 error{ target - positions[NumLinks] }; + const auto frames{ kinematics.Compute(links, q) }; + const Vector3 error{ target - frames.tool.translation }; const T errorNorm{ math::VectorNorm(error) }; if (errorNorm < config.tolerance) return { q, errorNorm, iteration, true }; - q = q + DampedLeastSquaresStep(ComputeJacobian(links, q, positions), error); + const auto jacobian{ GeometricJacobian::template TaskRows<3>(GeometricJacobian::Compute(frames)) }; + q = q + DampedLeastSquaresStep(jacobian, error); } return { q, PositionError(links, q, target), config.maxIterations, false }; } - template - typename InverseKinematics::Jacobian - InverseKinematics::ComputeJacobian( - const LinkArray& links, - const JointVector& q, - const PositionArray& positions) const - { - const Vector3& toolPosition = positions[NumLinks]; - - Matrix3 R{ Matrix3::Identity() }; - Jacobian J{}; - - for (std::size_t i = 0; i < NumLinks; ++i) - { - const Vector3 column{ math::CrossProduct(R * links[i].AngularAxis(), Vector3{ toolPosition - positions[i] }) + R * links[i].LinearAxis() }; - - J.at(0, i) = column.at(0, 0); - J.at(1, i) = column.at(1, 0); - J.at(2, i) = column.at(2, 0); - - R = R * links[i].JointTransform(q.at(i, 0)).rotation; - } - - return J; - } - template typename InverseKinematics::JointVector InverseKinematics::DampedLeastSquaresStep(const Jacobian& jacobian, const Vector3& error) const @@ -145,8 +115,7 @@ namespace kinematics template T InverseKinematics::PositionError(const LinkArray& links, const JointVector& q, const Vector3& target) const { - const auto positions = fk.Compute(links, q); - return math::VectorNorm(Vector3{ target - positions[NumLinks] }); + return math::VectorNorm(Vector3{ target - kinematics.ToolPose(links, q).translation }); } #ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD diff --git a/robotics/kinematics/JacobianProvider.hpp b/robotics/kinematics/JacobianProvider.hpp new file mode 100644 index 0000000..91489dd --- /dev/null +++ b/robotics/kinematics/JacobianProvider.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include +#include + +namespace kinematics +{ + template + class JacobianProvider + { + static_assert(std::is_floating_point_v, "JacobianProvider supports floating-point types"); + + public: + using JointVector = math::Vector; + using TaskJacobian = math::Matrix; + using TaskVector = math::Vector; + + virtual TaskJacobian Jacobian(const JointVector& q) const = 0; + virtual TaskVector BiasAcceleration(const JointVector& q, const JointVector& qDot) const = 0; + virtual math::SE3Transform ToolPose(const JointVector& q) const = 0; + + protected: + ~JacobianProvider() = default; + }; +} diff --git a/robotics/kinematics/test/CMakeLists.txt b/robotics/kinematics/test/CMakeLists.txt index 4268fbe..bd734e4 100644 --- a/robotics/kinematics/test/CMakeLists.txt +++ b/robotics/kinematics/test/CMakeLists.txt @@ -9,6 +9,8 @@ target_link_libraries(robotics.kinematics_test PUBLIC target_sources(robotics.kinematics_test PRIVATE TestChainPoseKinematics.cpp + TestDenavitHartenberg.cpp TestForwardKinematics.cpp + TestGeometricJacobian.cpp TestInverseKinematics.cpp ) diff --git a/robotics/kinematics/test/TestDenavitHartenberg.cpp b/robotics/kinematics/test/TestDenavitHartenberg.cpp new file mode 100644 index 0000000..ae4ff92 --- /dev/null +++ b/robotics/kinematics/test/TestDenavitHartenberg.cpp @@ -0,0 +1,151 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "numerical/math/Tolerance.hpp" +#include "robotics/kinematics/DenavitHartenberg.hpp" +#include "robotics/kinematics/DhTaskJacobian.hpp" +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Transform = math::SE3Transform; + using DhLink = kinematics::DhLink; + using kinematics::DhConvention; + + constexpr float pi{ std::numbers::pi_v }; + constexpr dynamics::JointType revolute{ dynamics::JointType::Revolute }; + constexpr dynamics::JointType prismatic{ dynamics::JointType::Prismatic }; + const Vector3 xAxis{ 1.0f, 0.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + class TestDenavitHartenberg + : public ::testing::Test + { + protected: + void ExpectNear(const Vector3& actual, const Vector3& expected, float tolerance = math::Tolerance()) const + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + 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()); + } + + kinematics::DenavitHartenberg standard{ { DhLink{ 1.0f, 0.0f, 0.0f, 0.0f, revolute }, DhLink{ 1.0f, 0.0f, 0.0f, 0.0f, revolute } }, DhConvention::Standard }; + kinematics::DenavitHartenberg modified{ { DhLink{ 0.0f, 0.0f, 0.0f, 0.0f, revolute }, DhLink{ 1.0f, 0.0f, 0.0f, 0.0f, revolute } }, DhConvention::Modified, + Transform{ Matrix3::Identity(), xAxis } }; + }; +} + +TEST_F(TestDenavitHartenberg, zero_config_is_stretched_along_x) +{ + const auto pose{ standard.Forward(math::Vector{}) }; + + ExpectNear(pose.translation, Vector3{ 2.0f, 0.0f, 0.0f }); + ExpectNear(pose.rotation, Matrix3::Identity()); +} + +TEST_F(TestDenavitHartenberg, planar_elbow_ninety_deg) +{ + ExpectNear(standard.Forward(math::Vector{ 0.0f, pi / 2.0f }).translation, Vector3{ 1.0f, 1.0f, 0.0f }); +} + +TEST_F(TestDenavitHartenberg, single_link_revolute_rotates) +{ + const kinematics::DenavitHartenberg single{ { DhLink{ 1.0f, 0.0f, 0.0f, 0.0f, revolute } }, DhConvention::Standard }; + + ExpectNear(single.Forward(math::Vector{ pi / 2.0f }).translation, Vector3{ 0.0f, 1.0f, 0.0f }); +} + +TEST_F(TestDenavitHartenberg, theta_offset_is_added_to_the_joint_variable) +{ + const kinematics::DenavitHartenberg offset{ { DhLink{ 1.0f, 0.0f, 0.0f, pi / 2.0f, revolute }, DhLink{ 1.0f, 0.0f, 0.0f, 0.0f, revolute } }, DhConvention::Standard }; + + ExpectNear(offset.Forward(math::Vector{}).translation, Vector3{ 0.0f, 2.0f, 0.0f }); + ExpectNear(offset.Forward(math::Vector{ 0.0f, pi / 2.0f }).translation, Vector3{ -1.0f, 1.0f, 0.0f }); +} + +TEST_F(TestDenavitHartenberg, prismatic_variable_adds_to_d) +{ + const kinematics::DenavitHartenberg slider{ { DhLink{ 0.0f, 0.0f, 0.2f, 0.0f, prismatic } }, DhConvention::Standard }; + + ExpectNear(slider.Forward(math::Vector{ 0.5f }).translation, Vector3{ 0.0f, 0.0f, 0.7f }); +} + +TEST_F(TestDenavitHartenberg, link_twist_maps_z_to_minus_y) +{ + const DhLink twisted{ 0.0f, pi / 2.0f, 0.0f, 0.0f, revolute }; + const kinematics::DenavitHartenberg standardTwist{ { twisted }, DhConvention::Standard }; + const kinematics::DenavitHartenberg modifiedTwist{ { twisted }, DhConvention::Modified }; + + ExpectNear(standardTwist.LinkTransform(0, 0.0f).rotation * zAxis, Vector3{ 0.0f, -1.0f, 0.0f }); + ExpectNear(modifiedTwist.LinkTransform(0, 0.0f).rotation * zAxis, Vector3{ 0.0f, -1.0f, 0.0f }); +} + +TEST_F(TestDenavitHartenberg, modified_link_transform_matches_elementary_product) +{ + const kinematics::DenavitHartenberg link{ { DhLink{ 0.3f, 0.7f, 0.2f, 0.0f, revolute } }, DhConvention::Modified }; + + const auto transform{ link.LinkTransform(0, 1.1f) }; + + ExpectNear(transform.translation, Vector3{ 0.3f, -0.128844f, 0.152968f }); + ExpectNear(transform.rotation, Matrix3{ math::RotationAboutAxis(xAxis, 0.7f) * math::RotationAboutAxis(zAxis, 1.1f) }); +} + +TEST_F(TestDenavitHartenberg, standard_and_modified_give_the_same_pose) +{ + for (const auto& q : { math::Vector{}, math::Vector{ 0.0f, pi / 2.0f }, math::Vector{ 0.4f, -0.7f }, math::Vector{ 1.3f, 2.1f } }) + { + const auto fromStandard{ standard.Forward(q) }; + const auto fromModified{ modified.Forward(q) }; + ExpectNear(fromStandard.translation, fromModified.translation); + ExpectNear(fromStandard.rotation, fromModified.rotation); + } + ExpectNear(standard.Forward(math::Vector{ 0.4f, -0.7f }).translation, Vector3{ 1.876397f, 0.093898f, 0.0f }); + ExpectNear(standard.Forward(math::Vector{ 1.3f, 2.1f }).translation, Vector3{ -0.699299f, 0.708017f, 0.0f }); +} + +TEST_F(TestDenavitHartenberg, standard_and_modified_give_the_same_frame_chain) +{ + const math::Vector q{ 0.4f, -0.7f }; + + const auto fromStandard{ standard.Frames(q) }; + const auto fromModified{ modified.Frames(q) }; + + for (std::size_t i = 0; i < 2; ++i) + { + ExpectNear(fromStandard.jointOrigins[i], fromModified.jointOrigins[i]); + ExpectNear(fromStandard.jointAxes[i], zAxis); + ExpectNear(fromModified.jointAxes[i], zAxis); + } + ExpectNear(fromStandard.jointOrigins[0], Vector3{}); + ExpectNear(fromStandard.jointOrigins[1], Vector3{ std::cos(q[0]), std::sin(q[0]), 0.0f }); + ExpectNear(fromStandard.tool.translation, fromModified.tool.translation); +} + +TEST_F(TestDenavitHartenberg, frame_chain_jacobian_matches_finite_difference) +{ + const kinematics::DenavitHartenberg puma{ { DhLink{ 0.0f, pi / 2.0f, 0.0f, 0.0f, revolute }, DhLink{ 0.4318f, 0.0f, 0.0f, 0.0f, revolute }, + DhLink{ 0.0203f, -pi / 2.0f, 0.15005f, 0.0f, revolute }, DhLink{ 0.0f, pi / 2.0f, 0.4318f, 0.0f, revolute }, + DhLink{ 0.0f, -pi / 2.0f, 0.0f, 0.0f, revolute }, DhLink{ 0.0f, 0.0f, 0.0f, 0.0f, revolute } }, + DhConvention::Standard }; + const kinematics::DhTaskJacobian provider{ puma }; + const math::Vector q{ 0.3f, -0.4f, 0.5f, 0.6f, -0.7f, 0.8f }; + const math::Vector qDot{ 0.3f, -0.2f, 0.5f, 0.1f, -0.4f, 0.7f }; + constexpr float epsilon{ 1e-3f }; + + const auto twist{ math::Vector6{ provider.Jacobian(q) * qDot } }; + + const auto now{ provider.ToolPose(q) }; + const auto ahead{ puma.Forward(q + qDot * epsilon) }; + ExpectNear(now.translation, Vector3{ 0.402407f, -0.032586f, 0.263519f }); + ExpectNear(twist.GetBlock<3, 1>(0, 0), Vector3{ (ahead.translation - now.translation) * (1.0f / epsilon) }); + ExpectNear(twist.GetBlock<3, 1>(3, 0), Vector3{ Transform::LogRotation(Matrix3{ ahead.rotation * now.rotation.Transpose() }) * (1.0f / epsilon) }); +} diff --git a/robotics/kinematics/test/TestGeometricJacobian.cpp b/robotics/kinematics/test/TestGeometricJacobian.cpp new file mode 100644 index 0000000..b2885c7 --- /dev/null +++ b/robotics/kinematics/test/TestGeometricJacobian.cpp @@ -0,0 +1,164 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "numerical/math/Tolerance.hpp" +#include "robotics/kinematics/ChainPoseKinematics.hpp" +#include "robotics/kinematics/ChainTaskJacobian.hpp" +#include "robotics/kinematics/DenavitHartenberg.hpp" +#include "robotics/kinematics/GeometricJacobian.hpp" +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Link = dynamics::RevoluteJointLink; + using Transform = math::SE3Transform; + using Planar = kinematics::GeometricJacobian; + using Spatial = kinematics::GeometricJacobian; + + constexpr float epsilon{ 1e-3f }; + const Vector3 xAxis{ 1.0f, 0.0f, 0.0f }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeLink(const Vector3& axis, const Vector3& parentToJoint, dynamics::JointType type = dynamics::JointType::Revolute) + { + return Link{ 1.0f, Matrix3::Identity(), axis, parentToJoint, Vector3{}, type }; + } + + 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 }; + } + + class TestGeometricJacobian + : public ::testing::Test + { + protected: + template + math::Vector Step(const math::Vector& q, const math::Vector& qDot, float scale) const + { + return q + qDot * scale; + } + + 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()); + } + + std::array planar{ MakeLink(zAxis, Vector3{}), MakeLink(zAxis, Vector3{ 1.0f, 0.0f, 0.0f }) }; + kinematics::ChainPoseKinematics kinematics{ Transform{ Matrix3::Identity(), Vector3{ 1.0f, 0.0f, 0.0f } } }; + std::array spatial{ MakeLink(zAxis, Vector3{ 0.0f, 0.0f, 0.2f }), MakeLink(Normalized(1.0f, 0.3f, 0.2f), Vector3{ 0.0f, 0.1f, 0.4f }, dynamics::JointType::Prismatic), + MakeLink(Normalized(0.2f, 1.0f, 0.4f), Vector3{ 0.3f, 0.0f, 0.1f }) }; + kinematics::ChainPoseKinematics spatialKinematics{ Transform{ math::RotationAboutAxis(xAxis, 0.4f), Vector3{ 0.25f, 0.05f, -0.1f } } }; + math::Vector spatialQ{ 0.5f, 0.15f, -0.8f }; + math::Vector spatialQDot{ 0.7f, -0.4f, 1.1f }; + }; +} + +TEST_F(TestGeometricJacobian, stretched_planar_arm_columns) +{ + const auto J{ Planar::Compute(kinematics.Compute(planar, math::Vector{})) }; + + const math::Matrix expected{ { 0.0f, 0.0f }, { 2.0f, 1.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 1.0f, 1.0f } }; + for (std::size_t r = 0; r < 6; ++r) + for (std::size_t c = 0; c < 2; ++c) + EXPECT_NEAR(J.at(r, c), expected.at(r, c), math::Tolerance()); +} + +TEST_F(TestGeometricJacobian, linear_rows_match_finite_difference_of_tool_position) +{ + const math::Vector q{ 0.4f, -0.7f }; + const math::Vector qDot{ 0.3f, -0.5f }; + + const auto velocity{ Planar::TaskTwist<3>(Planar::Twist{ Planar::Compute(kinematics.Compute(planar, q)) * qDot }) }; + + const Vector3 ahead{ kinematics.ToolPose(planar, Step(q, qDot, epsilon)).translation }; + const Vector3 behind{ kinematics.ToolPose(planar, Step(q, qDot, -epsilon)).translation }; + ExpectNear(velocity, Vector3{ (ahead - behind) * (0.5f / epsilon) }); +} + +TEST_F(TestGeometricJacobian, angular_rows_match_finite_difference_of_tool_rotation) +{ + const auto twist{ Spatial::Twist{ Spatial::Compute(spatialKinematics.Compute(spatial, spatialQ)) * spatialQDot } }; + + const Matrix3 ahead{ spatialKinematics.ToolPose(spatial, Step(spatialQ, spatialQDot, epsilon)).rotation }; + const Matrix3 behind{ spatialKinematics.ToolPose(spatial, Step(spatialQ, spatialQDot, -epsilon)).rotation }; + const Vector3 rotation{ Transform::LogRotation(Matrix3{ ahead * behind.Transpose() }) }; + ExpectNear(twist.GetBlock<3, 1>(3, 0), Vector3{ rotation * (0.5f / epsilon) }); +} + +TEST_F(TestGeometricJacobian, bias_acceleration_matches_finite_difference) +{ + const auto bias{ Spatial::BiasAcceleration(spatialKinematics.Compute(spatial, spatialQ), spatialQDot) }; + + const auto ahead{ Spatial::Compute(spatialKinematics.Compute(spatial, Step(spatialQ, spatialQDot, epsilon))) }; + const auto behind{ Spatial::Compute(spatialKinematics.Compute(spatial, Step(spatialQ, spatialQDot, -epsilon))) }; + const Spatial::Twist expected{ (ahead - behind) * spatialQDot * (0.5f / epsilon) }; + for (std::size_t i = 0; i < 6; ++i) + EXPECT_NEAR(bias[i], expected[i], math::Tolerance()); +} + +TEST_F(TestGeometricJacobian, prismatic_column_is_pure_translation) +{ + kinematics::FrameChain frames{}; + frames.jointOrigins[0] = Vector3{ 0.3f, -0.2f, 0.5f }; + frames.jointAxes[0] = yAxis; + frames.jointTypes[0] = dynamics::JointType::Prismatic; + frames.tool = Transform{ Matrix3::Identity(), Vector3{ 1.0f, 2.0f, 3.0f } }; + + const auto J{ kinematics::GeometricJacobian::Compute(frames) }; + + ExpectNear(J.GetBlock<3, 1>(0, 0), yAxis); + ExpectNear(J.GetBlock<3, 1>(3, 0), Vector3{}); +} + +TEST_F(TestGeometricJacobian, wrench_along_link_line_needs_no_torque) +{ + const auto J{ Planar::Compute(kinematics.Compute(planar, math::Vector{})) }; + Planar::Twist wrench{}; + wrench[0] = 1.0f; + + const auto tau{ Planar::JointTorques(J, wrench) }; + + EXPECT_NEAR(tau[0], 0.0f, math::Tolerance()); + EXPECT_NEAR(tau[1], 0.0f, math::Tolerance()); +} + +TEST_F(TestGeometricJacobian, link_model_and_dh_frames_give_the_same_jacobian) +{ + const kinematics::DenavitHartenberg dh{ { kinematics::DhLink{ 1.0f, 0.0f, 0.0f, 0.0f }, kinematics::DhLink{ 1.0f, 0.0f, 0.0f, 0.0f } }, kinematics::DhConvention::Standard }; + const math::Vector q{ 0.4f, -0.7f }; + + const auto fromDh{ Planar::Compute(dh.Frames(q)) }; + const auto fromLinks{ Planar::Compute(kinematics.Compute(planar, q)) }; + + for (std::size_t r = 0; r < 6; ++r) + for (std::size_t c = 0; c < 2; ++c) + EXPECT_NEAR(fromDh.at(r, c), fromLinks.at(r, c), math::Tolerance()); +} + +TEST_F(TestGeometricJacobian, task_jacobian_position_rows_match_six_dof_rows) +{ + const Transform tool{ Matrix3::Identity(), Vector3{ 1.0f, 0.0f, 0.0f } }; + const kinematics::ChainTaskJacobian position{ planar, tool }; + const kinematics::ChainTaskJacobian pose{ planar, tool }; + const math::Vector q{ 0.4f, -0.7f }; + const math::Vector qDot{ 0.3f, -0.5f }; + + const auto positionRows{ position.Jacobian(q) }; + const auto poseRows{ pose.Jacobian(q) }; + const auto positionBias{ position.BiasAcceleration(q, qDot) }; + const auto poseBias{ pose.BiasAcceleration(q, qDot) }; + + for (std::size_t r = 0; r < 3; ++r) + { + for (std::size_t c = 0; c < 2; ++c) + EXPECT_EQ(positionRows.at(r, c), poseRows.at(r, c)); + EXPECT_EQ(positionBias[r], poseBias[r]); + } + ExpectNear(position.ToolPose(q).translation, pose.ToolPose(q).translation); +} From 7bf202759c323768ae7c155e2a9d7966d47c02cf Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 11:23:28 +0000 Subject: [PATCH 4/4] feat(dynamics): add ChainDynamicsModel and InverseDynamicsModel (M29) Adapter from a link chain to the manipulator equation. It implements EulerLagrangeDynamics (M via CRBA, C qdot and g via RNEA) and the new one-call InverseDynamicsModel used by computed-torque control. SetLinks handles payload changes. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 14 +-- ROADMAP.md | 9 +- doc/dynamics/ChainDynamicsModel.md | 101 ++++++++++++++++ doc/dynamics/README.md | 19 +-- roadmap/README.md | 4 +- .../ChainDynamicsModel/explanation.md | 31 ----- .../ChainDynamicsModel/implementation.md | 78 ------------- roadmap/dynamics/ChainDynamicsModel/tests.md | 40 ------- robotics/dynamics/CMakeLists.txt | 3 + robotics/dynamics/ChainDynamicsModel.cpp | 7 ++ robotics/dynamics/ChainDynamicsModel.hpp | 85 ++++++++++++++ robotics/dynamics/InverseDynamicsModel.hpp | 23 ++++ robotics/dynamics/test/CMakeLists.txt | 1 + .../dynamics/test/TestChainDynamicsModel.cpp | 109 ++++++++++++++++++ 14 files changed, 351 insertions(+), 173 deletions(-) create mode 100644 doc/dynamics/ChainDynamicsModel.md delete mode 100644 roadmap/dynamics/ChainDynamicsModel/explanation.md delete mode 100644 roadmap/dynamics/ChainDynamicsModel/implementation.md delete mode 100644 roadmap/dynamics/ChainDynamicsModel/tests.md create mode 100644 robotics/dynamics/ChainDynamicsModel.cpp create mode 100644 robotics/dynamics/ChainDynamicsModel.hpp create mode 100644 robotics/dynamics/InverseDynamicsModel.hpp create mode 100644 robotics/dynamics/test/TestChainDynamicsModel.cpp diff --git a/README.md b/README.md index 100c0df..224a14d 100644 --- a/README.md +++ b/README.md @@ -35,13 +35,13 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|--------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Inverse Kinematics (Damped Least Squares) | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), 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 | +| Category | Description | +|--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, 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 | 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 e7861d1..aac69a1 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -93,10 +93,11 @@ The library already has the core of manipulator **modelling**: forward/inverse d ([RNEA](robotics/dynamics/RecursiveNewtonEuler.hpp), [ABA](robotics/dynamics/ArticulatedBodyAlgorithm.hpp), [Euler-Lagrange](robotics/dynamics/EulerLagrangeSolver.hpp)), the joint-space mass matrix ([CRBA](robotics/dynamics/CompositeRigidBodyAlgorithm.hpp)), a fixed-step simulation step -([ForwardDynamicsIntegrator](robotics/dynamics/ForwardDynamicsIntegrator.hpp)), position forward kinematics with base and +([ForwardDynamicsIntegrator](robotics/dynamics/ForwardDynamicsIntegrator.hpp)), the manipulator-equation adapter +([ChainDynamicsModel](robotics/dynamics/ChainDynamicsModel.hpp)), position forward kinematics with base and tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). Full-pose kinematics and the geometric Jacobian have shipped (see the note below); what is missing is the -**model plumbing, planning and control layer** that turns those models into a usable manipulator stack. +**planning and control layer** that turns those models into a usable manipulator stack. > **Full-pose kinematics shipped.** Links are generic ([JointLink](robotics/dynamics/JointLink.hpp): > revolute or prismatic, limits, armature); [ChainPoseKinematics](robotics/kinematics/ChainPoseKinematics.hpp) @@ -119,7 +120,6 @@ existing `dynamics/` convention. | # | Component | Target module | Depends on | Difficulty | |-----|-----------------------------------------------------------------|---------------------------|----------------|------------| | M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | -| M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | | 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 | ★★☆☆☆ | @@ -151,9 +151,6 @@ retired here. ### Tier 2 — Easy ★★☆☆☆ -**M29. Chain dynamics model.** Adapter implementing `EulerLagrangeDynamics` (M via CRBA, `Cq̇` and `g` via RNEA) and a one-call `InverseDynamicsModel` for the controllers. -- *Reuses / builds on:* RNEA, M28; optional friction model from NT57. - **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. diff --git a/doc/dynamics/ChainDynamicsModel.md b/doc/dynamics/ChainDynamicsModel.md new file mode 100644 index 0000000..ad8dbc9 --- /dev/null +++ b/doc/dynamics/ChainDynamicsModel.md @@ -0,0 +1,101 @@ +# Chain Dynamics Model + +## Overview & Motivation + +Model-based manipulator controllers are written against the **manipulator equation** + +$$M(q)\,\ddot{q} + C(q, \dot{q})\,\dot{q} + g(q) = \tau$$ + +Different controllers need different pieces of it: + +- PD control with gravity compensation needs only $g(q)$. +- Operational-space control needs $M(q)$. +- Computed-torque control needs the whole right-hand side for a commanded acceleration, ideally in a single pass. + +The recursive algorithms compute these pieces efficiently from a physical link description, but they speak in links, frames and spatial vectors rather than in matrices of the joint space. + +The chain dynamics model is the adapter between the two worlds. Given a link chain and the gravity vector, it answers every term of the manipulator equation through two small abstract interfaces. Controllers stay independent of how the dynamics are computed, can be tested against mock models, and can be bound to the concrete model on a hard real-time path. + +## Mathematical Theory + +### Terms from the Recursions + +With inverse dynamics written as $\mathrm{ID}(q, \dot{q}, \ddot{q}, a_g)$ (the recursive Newton–Euler algorithm, with gravity entering as a base acceleration $a_g = -g$): + +$$\begin{aligned} +g(q) &= \mathrm{ID}(q, 0, 0, -g) \\ +C(q, \dot{q})\,\dot{q} &= \mathrm{ID}(q, \dot{q}, 0, 0) \\ +M(q)\,\ddot{q} + C\dot{q} + g &= \mathrm{ID}(q, \dot{q}, \ddot{q}, -g) \\ +M(q) &= \text{composite rigid body algorithm} +\end{aligned}$$ + +These identities hold because inverse dynamics is affine in $\ddot{q}$ and in the gravity vector, and quadratic in $\dot{q}$. Setting the other arguments to zero isolates each term. The mass matrix could also be assembled column by column, $M e_j = \mathrm{ID}(q, 0, e_j, 0)$, but the composite rigid body algorithm builds it directly at a lower cost. + +### Vector, Not Matrix + +The recursion returns the Coriolis and centrifugal **vector** $C(q, \dot{q})\dot{q}$, not the matrix $C(q, \dot{q})$. The vector is all that computed-torque control and simulation need. Passivity-based controllers and momentum observers need a particular factorization of the matrix itself: the Christoffel form, with $\dot{M} - 2C$ skew-symmetric. That form requires a modified recursion. + +### Consistency + +Because all four quantities come from the same link model, the identity $\mathrm{ID}(q, \dot{q}, \ddot{q}) = M\ddot{q} + C\dot{q} + g$ holds up to rounding. Forward dynamics by solving $M\ddot{q} = \tau - C\dot{q} - g$ reproduces the articulated-body result. Reflected actuator inertia (armature) appears consistently in $M$ and in the one-pass inverse dynamics. + +### Model Updates + +A changed payload or tool alters the last link's mass, centre of mass and inertia. Replacing the link description updates every term at once, since nothing is cached between calls. + +## Complexity Analysis + +| Term | Time | Space | Notes | +|-----------------------------|----------|----------|-----------------------------------------| +| Gravity $g(q)$ | $O(n)$ | $O(n)$ | One inverse-dynamics pass | +| Coriolis vector $C\dot{q}$ | $O(n)$ | $O(n)$ | One inverse-dynamics pass | +| Inverse dynamics (one call) | $O(n)$ | $O(n)$ | One pass for $M\ddot{q} + C\dot{q} + g$ | +| Mass matrix $M(q)$ | $O(n^2)$ | $O(n^2)$ | Composite rigid body algorithm | + +Computing the three terms separately and combining them costs three passes plus an $O(n^2)$ product. The one-call inverse dynamics does it in a single $O(n)$ pass, which is why computed-torque control uses it. + +## Step-by-Step Walkthrough + +Two uniform rods rotate about horizontal parallel axes, with angles measured from the horizontal and gravity $9.81$ m/s² downwards. Rod 1 has $m_1 = 1.2$ kg and $l_1 = 0.6$ m; rod 2 has $m_2 = 0.7$ kg and $l_2 = 0.45$ m. The state is $q = (0.4, -0.9)$, $\dot{q} = (1.1, -0.6)$, $\ddot{q} = (0.8, -1.7)$. + +1. Gravity: $g = -9.81\,[\,m_1 \tfrac{l_1}{2}\cos q_1 + m_2(l_1 \cos q_1 + \tfrac{l_2}{2}\cos(q_1{+}q_2)),\ m_2 \tfrac{l_2}{2}\cos(q_1{+}q_2)\,]$. +2. Coriolis: with $h = m_2 l_1 \tfrac{l_2}{2} \sin q_2$, $C\dot{q} = [-h(2\dot{q}_1\dot{q}_2 + \dot{q}_2^2),\ h\dot{q}_1^2]$. +3. Mass matrix: $M_{11} = \tfrac{m_1 l_1^2}{3} + m_2(l_1^2 + \tfrac{l_2^2}{3} + l_1 l_2 \cos q_2)$, $M_{12} = m_2(\tfrac{l_2^2}{3} + \tfrac{l_1 l_2}{2}\cos q_2)$, $M_{22} = \tfrac{m_2 l_2^2}{3}$. +4. Their sum at the given state is $\tau = (-8.2064, -1.4410)$ N·m, exactly the one-pass inverse dynamics. +5. Doubling $m_2$ (a payload) changes only the $m_2$ terms of $g$, and the model reports the new holding torques immediately. + +## Pitfalls & Edge Cases + +- **Gravity sign**: gravity is the physical vector, e.g. $(0, 0, -9.81)$. The recursion internally applies it as a base acceleration of $+9.81$ upwards; passing $+g$ flips every holding torque. +- **Coriolis matrix vs vector**: do not reconstruct $C(q, \dot{q})$ from the vector. Many matrices produce the same vector, and only the Christoffel one preserves the passivity property. +- **Friction is not dynamics**: joint friction belongs in a separate model added to the torque command. Folding it into $C\dot{q}$ breaks the energy structure that controllers rely on. +- **Virtual dispatch**: the abstract interfaces cost an indirect call per term. On hard real-time paths, bind the controller to the concrete model to devirtualize. +- **Stale payloads**: the model is only as good as its link parameters. Update the last link whenever the gripper picks up a known load. + +## Variants & Generalizations + +- **Christoffel Coriolis matrix and regressor**: a modified recursion gives $C(q, \dot{q})\dot{q}_r$ with the skew-symmetric property and the inertial-parameter regressor $Y(q, \dot{q}, \dot{q}_r, \ddot{q}_r)$ for adaptive control. +- **Friction and motor models**: Coulomb, viscous and Stribeck terms added beside the rigid-body model. +- **Floating base**: the same interface over a floating-base chain yields the dynamics of legged and aerial manipulators. +- **Identified models**: base-parameter identification replaces nominal link parameters with fitted ones while keeping the same interface. + +## Applications + +- PD control with gravity compensation (gravity term only). +- Computed-torque and inverse-dynamics control (one-pass inverse dynamics). +- Operational-space and impedance control with inertia shaping (mass matrix). +- Forward-dynamics simulation through $M\ddot{q} = \tau - C\dot{q} - g$, and model-in-the-loop controller tests. + +## Connections to Other Algorithms + +- [Recursive Newton-Euler](RecursiveNewtonEuler.md): computes the gravity, Coriolis and one-pass inverse-dynamics terms. +- [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md): computes the mass matrix. +- [Euler-Lagrange](EulerLagrange.md): its forward and inverse dynamics run on any model that supplies $M$, $C\dot{q}$ and $g$, including this one. +- [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md): forward dynamics without forming $M$; it must agree with solving the manipulator equation. +- [PD Control with Gravity Compensation](../controllers/manipulator/PdGravityCompensation.md): consumes the gravity term. + +## References & Further Reading + +- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. Chapters 5 and 6. +- Siciliano, B., Sciavicco, L., Villani, L. and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 7. +- Spong, M.W., Hutchinson, S. and Vidyasagar, M. (2020). *Robot Modeling and Control*. 2nd ed. Wiley. Chapters 7 and 9. diff --git a/doc/dynamics/README.md b/doc/dynamics/README.md index a1a75b2..a730924 100644 --- a/doc/dynamics/README.md +++ b/doc/dynamics/README.md @@ -4,12 +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 | -| [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 | diff --git a/roadmap/README.md b/roadmap/README.md index 92436ee..8185c6a 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -29,7 +29,7 @@ Conventions shared by every spec: 6-vectors are ordered linear part first — tw `(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 3-vectors `(vx, vy, ω)` by [math/SE2Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE2Transform/implementation.md) (NT66); dynamics and Jacobians reach controllers through interfaces implemented by -[dynamics/ChainDynamicsModel](dynamics/ChainDynamicsModel/implementation.md) and +[dynamics/ChainDynamicsModel](../robotics/dynamics/ChainDynamicsModel.hpp) and [kinematics/JacobianProvider](../robotics/kinematics/JacobianProvider.hpp) (shipped). ## Deployment shape (float-only) @@ -92,7 +92,7 @@ prerequisites. `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) ### `dynamics` -`MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) +`MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) ### `trajectory` `CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) diff --git a/roadmap/dynamics/ChainDynamicsModel/explanation.md b/roadmap/dynamics/ChainDynamicsModel/explanation.md deleted file mode 100644 index 290f2aa..0000000 --- a/roadmap/dynamics/ChainDynamicsModel/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Chain Dynamics Model — Overview - -## What it is -An adapter that turns a robot described link by link into the "manipulator equation" -`M(q)q̈ + C(q,q̇)q̇ + g(q) = τ` that model-based controllers speak: it answers "what is the mass -matrix?", "what are the Coriolis/centrifugal torques?", "what are the gravity torques?" and "what -torque produces this acceleration?". - -## Why it matters (embedded) -Gravity compensation, computed-torque, impedance and operational-space controllers all need these -terms. Without an adapter every controller would re-derive them or require hand-written analytic -models; with it, one link description drives all of them, and the one-call inverse-dynamics path -keeps computed-torque control at linear cost per cycle. - -## How it works (intuition) -The recursive Newton-Euler algorithm already computes joint torques for any state; zeroing the -acceleration and gravity isolates the velocity-dependent torques, zeroing velocity and acceleration -isolates gravity, and passing everything gives the full inverse dynamics in one pass. The composite -rigid body algorithm supplies the mass matrix. - -## Key parameters -- **Link array** — the same inertial and geometric description the dynamics algorithms use. -- **Gravity vector** — expressed in the base frame. - -## Reference -R. Featherstone, *Rigid Body Dynamics Algorithms* (2008), Ch. 5–6; M. W. Spong, S. Hutchinson, -M. Vidyasagar, *Robot Modeling and Control* (2020), Ch. 6. - -## See also -`RecursiveNewtonEuler`, `CompositeRigidBodyAlgorithm` (M28), `EulerLagrangeSolver`, -`PdGravityCompensation` (M5), `ComputedTorqueControl` (M12), `CoriolisMatrixAndRegressor` (M31). diff --git a/roadmap/dynamics/ChainDynamicsModel/implementation.md b/roadmap/dynamics/ChainDynamicsModel/implementation.md deleted file mode 100644 index e5fe2a4..0000000 --- a/roadmap/dynamics/ChainDynamicsModel/implementation.md +++ /dev/null @@ -1,78 +0,0 @@ -# Chain Dynamics Model (Link Chain → Manipulator Equation) — Implementation Pseudocode - -> Roadmap ref: #M29 (Tier 2) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -The manipulator controllers (M5, M12, M16–M20) consume dynamics through interfaces, but nothing in the -library turns a link description into those interfaces. This adapter does: it implements the shipped -`EulerLagrangeDynamics` interface (`M`, `C q̇`, `g`) with CRBA (M28) and RNEA, and a new one-call -`InverseDynamicsModel` interface so computed-torque control can run in `O(n)`. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class InverseDynamicsModel: # new interface, protected ~InverseDynamicsModel() = default - virtual StateVector ComputeInverseDynamics(const StateVector& q, const StateVector& qDot, - const StateVector& qDDot) const = 0 - -template -class ChainDynamicsModel - : public EulerLagrangeDynamics # shipped interface - , public InverseDynamicsModel: - LinkArray links # owned copy (payload updates via SetLinks) - Vector3 gravity # base-frame gravity, e.g. (0, 0, −9.81) - RecursiveNewtonEuler rnea - CompositeRigidBodyAlgorithm crba # M28 -``` - -## Interface - -```text -ChainDynamicsModel(const LinkArray& links, const Vector3& gravity) -void SetLinks(const LinkArray& links) # e.g. payload change -MassMatrix ComputeMassMatrix(const StateVector& q) const override -StateVector ComputeCoriolisTerms(const StateVector& q, const StateVector& qDot) const override -StateVector ComputeGravityTerms(const StateVector& q) const override -StateVector ComputeInverseDynamics(const StateVector& q, const StateVector& qDot, - const StateVector& qDDot) const override # hot path -``` - -## Algorithm (pseudocode) - -```text -ComputeMassMatrix(q) = crba.Compute(links, q) -ComputeCoriolisTerms(q, q̇) = rnea.InverseDynamics(links, q, q̇, 0, 0) # no gravity, no q̈ -ComputeGravityTerms(q) = rnea.InverseDynamics(links, q, 0, 0, gravity) -ComputeInverseDynamics(q,q̇,q̈) = rnea.InverseDynamics(links, q, q̇, q̈, gravity) # one O(n) pass -# with M1 fields and an NT57 friction model present: + armature ⊙ q̈ + friction(q̇) in ComputeInverseDynamics, -# + diag(armature) in ComputeMassMatrix, friction reported separately (it is not part of C q̇) -``` - -## Complexity & memory - -- `ComputeInverseDynamics`, `ComputeCoriolisTerms`, `ComputeGravityTerms`: `O(n)` each (one RNEA pass). -- `ComputeMassMatrix`: `O(n²)` (CRBA). -- Memory: one copy of the link array; no heap. - -## Numerical / embedded notes - -- `C(q,q̇)q̇` obtained from RNEA is the correct Coriolis/centrifugal *vector*; controllers that need - the Coriolis *matrix* (Slotine–Li, momentum observer) must use M31 instead. -- The interfaces are virtual so controllers can be tested with StrictMock models. On a hard real-time - path, template the controller on `ChainDynamicsModel` directly to devirtualize (AGENTS.md: no virtual - calls in real-time paths). -- Must satisfy `ComputeInverseDynamics(q, q̇, q̈) = M q̈ + C q̇ + g` exactly (up to rounding) — the - cross-check test pins it. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Headers: `robotics/dynamics/InverseDynamicsModel.hpp` (pure interface) and - `robotics/dynamics/ChainDynamicsModel.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `ComputeInverseDynamics`, and `extern template class ChainDynamicsModel;` - / `` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/dynamics/ChainDynamicsModel.cpp` → the same instantiations. -- Test: `robotics/dynamics/test/TestChainDynamicsModel.cpp` -- Doc: `doc/dynamics/ChainDynamicsModel.md` (per `doc/TEMPLATE.md`) -- Depends on: M28. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/ChainDynamicsModel/tests.md b/roadmap/dynamics/ChainDynamicsModel/tests.md deleted file mode 100644 index 29ee47c..0000000 --- a/roadmap/dynamics/ChainDynamicsModel/tests.md +++ /dev/null @@ -1,40 +0,0 @@ -# Chain Dynamics Model — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestChainDynamicsModel : public ::testing::Test: - # 2-link uniform rods about ŷ, gravity (0, 0, −9.81), q measured from horizontal - ChainDynamicsModel model{ MakeRodChain(1.2, 0.6, 0.7, 0.45), (0, 0, −9.81) } -# each case below is a TEST_F(TestChainDynamicsModel, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -gravity_terms_match_closed_form: - Assert: g(q) = −g·[m1l1/2·c1 + m2(l1c1 + l2/2·c12), m2l2/2·c12] -coriolis_terms_match_closed_form: - Assert: C q̇ = [−h(2q̇1q̇2 + q̇2²), h q̇1²], h = m2 l1 l2/2 · sin q2 -mass_matrix_matches_crba: - Assert: ComputeMassMatrix(q) == CompositeRigidBodyAlgorithm::Compute(links, q) -inverse_dynamics_equals_manipulator_equation: - Assert: ComputeInverseDynamics(q, q̇, q̈) ≈ M q̈ + C q̇ + g -euler_lagrange_forward_dynamics_matches_articulated_body_algorithm: - Assert: EulerLagrangeSolver::ForwardDynamics(model, q, q̇, τ) ≈ ABA(links, q, q̇, τ, gravity) -set_links_updates_every_term: - Arrange: double the last link mass via SetLinks - Assert: g(q) changes by the closed-form payload contribution -``` - -## Reference vectors - -- `m1=1.2, l1=0.6, m2=0.7, l2=0.45`, `q=(0.4,−0.9)`, `q̇=(1.1,−0.6)`, `q̈=(0.8,−1.7)` ⇒ - `τ = (−8.2064, −1.4410)` (analytic, matches RNEA). - -## Edge cases - -- Zero gravity ⇒ `g(q) = 0` for every `q`. -- `q̇ = 0` ⇒ `C q̇ = 0`. diff --git a/robotics/dynamics/CMakeLists.txt b/robotics/dynamics/CMakeLists.txt index 9546bdf..5f023f7 100644 --- a/robotics/dynamics/CMakeLists.txt +++ b/robotics/dynamics/CMakeLists.txt @@ -13,10 +13,12 @@ target_link_libraries(robotics.dynamics ${ROBOTICS_VISIBILITY} target_sources(robotics.dynamics PRIVATE ArticulatedBodyAlgorithm.hpp + ChainDynamicsModel.hpp CompositeRigidBodyAlgorithm.hpp EulerLagrangeDynamics.hpp EulerLagrangeSolver.hpp ForwardDynamicsIntegrator.hpp + InverseDynamicsModel.hpp JointLink.hpp NewtonEulerBody.hpp NewtonEulerSolver.hpp @@ -27,6 +29,7 @@ target_sources(robotics.dynamics PRIVATE robotics_add_coverage_sources(robotics.dynamics ArticulatedBodyAlgorithm.cpp + ChainDynamicsModel.cpp CompositeRigidBodyAlgorithm.cpp EulerLagrangeSolver.cpp ForwardDynamicsIntegrator.cpp diff --git a/robotics/dynamics/ChainDynamicsModel.cpp b/robotics/dynamics/ChainDynamicsModel.cpp new file mode 100644 index 0000000..2a14916 --- /dev/null +++ b/robotics/dynamics/ChainDynamicsModel.cpp @@ -0,0 +1,7 @@ +#include "robotics/dynamics/ChainDynamicsModel.hpp" + +namespace dynamics +{ + template class ChainDynamicsModel; + template class ChainDynamicsModel; +} diff --git a/robotics/dynamics/ChainDynamicsModel.hpp b/robotics/dynamics/ChainDynamicsModel.hpp new file mode 100644 index 0000000..08b5162 --- /dev/null +++ b/robotics/dynamics/ChainDynamicsModel.hpp @@ -0,0 +1,85 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/CompositeRigidBodyAlgorithm.hpp" +#include "robotics/dynamics/EulerLagrangeDynamics.hpp" +#include "robotics/dynamics/InverseDynamicsModel.hpp" +#include "robotics/dynamics/JointLink.hpp" +#include "robotics/dynamics/RecursiveNewtonEuler.hpp" +#include +#include +#include + +namespace dynamics +{ + template + class ChainDynamicsModel + : public EulerLagrangeDynamics + , public InverseDynamicsModel + { + static_assert(std::is_floating_point_v, "ChainDynamicsModel supports floating-point types"); + + public: + using StateVector = math::Vector; + using MassMatrix = math::SquareMatrix; + using Vector3 = math::Vector; + using LinkArray = std::array, NumLinks>; + + ChainDynamicsModel(const LinkArray& links, const Vector3& gravity); + + void SetLinks(const LinkArray& newLinks); + + MassMatrix ComputeMassMatrix(const StateVector& q) const override; + StateVector ComputeCoriolisTerms(const StateVector& q, const StateVector& qDot) const override; + StateVector ComputeGravityTerms(const StateVector& q) const override; + OPTIMIZE_FOR_SPEED StateVector ComputeInverseDynamics(const StateVector& q, const StateVector& qDot, const StateVector& qDDot) const override; + + private: + LinkArray links; + Vector3 gravity; + [[no_unique_address]] RecursiveNewtonEuler rnea; + [[no_unique_address]] CompositeRigidBodyAlgorithm crba; + }; + + template + ChainDynamicsModel::ChainDynamicsModel(const LinkArray& links, const Vector3& gravity) + : links{ links } + , gravity{ gravity } + {} + + template + void ChainDynamicsModel::SetLinks(const LinkArray& newLinks) + { + links = newLinks; + } + + template + typename ChainDynamicsModel::MassMatrix ChainDynamicsModel::ComputeMassMatrix(const StateVector& q) const + { + return crba.Compute(links, q); + } + + template + typename ChainDynamicsModel::StateVector ChainDynamicsModel::ComputeCoriolisTerms(const StateVector& q, const StateVector& qDot) const + { + return rnea.InverseDynamics(links, q, qDot, StateVector{}, Vector3{}); + } + + template + typename ChainDynamicsModel::StateVector ChainDynamicsModel::ComputeGravityTerms(const StateVector& q) const + { + return rnea.InverseDynamics(links, q, StateVector{}, StateVector{}, gravity); + } + + template + OPTIMIZE_FOR_SPEED typename ChainDynamicsModel::StateVector ChainDynamicsModel::ComputeInverseDynamics(const StateVector& q, const StateVector& qDot, const StateVector& qDDot) const + { + return rnea.InverseDynamics(links, q, qDot, qDDot, gravity); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ChainDynamicsModel; + extern template class ChainDynamicsModel; +#endif +} diff --git a/robotics/dynamics/InverseDynamicsModel.hpp b/robotics/dynamics/InverseDynamicsModel.hpp new file mode 100644 index 0000000..a1f2cb9 --- /dev/null +++ b/robotics/dynamics/InverseDynamicsModel.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include +#include + +namespace dynamics +{ + template + class InverseDynamicsModel + { + static_assert(std::is_floating_point_v, "InverseDynamicsModel supports floating-point types"); + static_assert(Dof > 0, "Degrees of freedom must be positive"); + + public: + using StateVector = math::Vector; + + virtual StateVector ComputeInverseDynamics(const StateVector& q, const StateVector& qDot, const StateVector& qDDot) const = 0; + + protected: + ~InverseDynamicsModel() = default; + }; +} diff --git a/robotics/dynamics/test/CMakeLists.txt b/robotics/dynamics/test/CMakeLists.txt index 112d5b4..6fbb582 100644 --- a/robotics/dynamics/test/CMakeLists.txt +++ b/robotics/dynamics/test/CMakeLists.txt @@ -9,6 +9,7 @@ target_link_libraries(robotics.dynamics_test PUBLIC target_sources(robotics.dynamics_test PRIVATE TestArticulatedBodyAlgorithm.cpp + TestChainDynamicsModel.cpp TestCompositeRigidBodyAlgorithm.cpp TestEulerLagrangeSolver.cpp TestForwardDynamicsIntegrator.cpp diff --git a/robotics/dynamics/test/TestChainDynamicsModel.cpp b/robotics/dynamics/test/TestChainDynamicsModel.cpp new file mode 100644 index 0000000..64808f2 --- /dev/null +++ b/robotics/dynamics/test/TestChainDynamicsModel.cpp @@ -0,0 +1,109 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/dynamics/ArticulatedBodyAlgorithm.hpp" +#include "robotics/dynamics/ChainDynamicsModel.hpp" +#include "robotics/dynamics/CompositeRigidBodyAlgorithm.hpp" +#include "robotics/dynamics/EulerLagrangeSolver.hpp" +#include +#include + +namespace +{ + using Vector2 = math::Vector; + using Vector3 = math::Vector; + using Link = dynamics::JointLink; + using LinkArray = std::array; + + constexpr float gravity{ 9.81f }; + constexpr float m1{ 1.2f }; + constexpr float l1{ 0.6f }; + constexpr float m2{ 0.7f }; + constexpr float l2{ 0.45f }; + const Vector3 gravityVector{ 0.0f, 0.0f, -gravity }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + + 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 } }, yAxis, parentToJoint, + Vector3{ length / 2.0f, 0.0f, 0.0f } }; + } + + LinkArray MakeRodChain(float mass1, float length1, float mass2, float length2) + { + return LinkArray{ MakeRod(mass1, length1, Vector3{}), MakeRod(mass2, length2, Vector3{ length1, 0.0f, 0.0f }) }; + } + + Vector2 ClosedFormGravity(float mass2, const Vector2& q) + { + const float c1{ std::cos(q[0]) }; + const float c12{ std::cos(q[0] + q[1]) }; + return Vector2{ -gravity * (m1 * l1 / 2.0f * c1 + mass2 * (l1 * c1 + l2 / 2.0f * c12)), -gravity * mass2 * l2 / 2.0f * c12 }; + } + + class TestChainDynamicsModel + : public ::testing::Test + { + protected: + void ExpectNear(const Vector2& actual, const Vector2& expected) const + { + EXPECT_NEAR(actual[0], expected[0], math::Tolerance()); + EXPECT_NEAR(actual[1], expected[1], math::Tolerance()); + } + + LinkArray links{ MakeRodChain(m1, l1, m2, l2) }; + dynamics::ChainDynamicsModel model{ links, gravityVector }; + Vector2 q{ 0.4f, -0.9f }; + Vector2 qDot{ 1.1f, -0.6f }; + Vector2 qDDot{ 0.8f, -1.7f }; + }; +} + +TEST_F(TestChainDynamicsModel, gravity_terms_match_closed_form) +{ + ExpectNear(model.ComputeGravityTerms(q), ClosedFormGravity(m2, q)); +} + +TEST_F(TestChainDynamicsModel, coriolis_terms_match_closed_form) +{ + const float h{ m2 * l1 * l2 / 2.0f * std::sin(q[1]) }; + + ExpectNear(model.ComputeCoriolisTerms(q, qDot), Vector2{ -h * (2.0f * qDot[0] * qDot[1] + qDot[1] * qDot[1]), h * qDot[0] * qDot[0] }); +} + +TEST_F(TestChainDynamicsModel, mass_matrix_matches_crba) +{ + const auto M{ model.ComputeMassMatrix(q) }; + const auto expected{ dynamics::CompositeRigidBodyAlgorithm{}.Compute(links, q) }; + + for (std::size_t r = 0; r < 2; ++r) + for (std::size_t c = 0; c < 2; ++c) + EXPECT_EQ(M.at(r, c), expected.at(r, c)); +} + +TEST_F(TestChainDynamicsModel, inverse_dynamics_equals_manipulator_equation) +{ + const auto tau{ model.ComputeInverseDynamics(q, qDot, qDDot) }; + + ExpectNear(tau, Vector2{ model.ComputeMassMatrix(q) * qDDot + model.ComputeCoriolisTerms(q, qDot) + model.ComputeGravityTerms(q) }); + ExpectNear(tau, Vector2{ -8.2064f, -1.4410f }); +} + +TEST_F(TestChainDynamicsModel, euler_lagrange_forward_dynamics_matches_articulated_body_algorithm) +{ + const Vector2 tau{ 1.5f, -0.4f }; + + const auto fromModel{ dynamics::EulerLagrangeSolver{}.ForwardDynamics(model, q, qDot, tau) }; + const auto fromAba{ dynamics::ArticulatedBodyAlgorithm{}.ForwardDynamics(links, q, qDot, tau, gravityVector) }; + + ExpectNear(fromModel, fromAba); +} + +TEST_F(TestChainDynamicsModel, set_links_updates_every_term) +{ + LinkArray heavier{ links }; + heavier[1].mass = 2.0f * m2; + + model.SetLinks(heavier); + + ExpectNear(model.ComputeGravityTerms(q), ClosedFormGravity(2.0f * m2, q)); +}