diff --git a/.vscode/launch.json b/.vscode/launch.json index 8c9f63fb..d2d1d5ec 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -81,26 +81,6 @@ } ] }, - { - "name": "Robot Arm Simulator", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/host/simulator/dynamics/RobotArm/Debug/numerical.simulator.dynamics.robot_arm", - "args": [], - "stopAtEntry": false, - "cwd": "${workspaceFolder}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "miDebuggerPath": "/usr/bin/gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - } - ] - }, { "name": "Kalman Filter Simulator", "type": "cppdbg", diff --git a/AGENTS.md b/AGENTS.md index 6a8d7342..48a2aa2d 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -4,7 +4,7 @@ Single source of truth for **Claude and Copilot**. `CLAUDE.md` and `.github/copilot-instructions.md` point here. Code-file specifics load on demand from `.github/instructions/`. Deployment recipe: `roadmap/DEPLOYMENT.md`. -Numerical algorithms library (DSP, control, filters, estimators, kinematics/dynamics) for +Numerical algorithms library (DSP, control, filters, estimators, solvers) for resource-constrained embedded systems. Real-time, deterministic, no heap. ## Numeric policy — FLOAT-ONLY (current) @@ -49,9 +49,9 @@ resource-constrained embedded systems. Real-time, deterministic, no heap. ## Namespaces -`analysis`, `windowing`, `control_analysis`, `controllers`, `dynamics`, `estimators`, +`analysis`, `windowing`, `control_analysis`, `controllers`, `estimators`, `filters` (active Kalman family — **not** `filters::active`), `filters::passive`, `math`, -`neural_network`, `optimization`, `regularization`, `solvers`, and new: `trajectory`, +`neural_network`, `optimization`, `regularization`, `solvers`, and new: `robust_control`, `nonlinear_control`. ## Testing diff --git a/README.md b/README.md index c7bb4ef7..ec1a4167 100644 --- a/README.md +++ b/README.md @@ -19,10 +19,8 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal | | [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space | | [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler, ABA | | [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Yule-Walker (offline), Recursive Least Squares, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS | | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, Madgwick/Mahony AHRS | -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics | | [Neural Network](doc/neural_network/README.md) | Layers, activations, losses, model | | [Optimization](doc/optimization/README.md) | Gradient Descent | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | diff --git a/ROADMAP.md b/ROADMAP.md index 076c5174..1f34e7ca 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -281,175 +281,6 @@ the library does not yet expose. Detailed below under --- -## Robot manipulators & other manipulator types - -The library already has the hard parts of manipulator **modelling**: forward/inverse dynamics -([RNEA](numerical/dynamics/RecursiveNewtonEuler.hpp), [ABA](numerical/dynamics/ArticulatedBodyAlgorithm.hpp), -[Euler-Lagrange](numerical/dynamics/EulerLagrangeSolver.hpp) giving $M$, $C$, $g$) plus -damped-least-squares [IK](numerical/kinematics/InverseKinematics.hpp). What is missing is the -**control, planning, and full-pose kinematics layer** that turns those models into a usable -manipulator stack. Two current limitations gate most of the items below: - -> **Position-only, revolute-only.** [ForwardKinematics.hpp](numerical/kinematics/ForwardKinematics.hpp) -> returns joint *positions* (a 3×N Jacobian lives privately inside IK), and only -> [RevoluteJointLink](numerical/dynamics/RevoluteJointLink.hpp) exists. Items **M1** (prismatic/generic -> joints) and **M8** (full 6×N spatial Jacobian) lift these limits and unblock the rest. - -All manipulator items are *(float-first)* — torques, lengths, and inertias exceed the `Q15`/`Q31` -range, matching the existing `dynamics/` convention. - -### Manipulator list (by priority) - -| # | Component | Target module | Difficulty | -|-----|---------------------------------------------------------|---------------------------------|------------| -| M1 | Prismatic / generic joint link | `dynamics` + `kinematics` | ★☆☆☆☆ | -| M2 | Cubic / quintic polynomial joint trajectory | `trajectory` (new) | ★☆☆☆☆ | -| M3 | Trapezoidal (LSPB) velocity profile | `trajectory` (new) | ★☆☆☆☆ | -| M4 | Friction compensation (Coulomb + viscous + Stribeck) | `dynamics` | ★☆☆☆☆ | -| M5 | PD + gravity compensation control | `controllers/manipulator` (new) | ★☆☆☆☆ | -| M6 | Homogeneous transform / SE(3) + adjoint (twists) | `math` | ★★☆☆☆ | -| M7 | Denavit-Hartenberg parameters | `kinematics` | ★★☆☆☆ | -| M8 | Geometric / analytic Jacobian (6×N) | `kinematics` | ★★☆☆☆ | -| M9 | S-curve (jerk-limited) trajectory | `trajectory` (new) | ★★☆☆☆ | -| M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | ★★☆☆☆ | -| M11 | Manipulability ellipsoid / Yoshikawa index | `kinematics` | ★★☆☆☆ | -| M12 | Computed-torque (inverse-dynamics) control | `controllers/manipulator` (new) | ★★★☆☆ | -| M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | ★★★☆☆ | -| M14 | Redundancy resolution / null-space projection | `kinematics` | ★★★☆☆ | -| M15 | Product-of-Exponentials forward kinematics | `kinematics` | ★★★☆☆ | -| M16 | Momentum-based collision-detection observer | `estimators/online` | ★★★☆☆ | -| M17 | Impedance / admittance control | `controllers/manipulator` (new) | ★★★☆☆ | -| M18 | Operational-space (task-space) control | `controllers/manipulator` (new) | ★★★★☆ | -| M19 | Hybrid position/force control | `controllers/manipulator` (new) | ★★★★☆ | -| M20 | Passivity-based adaptive control (Slotine-Li) | `controllers/manipulator` (new) | ★★★★☆ | -| M21 | Analytical IK (Pieper, wrist-partitioned 6R) | `kinematics` | ★★★★☆ | -| M22 | Dynamic (base-parameter) identification | `estimators/offline` | ★★★★☆ | -| M23 | Parallel-manipulator kinematics (Delta / Stewart-Gough) | `kinematics` | ★★★★☆ | -| M24 | Mobile-manipulator / nonholonomic-base kinematics | `kinematics` | ★★★★☆ | -| M25 | Cable-driven tension distribution | `controllers/manipulator` (new) | ★★★★☆ | -| M26 | Continuum / soft constant-curvature kinematics | `kinematics` | ★★★★☆ | -| M27 | Time-optimal path parameterization (TOPP) | `trajectory` (new) | ★★★★★ | - -### Tier 1 — Trivial ★☆☆☆☆ - -**M1. Prismatic / generic joint link.** Generalize `RevoluteJointLink` to a joint type carrying an axis + type (revolute/prismatic), so FK/IK/dynamics handle sliding joints (SCARA, gantries, hydraulic actuators). -- *Algorithm / paper:* J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3. -- *Reuses / builds on:* [RevoluteJointLink.hpp](numerical/dynamics/RevoluteJointLink.hpp); unblocks FK, IK, RNEA for mixed chains. - -**M2. Cubic / quintic polynomial joint trajectory.** Point-to-point motion with matched position/velocity(/acceleration) boundary conditions via closed-form polynomial coefficients. -- *Algorithm / paper:* Spong, Hutchinson, Vidyasagar, *Robot Modeling and Control*, Ch. 5 (polynomial trajectories). -- *Reuses / builds on:* scalar `math`; new `trajectory/` module. - -**M3. Trapezoidal (LSPB) velocity profile.** Linear-segment-with-parabolic-blends profile respecting velocity/acceleration limits. -- *Algorithm / paper:* L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), Ch. 3. -- *Reuses / builds on:* new `trajectory/` module. - -**M4. Friction compensation model.** Feedforward Coulomb + viscous + Stribeck joint-friction term added to any torque controller. -- *Algorithm / paper:* B. Armstrong-Hélouvry, P. Dupont, C. Canudas de Wit, "A survey of models, analysis tools and compensation methods for the control of machines with friction," *Automatica*, 30(7), 1994. -- *Reuses / builds on:* `dynamics`; composes with M5/M12. - -**M5. PD + gravity compensation control.** The simplest globally-stable set-point regulator: `τ = Kp·e − Kd·q̇ + g(q)`. -- *Algorithm / paper:* M. Takegaki, S. Arimoto, "A New Feedback Method for Dynamic Control of Manipulators," *ASME J. Dyn. Sys. Meas. Control*, 1981. -- *Reuses / builds on:* $g(q)$ from [RNEA](numerical/dynamics/RecursiveNewtonEuler.hpp) / Euler-Lagrange; new `controllers/manipulator/` module. - -### Tier 2 — Easy ★★☆☆☆ - -**M6. Homogeneous transform / SE(3) + adjoint.** Rigid-body transforms (4×4), twists/wrenches (6-vectors), and the adjoint map — the algebra all modern manipulator code is built on. -- *Algorithm / paper:* K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 3; Murray, Li, Sastry, *A Mathematical Introduction to Robotic Manipulation* (1994). -- *Reuses / builds on:* [Geometry3D.hpp](numerical/math/Geometry3D.hpp), item 18 (Quaternion). - -**M7. Denavit-Hartenberg parameters.** Standard `(a, α, d, θ)` link description and per-joint transform generation. -- *Algorithm / paper:* Craig, *Introduction to Robotics*, Ch. 3 (DH convention). -- *Reuses / builds on:* M6, `math::Matrix`. - -**M8. Geometric / analytic Jacobian (6×N).** Full spatial Jacobian mapping joint rates → end-effector linear + angular velocity (and its transpose for force mapping). -- *Algorithm / paper:* Lynch & Park, *Modern Robotics*, Ch. 5 (velocity kinematics). -- *Reuses / builds on:* promotes the private 3×N Jacobian in [InverseKinematics.hpp](numerical/kinematics/InverseKinematics.hpp); unblocks M11, M13, M14, M17, M18. - -**M9. S-curve (jerk-limited) trajectory.** Seven-segment jerk-bounded profile for smooth, low-vibration motion. -- *Algorithm / paper:* Biagiotti & Melchiorri, *Trajectory Planning*, Ch. 3 (double-S profiles). -- *Reuses / builds on:* M3; new `trajectory/` module. - -**M10. Cartesian path + orientation interpolation.** Straight-line/screw position paths with SLERP orientation blending for task-space moves. -- *Algorithm / paper:* Lynch & Park, Ch. 9; K. Shoemake, SLERP, *SIGGRAPH* 1985. -- *Reuses / builds on:* item 18 (Quaternion), M6. - -**M11. Manipulability ellipsoid / Yoshikawa index.** Scalar dexterity/singularity measure `√det(J Jᵀ)` for posture optimization and singularity avoidance. -- *Algorithm / paper:* T. Yoshikawa, "Manipulability of Robotic Mechanisms," *Int. J. Robotics Research*, 4(2), 1985. -- *Reuses / builds on:* M8, item 43 (SVD) or determinant of `math::Matrix`. - -### Tier 3 — Moderate ★★★☆☆ - -**M12. Computed-torque (inverse-dynamics) control.** Feedback-linearizing manipulator law `τ = M(q)(q̈_d + Kd·ė + Kp·e) + C(q,q̇)q̇ + g(q)` yielding decoupled error dynamics. -- *Algorithm / paper:* Spong et al., *Robot Modeling and Control*, Ch. 8; Luh, Walker, Paul (1980). -- *Reuses / builds on:* **directly leverages existing [RNEA](numerical/dynamics/RecursiveNewtonEuler.hpp) / [Euler-Lagrange](numerical/dynamics/EulerLagrangeSolver.hpp)** for $M$, $C$, $g$; item 40 (feedback linearization). - -**M13. Full 6-DOF pose IK.** Extend damped-least-squares IK to a position **and** orientation target using the 6×N Jacobian and a quaternion/log orientation error. -- *Algorithm / paper:* S. R. Buss, "Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods," 2004; Nakamura & Hanafusa (1986). -- *Reuses / builds on:* [InverseKinematics.hpp](numerical/kinematics/InverseKinematics.hpp), M8, item 18. - -**M14. Redundancy resolution / null-space projection.** Exploit extra DOF (7-DOF arms) via `q̇ = J⁺ẋ + (I − J⁺J)·q̇₀` for secondary objectives (joint-limit / obstacle avoidance). -- *Algorithm / paper:* A. Liégeois, "Automatic supervisory control of the configuration and behavior of multibody mechanisms," *IEEE Trans. SMC*, 7(12), 1977. -- *Reuses / builds on:* M8, item 27 (QR) / 43 (SVD) for the pseudo-inverse. - -**M15. Product-of-Exponentials forward kinematics.** Screw-theory FK (`T = e^{[S₁]θ₁}···e^{[Sₙ]θₙ}·M`), avoiding DH frame bookkeeping. -- *Algorithm / paper:* Lynch & Park, *Modern Robotics*, Ch. 4. -- *Reuses / builds on:* M6 (SE(3)/twists), item 29 (matrix exponential). - -**M16. Momentum-based collision-detection observer.** Estimate external joint torques from generalized-momentum residual — no joint-torque sensors or acceleration needed. -- *Algorithm / paper:* A. De Luca, A. Albu-Schäffer, S. Haddadin, G. Hirzinger, "Collision Detection and Safe Reaction with the DLR-III Lightweight Manipulator Arm," *IROS*, 2006. -- *Reuses / builds on:* RNEA, `estimators/online`. - -**M17. Impedance / admittance control.** Render a programmable mass-spring-damper at the end-effector for safe contact and compliant assembly. -- *Algorithm / paper:* N. Hogan, "Impedance Control: An Approach to Manipulation, Parts I–III," *ASME J. Dyn. Sys. Meas. Control*, 1985. -- *Reuses / builds on:* M8, M12, `dynamics`; new `controllers/manipulator/` module. - -### Tier 4 — Advanced ★★★★☆ - -**M18. Operational-space (task-space) control.** Control directly in Cartesian space using the task-space inertia `Λ = (J M⁻¹ Jᵀ)⁻¹` and dynamically-consistent null-space projection. -- *Algorithm / paper:* O. Khatib, "A Unified Approach for Motion and Force Control of Robot Manipulators: The Operational Space Formulation," *IEEE J. Robotics and Automation*, 3(1), 1987. -- *Reuses / builds on:* M8, M12, item 28 (LU) for the $M^{-1}$ solve. - -**M19. Hybrid position/force control.** Partition task directions into force-controlled and motion-controlled subspaces via a selection matrix. -- *Algorithm / paper:* M. Raibert, J. Craig, "Hybrid Position/Force Control of Manipulators," *ASME J. Dyn. Sys. Meas. Control*, 1981. -- *Reuses / builds on:* M8, M17, `controllers/manipulator/`. - -**M20. Passivity-based adaptive control (Slotine-Li).** Track trajectories while online-estimating inertial parameters, exploiting linearity-in-parameters `Y(q,q̇,q̈)·a = τ`. -- *Algorithm / paper:* J.-J. Slotine, W. Li, "On the Adaptive Control of Robot Manipulators," *Int. J. Robotics Research*, 6(3), 1987. -- *Reuses / builds on:* RNEA regressor form, `estimators/online`; item 47 (MRAC) kinship. - -**M21. Analytical IK (Pieper, wrist-partitioned 6R).** Closed-form inverse kinematics for the common 6R arm with a spherical wrist (all real solutions, no iteration). -- *Algorithm / paper:* D. Pieper, "The Kinematics of Manipulators Under Computer Control," PhD thesis, Stanford, 1968. -- *Reuses / builds on:* M6/M7, item 23 (CORDIC) or trig for the closed-form angles. - -**M22. Dynamic (base-parameter) identification.** Least-squares estimation of link inertial parameters from excitation trajectories via the linear regressor. -- *Algorithm / paper:* C. Atkeson, C. An, J. Hollerbach, "Estimation of Inertial Parameters of Manipulator Loads and Links," *Int. J. Robotics Research*, 5(3), 1986. -- *Reuses / builds on:* RNEA regressor, item 27 (QR) / 12 (poly LS), `estimators/offline`. - -**M23. Parallel-manipulator kinematics (Delta / Stewart-Gough).** Closed-form inverse kinematics and iterative forward kinematics for parallel platforms (pick-and-place Delta, 6-DOF hexapods). -- *Algorithm / paper:* J.-P. Merlet, *Parallel Robots*, 2nd ed. (2006); R. Clavel, delta robot (1990). -- *Reuses / builds on:* M6, item 28 (LU) / Newton iteration for the forward solve. - -**M24. Mobile-manipulator / nonholonomic-base kinematics.** Combined base + arm Jacobian with nonholonomic (differential-drive) constraints. -- *Algorithm / paper:* Y. Yamamoto, X. Yun, "Coordinating Locomotion and Manipulation of a Mobile Manipulator," *IEEE Trans. Automatic Control*, 39(6), 1994. -- *Reuses / builds on:* M8, M14 (redundancy). - -**M25. Cable-driven tension distribution.** Compute non-negative cable tensions realizing a desired wrench (cable robots, tendon-driven hands) via a bounded QP/LP. -- *Algorithm / paper:* T. Bruckmann, A. Pott (eds.), *Cable-Driven Parallel Robots* (2013); Pott tension-distribution methods. -- *Reuses / builds on:* [MPC](numerical/controllers/implementations/Mpc.hpp) QP machinery, M8. - -**M26. Continuum / soft constant-curvature kinematics.** Piecewise-constant-curvature FK/IK for tendon/pneumatic continuum arms. -- *Algorithm / paper:* R. Webster, B. Jones, "Design and Kinematic Modeling of Constant Curvature Continuum Robots: A Review," *Int. J. Robotics Research*, 29(13), 2010. -- *Reuses / builds on:* M6 (SE(3)), item 18 (Quaternion). - -### Tier 5 — Hard / research-grade ★★★★★ - -**M27. Time-optimal path parameterization (TOPP).** Minimum-time traversal of a fixed geometric path subject to joint torque/velocity limits. -- *Algorithm / paper:* J. Bobrow, S. Dubowsky, J. Gibson, "Time-Optimal Control of Robotic Manipulators Along Specified Paths," *IJRR*, 4(3), 1985; Q.-C. Pham, TOPP-RA, *IEEE T-RO*, 2014. -- *Reuses / builds on:* RNEA (torque limits along path), M10 (path), new `trajectory/` module. - ---- - ## Dependency notes Implement prerequisites first to avoid rework: @@ -462,15 +293,6 @@ Implement prerequisites first to avoid rework: - **15 Biquad** → 16 notch/comb, 17 lead-lag, 45 IIR design - **DARE (exists) + 29 + 31** → 46 H∞ -Manipulator build order: - -- **M1 generic joint** → mixed revolute/prismatic FK, IK, RNEA -- **M6 SE(3)** → M7 DH, M15 POE, M10 Cartesian paths, M26 continuum -- **M8 spatial Jacobian** → M11 manipulability, M13 pose IK, M14 redundancy, M17 impedance, M18 operational-space, M19 hybrid force -- **RNEA (exists)** → M12 computed torque, M16 collision observer, M20 adaptive, M22 identification, M27 TOPP -- **M12 → M17 → M18 → M19** (increasing contact-control capability) -- **M8 + M14** → M24 mobile manipulator - ```mermaid graph LR Q18[18 Quaternion] --> A33[33 AHRS] @@ -492,25 +314,6 @@ graph LR L31 --> H46 ``` -Manipulator prerequisite chain: - -```mermaid -graph LR - RNEA[(RNEA exists)] --> CTC[M12 computed torque] - J1[M1 generic joint] --> RNEA - SE3[M6 SE3] --> DH[M7 DH] - SE3 --> POE[M15 POE] - JAC[M8 6xN Jacobian] --> MAN[M11 manipulability] - JAC --> PIK[M13 pose IK] - JAC --> RED[M14 redundancy] - JAC --> IMP[M17 impedance] - CTC --> IMP - IMP --> OSC[M18 op-space] - OSC --> HYB[M19 hybrid force] - RED --> MOB[M24 mobile manip] - RNEA --> TOPP[M27 TOPP] -``` - ## Evaluation & metrics primitives Reusable quantities that quantify *how well* an algorithm behaves — the properties the per-family @@ -551,12 +354,10 @@ each mirroring the existing header-library + `test/` + `doc/` layout: - `numerical/robust_control/` → items 34, 35, 36, 46 (namespace `robust_control`) - `numerical/nonlinear_control/` → items 40, 41, 47 (namespace `nonlinear_control`) -- `numerical/trajectory/` → items M2, M3, M9, M10, M27 (namespace `trajectory`) -- `numerical/controllers/manipulator/` → items M5, M12, M17, M18, M19, M20, M25 (namespace `controllers`) -Existing `numerical/kinematics/` and `numerical/dynamics/` are **extended in place** for the -remaining manipulator items (generic joints, DH, spatial Jacobian, POE, parallel/mobile/continuum -kinematics) rather than adding new top-level folders. +Robot-manipulator kinematics, dynamics, trajectory generation, and manipulator control now live in +[robotics-toolbox-cpp](https://github.com/embedded-pro/robotics-toolbox-cpp), which consumes this +library via `FetchContent`. ## Per-component implementation checklist diff --git a/doc/dynamics/ArticulatedBodyAlgorithm.md b/doc/dynamics/ArticulatedBodyAlgorithm.md deleted file mode 100644 index fa3a29cc..00000000 --- a/doc/dynamics/ArticulatedBodyAlgorithm.md +++ /dev/null @@ -1,96 +0,0 @@ -# Articulated Body Algorithm (ABA) - -## Overview & Motivation - -The Articulated Body Algorithm (ABA) is the most efficient method for computing **forward dynamics** of serial kinematic chains. Given joint positions $q$, velocities $\dot{q}$, and applied torques $\tau$, it computes joint accelerations $\ddot{q}$ in $O(n)$ time — linear in the number of links. - -This is the forward-dynamics counterpart of the [Recursive Newton-Euler Algorithm](RecursiveNewtonEuler.md) (which solves inverse dynamics). Together, they form the two fundamental $O(n)$ algorithms in rigid-body dynamics. - -Alternatives like Euler-Lagrange require explicitly forming and inverting the $n \times n$ mass matrix ($O(n^3)$), making ABA significantly faster for chains with more than 3-4 links. - -## Mathematical Theory - -### Problem Statement - -Given a serial chain of $n$ rigid bodies connected by revolute joints, find: - -$$\ddot{q} = M(q)^{-1} [\tau - C(q, \dot{q})\dot{q} - g(q)]$$ - -ABA computes this without ever forming or inverting $M$. - -### Algorithm Structure - -ABA has three passes over the kinematic chain: - -1. **Forward pass** (base → tip): Compute link kinematics — angular velocities, linear velocities, and velocity-product accelerations from joint positions and velocities. - -2. **Backward pass** (tip → base): Compute articulated-body inertias $I^A_i$ and bias wrenches $p^A_i$ for each link. At each step, the joint projection removes one degree of freedom from the spatial inertia, and the remainder is propagated to the parent. - -3. **Forward pass** (base → tip): Resolve joint accelerations using the articulated-body quantities. Each joint acceleration $\ddot{q}_i$ is computed from the projected articulated inertia and the transformed parent acceleration. - -### Key Formulas - -**Articulated-body inertia** starts as the rigid-body spatial inertia of each link, then accumulates child contributions: - -$$I^A_i \leftarrow I^A_i + X^*_{i+1} \hat{I}^A_{i+1} X_{i+1}$$ - -where $\hat{I}^A$ is the inertia after removing the joint DOF (the "downdate"): - -$$\hat{I}^A_i = I^A_i - \frac{U_i U_i^T}{D_i}$$ - -with $U_i = I^A_i s_i$, $D_i = s_i^T U_i$, and $s_i$ the joint motion subspace (joint axis for revolute joints). - -**Joint acceleration**: - -$$\ddot{q}_i = \frac{u_i - U_i^T \hat{a}_i}{D_i}$$ - -where $u_i = \tau_i - s_i^T p^A_i$ and $\hat{a}_i$ is the spatial acceleration contribution from the parent. - -## Complexity Analysis - -| Case | Time | Space | Notes | -|------|--------|--------|------------------------------------| -| All | $O(n)$ | $O(n)$ | Three linear passes over the chain | - -Each pass visits every link exactly once, performing constant-time spatial algebra (3×3 matrix operations) per link. - -## Step-by-Step Walkthrough - -Consider a 2-link planar arm with unit masses, unit lengths, Y-axis joints, zero velocities, and zero applied torques under gravity $g = [0, 0, -9.81]^T$. - -**Pass 1** — Forward kinematics: both links at $q = [0, 0]^T$ with $\dot{q} = [0, 0]^T$, so all velocities and velocity-product terms are zero. - -**Pass 2** — Backward pass: starting from link 2, compute the rigid-body inertia, project out the joint DOF, transform to link 1's frame, and accumulate. The articulated inertia at the base now represents the effective inertia of the entire chain. - -**Pass 3** — Forward pass: at the base, the gravitational acceleration is transformed into the base frame and used to compute $\ddot{q}_1$. The resulting acceleration is propagated to link 2 to compute $\ddot{q}_2$. - -The result: both joints accelerate downward due to gravity, with the base joint carrying the larger moment from the full chain. - -## Pitfalls & Edge Cases - -- **Singular configurations**: When $D_i \to 0$, the joint becomes locked. This should not happen for well-formed revolute joint models with positive inertia. -- **Numerical precision**: The division by $D_i$ amplifies errors if $D_i$ is small. Use `float` (not fixed-point) for dynamics computations. -- **Floating-point only**: The algorithm involves trigonometric functions, divisions, and large dynamic ranges that are unsuitable for Q15/Q31 fixed-point. - -## Variants & Generalizations - -- **Composite Rigid Body Algorithm (CRBA)**: Computes the mass matrix $M(q)$ explicitly in $O(n^2)$. Useful when $M$ itself is needed (e.g., for operational-space control). -- **Extended ABA**: Supports branching chains (trees) by processing children before parents in the backward pass. -- **ABA with friction**: Joint friction torques can be added directly to $\tau$. - -## Applications - -- Real-time simulation of robot arms and multi-body systems -- Model-based control (computed torque, impedance control) -- Forward integration of manipulator dynamics - -## Connections to Other Algorithms - -- [Recursive Newton-Euler](RecursiveNewtonEuler.md): solves the inverse problem ($q, \dot{q}, \ddot{q} \to \tau$). ABA and RNEA are duals: `RNEA(q, qDot, ABA(q, qDot, tau)) ≈ tau`. -- [Euler-Lagrange](EulerLagrange.md): equivalent $O(n^3)$ formulation using the mass matrix. -- [Gaussian Elimination](../solvers/GaussianElimination.md): used if the mass matrix approach is taken instead. - -## References & Further Reading - -- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. Chapter 7. -- Featherstone, R. (1983). "The Calculation of Robot Dynamics Using Articulated-Body Inertias." *International Journal of Robotics Research*, 2(1), 13–30. diff --git a/doc/dynamics/EulerLagrange.md b/doc/dynamics/EulerLagrange.md deleted file mode 100644 index 93c16fd5..00000000 --- a/doc/dynamics/EulerLagrange.md +++ /dev/null @@ -1,145 +0,0 @@ -# Euler-Lagrange Dynamics - -## Overview & Motivation - -The Euler-Lagrange equations provide a systematic way to derive the equations of motion for mechanical systems from energy principles. Instead of tracking forces on each body (as in Newton's method), they express dynamics in terms of *generalized coordinates* $q$ and the system's kinetic and potential energy. - -For a robotic manipulator or any articulated mechanism with $n$ degrees of freedom, the Euler-Lagrange formulation yields a compact matrix equation: - -$$M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau$$ - -Where: -- $q \in \mathbb{R}^n$ — generalized coordinates (joint angles/positions) -- $\dot{q}, \ddot{q}$ — generalized velocities and accelerations -- $M(q) \in \mathbb{R}^{n \times n}$ — mass (inertia) matrix, symmetric positive definite -- $C(q, \dot{q})\dot{q} \in \mathbb{R}^n$ — Coriolis and centrifugal terms -- $g(q) \in \mathbb{R}^n$ — gravitational terms -- $\tau \in \mathbb{R}^n$ — generalized forces/torques (inputs) - -This formulation supports two fundamental computations: -- **Forward dynamics**: given torques $\tau$, compute accelerations $\ddot{q} = M^{-1}(\tau - C\dot{q} - g)$ -- **Inverse dynamics**: given desired accelerations $\ddot{q}$, compute required torques $\tau = M\ddot{q} + C\dot{q} + g$ - -## Mathematical Theory - -### Prerequisites - -The **Lagrangian** of a system is defined as: - -$$\mathcal{L}(q, \dot{q}) = T(q, \dot{q}) - V(q)$$ - -where $T$ is kinetic energy and $V$ is potential energy. - -### Core Definitions - -The **Euler-Lagrange equation** for coordinate $q_i$ is: - -$$\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}_i} - \frac{\partial \mathcal{L}}{\partial q_i} = \tau_i \quad \text{for } i = 1, \ldots, n$$ - -For a rigid-body system, kinetic energy takes the form: - -$$T = \frac{1}{2}\dot{q}^T M(q) \dot{q}$$ - -Applying the Euler-Lagrange equation to this quadratic form yields the standard manipulator equation: - -$$M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau$$ - -### Mass Matrix $M(q)$ - -The mass matrix is always **symmetric** ($M = M^T$) and **positive definite** ($\dot{q}^T M \dot{q} > 0$ for $\dot{q} \neq 0$). These properties guarantee: -- The forward dynamics linear system $M\ddot{q} = f$ always has a unique solution -- Physical kinetic energy is always non-negative - -### Coriolis Matrix $C(q, \dot{q})$ - -The Coriolis matrix captures velocity-dependent forces using **Christoffel symbols of the first kind**: - -$$c_{ij} = \sum_{k=1}^{n} \frac{1}{2}\left(\frac{\partial m_{ij}}{\partial q_k} + \frac{\partial m_{ik}}{\partial q_j} - \frac{\partial m_{jk}}{\partial q_i}\right)\dot{q}_k$$ - -A key property: $\dot{M} - 2C$ is skew-symmetric, which is important for passivity-based control. - -### Gravity Vector $g(q)$ - -$$g_i(q) = \frac{\partial V}{\partial q_i}$$ - -where $V(q)$ is the total potential energy of the system. - -## Complexity Analysis - -| Operation | Time | Space | Notes | -|------------------|-----------------|----------|------------------------------------------------------| -| Forward dynamics | $O(n^3)$ | $O(n^2)$ | Dominated by $M^{-1}$ solve via Gaussian elimination | -| Inverse dynamics | $O(n^2)$ | $O(n^2)$ | Matrix-vector multiplication $M \ddot{q}$ | -| Model evaluation | Model-dependent | $O(n^2)$ | Computing $M(q)$, $C(q,\dot{q})$, $g(q)$ | - -For systems with $n \leq 6$ DOF (typical robotic manipulators), the $O(n^3)$ cost of Gaussian elimination is negligible. For larger systems, consider the Recursive Newton-Euler Algorithm (RNEA) which achieves $O(n)$ inverse dynamics. - -## Step-by-Step Walkthrough - -### Example: Simple Pendulum (1-DOF) - -A point mass $m$ on a rigid rod of length $l$: - -**System parameters:** $m = 1\,\text{kg}$, $l = 1\,\text{m}$, $g_0 = 9.81\,\text{m/s}^2$ - -**Model matrices:** -- $M(q) = ml^2 = 1.0$ -- $C(q, \dot{q})\dot{q} = 0$ (single DOF → no Coriolis) -- $g(q) = mgl\sin(q) = 9.81\sin(q)$ - -**Forward dynamics** at $q = 0.5\,\text{rad}$, $\dot{q} = 0$, $\tau = 0$: - -$$\ddot{q} = M^{-1}(\tau - C\dot{q} - g) = \frac{0 - 0 - 9.81\sin(0.5)}{1.0} = -4.703\,\text{rad/s}^2$$ - -**Inverse dynamics** for holding position ($\ddot{q} = 0$): - -$$\tau = M\ddot{q} + C\dot{q} + g = 0 + 0 + 9.81\sin(0.5) = 4.703\,\text{N·m}$$ - -### Example: Two-Link Planar Arm (2-DOF) - -For a 2-DOF arm with equal links ($m_1 = m_2 = 1\,\text{kg}$, $l_1 = l_2 = 1\,\text{m}$): - -**At rest** ($q = [0, 0]^T$, $\dot{q} = [0, 0]^T$): -- $\sin(q_1) = 0$, $\sin(q_1 + q_2) = 0$ -- $g = [0, 0]^T$ → gravity terms vanish when links hang vertically -- Forward dynamics with $\tau = 0$ produces $\ddot{q} = [0, 0]^T$ (equilibrium) - -## Pitfalls & Edge Cases - -- **Singular mass matrix**: Should not happen for physically valid systems, but numerical conditioning degrades near kinematic singularities (e.g., fully extended arm). The Gaussian elimination solver uses partial pivoting to help. -- **Fixed-point arithmetic**: Dynamics values (torques, accelerations) typically exceed the $[-1, 1)$ range of Q15/Q31 formats. Use `float` unless inputs are carefully scaled. -- **Coriolis computation**: The choice of Coriolis parameterization (Christoffel vs. factored form) affects the skew-symmetry property of $\dot{M} - 2C$. The interface returns the product $C(q,\dot{q})\dot{q}$ directly, leaving the parameterization choice to the user. -- **Energy consistency**: For simulation, ensure the model matrices satisfy the skew-symmetry property; otherwise, energy may not be conserved in numerical integration. -- **Units**: Be explicit about whether angles are in radians (standard) and torques in N·m. - -## Variants & Generalizations - -| Variant | Trade-off | Use Case | -|--------------------------------------|------------------------------------------|------------------------------------------------------| -| **Euler-Lagrange** (this) | $O(n^3)$ forward, closed-form, intuitive | Small systems ($n \leq 6$), control design, analysis | -| **Newton-Euler** | Body-by-body force balance | Single rigid bodies, intuitive for simple systems | -| **Recursive Newton-Euler (RNEA)** | $O(n)$ inverse dynamics | Large kinematic chains, real-time control | -| **Articulated Body Algorithm (ABA)** | $O(n)$ forward dynamics | Large chains, simulation | -| **Hamiltonian formulation** | Phase-space, symplectic integration | Long-horizon simulation, energy preservation | - -## Applications - -- **Robot control**: Computed torque control, gravity compensation, impedance control -- **Simulation**: Forward dynamics integration for physics simulation -- **System identification**: Estimate dynamic parameters from measured motion -- **Trajectory optimization**: Inverse dynamics as constraints in optimization -- **Vibration analysis**: Linearized dynamics around equilibrium points - -## Connections to Other Algorithms - -- **Gaussian Elimination** ([GaussianElimination.md](../solvers/GaussianElimination.md)): Used internally to solve $M\ddot{q} = f$ in forward dynamics -- **LQR Controller** ([Lqr.md](../controllers/Lqr.md)): Euler-Lagrange dynamics can be linearized to produce the $(A, B)$ state-space matrices needed for LQR design -- **Kalman Filter** (../filters/active/): State estimation for partially observed dynamical systems derived from Euler-Lagrange models -- **DARE** ([DiscreteAlgebraicRiccatiEquation.md](../solvers/DiscreteAlgebraicRiccatiEquation.md)): Discretized Euler-Lagrange systems feed into DARE for optimal control gain computation - -## References & Further Reading - -- Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapters 7–8. -- Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2020). *Robot Modeling and Control* (2nd ed.). Wiley. Chapter 6. -- Murray, R. M., Li, Z., & Sastry, S. S. (1994). *A Mathematical Introduction to Robotic Manipulation*. CRC Press. -- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. (For RNEA and ABA extensions.) diff --git a/doc/dynamics/NewtonEuler.md b/doc/dynamics/NewtonEuler.md deleted file mode 100644 index e0d3e3d5..00000000 --- a/doc/dynamics/NewtonEuler.md +++ /dev/null @@ -1,140 +0,0 @@ -# Newton-Euler Dynamics - -## Overview & Motivation - -The Newton-Euler formulation describes the dynamics of a single rigid body by directly applying Newton's second law for translational motion and Euler's equation for rotational motion. Unlike the Euler-Lagrange approach which works in generalized (joint-space) coordinates, Newton-Euler operates in Cartesian space, computing forces and torques on individual bodies. - -In the **body-fixed reference frame**, the equations are: - -$$F = m(\dot{v} + \omega \times v)$$ -$$\tau = I\dot{\omega} + \omega \times (I\omega)$$ - -Where: -- $F \in \mathbb{R}^3$ — applied force in the body frame -- $\tau \in \mathbb{R}^3$ — applied torque in the body frame -- $m$ — body mass (scalar) -- $I \in \mathbb{R}^{3 \times 3}$ — inertia tensor in the body frame -- $v, \dot{v} \in \mathbb{R}^3$ — linear velocity and acceleration in the body frame -- $\omega, \dot{\omega} \in \mathbb{R}^3$ — angular velocity and acceleration in the body frame -- $\omega \times v$ — Coriolis term due to body-frame formulation -- $\omega \times (I\omega)$ — gyroscopic coupling term - -This supports two computations: -- **Forward dynamics**: given forces/torques, compute linear and angular accelerations -- **Inverse dynamics**: given desired accelerations, compute required forces/torques - -## Mathematical Theory - -### Newton's Second Law (Translation) - -In an inertial frame, Newton's second law is simply $F = ma$. In a body-fixed (rotating) frame, the transport theorem introduces an additional term: - -$$F = m(\dot{v} + \omega \times v)$$ - -The $\omega \times v$ term is the Coriolis acceleration that arises because the reference frame itself is rotating. - -**Forward**: $\dot{v} = F/m - \omega \times v$ - -**Inverse**: $F = m(\dot{v} + \omega \times v)$ - -### Euler's Equation (Rotation) - -Euler's equation governs rotational dynamics: - -$$\tau = I\dot{\omega} + \omega \times (I\omega)$$ - -The $\omega \times (I\omega)$ term is the **gyroscopic torque** — it couples different rotational axes when the inertia tensor is non-spherical. For a body spinning about a non-principal axis, this term causes precession even without external torque. - -**Forward**: $\dot{\omega} = I^{-1}(\tau - \omega \times (I\omega))$ - -This requires solving the $3 \times 3$ linear system $I\dot{\omega} = \tau - \omega \times (I\omega)$ via Gaussian elimination. - -**Inverse**: $\tau = I\dot{\omega} + \omega \times (I\omega)$ - -This is a direct matrix-vector multiply plus cross product — no linear solve needed. - -### Properties of the Inertia Tensor - -The inertia tensor $I$ is: -- **Symmetric**: $I = I^T$ (6 independent components, not 9) -- **Positive definite**: $\omega^T I \omega > 0$ for $\omega \neq 0$ -- **Constant in body frame**: For a rigid body, $I$ does not change with the body's orientation when expressed in the body frame - -When $I$ is diagonal (principal axes frame), components are the **principal moments of inertia** $I_{xx}, I_{yy}, I_{zz}$, and Euler's equation simplifies to the well-known form: - -$$\tau_x = I_{xx}\dot{\omega}_x + (I_{zz} - I_{yy})\omega_y\omega_z$$ -$$\tau_y = I_{yy}\dot{\omega}_y + (I_{xx} - I_{zz})\omega_z\omega_x$$ -$$\tau_z = I_{zz}\dot{\omega}_z + (I_{yy} - I_{xx})\omega_x\omega_y$$ - -## Complexity Analysis - -| Operation | Time | Space | Notes | -|------------------|--------|--------|------------------------------------------------------| -| Forward dynamics | $O(1)$ | $O(1)$ | Fixed 3×3 system; Gaussian elimination on 3×3 matrix | -| Inverse dynamics | $O(1)$ | $O(1)$ | Matrix-vector multiply + cross products | -| Cross product | $O(1)$ | $O(1)$ | 6 multiplications and 3 subtractions | - -All operations are constant-time since the spatial dimension is fixed at 3. The $3 \times 3$ Gaussian elimination is effectively a small constant cost. - -## Step-by-Step Walkthrough - -### Example: Uniform Sphere - -**Parameters:** mass $m = 2\,\text{kg}$, radius $r = 0.5\,\text{m}$ - -**Inertia tensor:** $I = \frac{2}{5}mr^2 \cdot \mathbf{I}_3 = 0.2 \cdot \mathbf{I}_3$ - -**Forward dynamics** with $F = [6, 0, 0]^T\,\text{N}$, $\tau = [0, 0, 1]^T\,\text{N·m}$, $v = \omega = 0$: - -$$\dot{v} = F/m - 0 = [3, 0, 0]^T \,\text{m/s}^2$$ -$$\dot{\omega} = I^{-1}\tau = [0, 0, 5]^T \,\text{rad/s}^2$$ - -### Example: Gyroscopic Precession - -**Asymmetric body** with $I = \text{diag}(1, 2, 3)\,\text{kg·m}^2$ - -Spinning at $\omega = [1, 1, 0]^T\,\text{rad/s}$ with **no external torque**: - -$$I\omega = [1, 2, 0]^T$$ -$$\omega \times (I\omega) = [1,1,0] \times [1,2,0] = [0, 0, 1]^T$$ -$$\dot{\omega} = I^{-1}(0 - [0,0,1]) = [0, 0, -\tfrac{1}{3}]^T \,\text{rad/s}^2$$ - -The body accelerates about the z-axis despite no external torque — this is the gyroscopic coupling effect due to asymmetric inertia. - -## Pitfalls & Edge Cases - -- **Singular inertia**: An inertia tensor must be positive definite. Zero or negative eigenvalues indicate a non-physical body definition. -- **Body-frame vs. inertial frame**: The equations assume all quantities (forces, torques, velocities) are expressed in the body-fixed frame. Users must transform between frames externally. -- **Spherical inertia**: When $I = c \cdot \mathbf{I}_3$, the gyroscopic term $\omega \times (I\omega) = c(\omega \times \omega) = 0$, simplifying Euler's equation to $\tau = I\dot{\omega}$. -- **Fixed-point arithmetic**: Like Euler-Lagrange, Newton-Euler involves physical quantities (forces in Newtons, torques in N·m) that typically exceed Q15/Q31 range. Use `float`. -- **Energy conservation**: For torque-free motion, kinetic energy $T = \frac{1}{2}\omega^T I \omega$ and angular momentum $L = I\omega$ magnitude should be conserved. Numerical integration may violate this. - -## Variants & Generalizations - -| Variant | Key Difference | Use Case | -|-----------------------------------|----------------------------------------|---------------------------------------------| -| **Newton-Euler (this)** | Single rigid body, Cartesian space | Spacecraft attitude, single-body simulation | -| **Recursive Newton-Euler (RNEA)** | Multi-body recursive algorithm, $O(n)$ | Robot inverse dynamics, real-time control | -| **Euler-Lagrange** | Generalized coordinates, joint space | Control design, analytical derivations | -| **Spatial vector algebra** | 6D twist/wrench representation | Compact multi-body formulations | - -## Applications - -- **Spacecraft attitude control**: Reaction wheel and thruster control using Euler's equations -- **Drone / UAV dynamics**: Quadrotor translational and rotational dynamics -- **Projectile dynamics**: Spinning projectile with aerodynamic forces -- **Gyroscope modeling**: Precession and nutation analysis -- **Foundation for RNEA**: Body-level Newton-Euler equations form the building block for recursive multi-body algorithms - -## Connections to Other Algorithms - -- **Gaussian Elimination** ([GaussianElimination.md](../solvers/GaussianElimination.md)): Used to solve the $3 \times 3$ inertia system $I\dot{\omega} = \tau_{net}$ in forward dynamics -- **Euler-Lagrange** ([EulerLagrange.md](EulerLagrange.md)): Joint-space counterpart; Euler-Lagrange derives the same dynamics from energy principles in generalized coordinates -- **Recursive Newton-Euler (RNEA)**: Extends single-body Newton-Euler to kinematic chains by propagating velocities/accelerations forward and forces/torques backward through the chain - -## References & Further Reading - -- Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 7. -- Goldstein, H., Poole, C., & Safko, J. (2002). *Classical Mechanics* (3rd ed.). Chapters 4–5. -- Hughes, P. C. (1986). *Spacecraft Attitude Dynamics*. Dover Publications. -- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. Chapter 2. diff --git a/doc/dynamics/README.md b/doc/dynamics/README.md deleted file mode 100644 index b30e802c..00000000 --- a/doc/dynamics/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# Dynamics & Modeling - -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 | -| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | -| [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 deleted file mode 100644 index dca5e330..00000000 --- a/doc/dynamics/RecursiveNewtonEuler.md +++ /dev/null @@ -1,141 +0,0 @@ -# Recursive Newton-Euler Algorithm (RNEA) - -## Overview & Motivation - -The Recursive Newton-Euler Algorithm (RNEA) is the most efficient method for computing **inverse dynamics** of serial kinematic chains (robot arms, manipulators). Given joint positions, velocities, and desired accelerations, it computes the required joint torques in $O(n)$ time — linear in the number of links. - -This contrasts with the Euler-Lagrange approach, which requires explicitly forming and multiplying $n \times n$ matrices ($O(n^3)$). For a 6-DOF robot arm, RNEA is roughly 10× faster than the matrix-based approach. - -The algorithm has two passes over the kinematic chain: - -1. **Forward pass** (base → tip): propagate angular velocities, angular accelerations, and linear accelerations from link to link using kinematic relationships -2. **Backward pass** (tip → base): accumulate forces and torques from the tip back to the base using Newton-Euler equations on each link - -The joint torque at each joint is then the projection of the net torque onto the joint axis. - -## Mathematical Theory - -### Kinematic Chain Model - -A serial chain of $n$ rigid bodies (links) connected by revolute 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 -- $\hat{z}_i \in \mathbb{R}^3$ — joint axis (unit vector in the link frame) -- $r_{p \to j}^{(i)} \in \mathbb{R}^3$ — position of joint $i$ in the parent (link $i-1$) frame -- $r_{j \to c}^{(i)} \in \mathbb{R}^3$ — position of the center of mass in the link $i$ frame - -The rotation matrix $R_i(q_i)$ transforms vectors from link $i$ frame to parent frame, computed via Rodrigues' formula from the joint angle $q_i$ and joint axis $\hat{z}_i$. - -### Forward Pass — Velocity and Acceleration Propagation - -For link $i$, given parent link $i-1$ quantities: - -**Angular velocity:** -$$\omega_i = R_i^T \omega_{i-1} + \dot{q}_i \hat{z}_i$$ - -**Angular acceleration:** -$$\dot{\omega}_i = R_i^T \dot{\omega}_{i-1} + \ddot{q}_i \hat{z}_i + \omega_i \times (\dot{q}_i \hat{z}_i)$$ - -**Linear acceleration at joint origin of link $i$:** -$$a_{J_i} = R_i^T \left( a_{J_{i-1}} + \dot{\omega}_{i-1} \times r_{p \to j}^{(i)} + \omega_{i-1} \times (\omega_{i-1} \times r_{p \to j}^{(i)}) \right)$$ - -**Linear acceleration at center of mass:** -$$a_{C_i} = a_{J_i} + \dot{\omega}_i \times r_{j \to c}^{(i)} + \omega_i \times (\omega_i \times r_{j \to c}^{(i)})$$ - -For the base link ($i = 0$), the "parent" is the world frame with $\omega_{-1} = 0$ and $a_{J_{-1}} = -g$ (gravity expressed as a base acceleration, a standard RNEA convention). - -### Backward Pass — Force and Torque Accumulation - -For each link $i$, starting from the tip: - -**Net force on link $i$:** -$$f_i = m_i a_{C_i} + \sum_{\text{children } j} R_j f_j$$ - -**Net torque on link $i$ (about joint):** -$$\tau_i = I_i \dot{\omega}_i + \omega_i \times (I_i \omega_i) + r_{j \to c}^{(i)} \times (m_i a_{C_i}) + \sum_{\text{children } j} \left( R_j \tau_j + r_{p \to j}^{(j)} \times R_j f_j \right)$$ - -**Joint torque:** -$$\tau_{q_i} = \hat{z}_i^T \tau_i$$ - -### Rodrigues' Rotation Formula - -The rotation matrix for angle $\theta$ about unit axis $\hat{u} = [u_x, u_y, u_z]^T$: - -$$R = \cos\theta \cdot \mathbf{I}_3 + (1 - \cos\theta) \hat{u}\hat{u}^T + \sin\theta [\hat{u}]_\times$$ - -where $[\hat{u}]_\times$ is the skew-symmetric matrix of $\hat{u}$. - -## Complexity Analysis - -| Operation | Time | Space | Notes | -|------------------------|--------|--------|-------------------------------------------------| -| Inverse dynamics | $O(n)$ | $O(n)$ | Two linear passes over $n$ links | -| Per-link forward pass | $O(1)$ | $O(1)$ | Fixed 3×3 matrix-vector operations | -| Per-link backward pass | $O(1)$ | $O(1)$ | Cross products and additions | -| Rotation matrix | $O(1)$ | $O(1)$ | Rodrigues' formula: trig functions + 3×3 matrix | - -Compared to Euler-Lagrange $O(n^3)$ inverse dynamics, RNEA is dramatically faster for large $n$. For $n = 6$ (typical robot arm), RNEA performs roughly 780 floating-point operations vs. ~14,000 for the matrix approach. - -## Step-by-Step Walkthrough - -### Example: 1-DOF Pendulum - -A single thin rod of mass $m = 2\,\text{kg}$ and length $l = 1\,\text{m}$, rotating about the z-axis. Joint at origin, CoM at $[l/2, 0, 0]^T$. - -**Inertia at CoM:** $I = ml^2/12 = 0.167\,\text{kg·m}^2$ (about y and z axes) - -**Setup:** $q = 0$, $\dot{q} = 0$, $\ddot{q} = 1\,\text{rad/s}^2$, zero gravity. - -**Forward pass:** -- $\omega_0 = [0, 0, 0]^T + 0 \cdot [0, 0, 1]^T = [0, 0, 0]^T$ -- $\dot{\omega}_0 = [0, 0, 1]^T$ (from $\ddot{q}$) -- $a_{C_0} = 0 + [0, 0, 1] \times [0.5, 0, 0] + 0 = [0, 0.5, 0]^T\,\text{m/s}^2$ - -**Backward pass:** -- $f_0 = 2 \cdot [0, 0.5, 0]^T = [0, 1, 0]^T\,\text{N}$ -- $\tau_0 = I \cdot [0, 0, 1]^T + 0 + [0.5, 0, 0] \times [0, 1, 0]^T$ -- $= [0, 0, 0.167]^T + [0, 0, 0.5]^T = [0, 0, 0.667]^T$ -- $\tau_q = [0, 0, 1]^T \cdot [0, 0, 0.667]^T = 0.667\,\text{N·m}$ - -This matches $I_{\text{end}} \cdot \ddot{q} = \frac{ml^2}{3} \cdot 1 = 0.667$. - -## Pitfalls & Edge Cases - -- **Joint axis convention**: The joint axis must be a unit vector. Non-unit vectors will produce incorrect rotation matrices and torque projections. -- **Frame consistency**: All link parameters (inertia, CoM position, joint axis) must be expressed in the link's own body frame. Mixing world-frame and body-frame quantities is a common source of bugs. -- **Gravity convention**: Gravity enters as $a_0 = -g$ (negative) in the base acceleration. This "fictitious force" convention avoids special-casing gravity in each link's force calculation. -- **Serial chains only**: This implementation assumes a single serial chain with no branches. Tree-structured or closed-chain mechanisms require extensions. -- **Numerical precision**: For long chains, the recursive propagation can accumulate floating-point errors. For $n \leq 10$, this is typically negligible. -- **Fixed-point arithmetic**: Like other dynamics algorithms, physical quantities exceed Q15/Q31 range. Use `float`. - -## Variants & Generalizations - -| Variant | Key Difference | Use Case | -|--------------------------------------|---------------------------------------|----------------------------------------------------| -| **RNEA (this)** | $O(n)$ inverse dynamics, serial chain | Real-time robot control, computed torque | -| **RNEA + mass matrix** | Call RNEA $n+1$ times to build $M(q)$ | Forward dynamics via $\ddot{q} = M^{-1}(\tau - h)$ | -| **Articulated Body Algorithm (ABA)** | $O(n)$ forward dynamics directly | Simulation without forming $M$ | -| **Extended RNEA** | Includes friction, motor inertia | Realistic robot modeling | -| **RNEA for trees** | Handles branching chains | Humanoid robots, multi-fingered hands | - -## Applications - -- **Computed torque control**: RNEA provides the feedforward torque $\tau = M\ddot{q}_d + h(q, \dot{q})$ for tracking controllers at each control cycle -- **Gravity compensation**: RNEA with $\dot{q} = \ddot{q} = 0$ yields gravity torques directly -- **Simulation**: Build the mass matrix column-by-column using $n$ RNEA calls, then solve $M\ddot{q} = \tau - h$ for forward dynamics -- **Model-based friction estimation**: Compare RNEA-predicted torques with measured motor currents -- **Real-time control loops**: The $O(n)$ complexity makes RNEA suitable for 1 kHz+ control rates - -## Connections to Other Algorithms - -- **Newton-Euler** ([NewtonEuler.md](NewtonEuler.md)): RNEA applies the single-body Newton-Euler equations recursively across the chain. The per-link force/torque computation is identical. -- **Euler-Lagrange** ([EulerLagrange.md](EulerLagrange.md)): Both compute the same dynamics ($M\ddot{q} + C\dot{q} + g = \tau$) but RNEA is $O(n)$ vs. $O(n^3)$. RNEA is preferred for computation; Euler-Lagrange for analytical derivation. -- **Gaussian Elimination** ([GaussianElimination.md](../solvers/GaussianElimination.md)): When using RNEA for forward dynamics, the mass matrix built via RNEA is solved with Gaussian elimination. - -## References & Further Reading - -- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. Chapters 3, 5. -- Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 7. -- Luh, J. Y. S., Walker, M. W., & Paul, R. P. C. (1980). On-line computational scheme for mechanical manipulators. *Journal of Dynamic Systems, Measurement, and Control*, 102(2), 69–76. -- Craig, J. J. (2005). *Introduction to Robotics: Mechanics and Control* (3rd ed.). Pearson. Chapter 6. diff --git a/doc/kinematics/ForwardKinematics.md b/doc/kinematics/ForwardKinematics.md deleted file mode 100644 index 9a892522..00000000 --- a/doc/kinematics/ForwardKinematics.md +++ /dev/null @@ -1,71 +0,0 @@ -# Forward Kinematics - -## Overview & Motivation - -Forward Kinematics computes the 3D Cartesian positions of all joints in a serial kinematic chain, given the joint angles. For a chain of $n$ revolute joints, it produces $n + 1$ position vectors (base through end-effector) in $O(n)$ time. - -This is the geometric foundation for visualization, collision detection, and workspace analysis. It answers the question: "given these joint angles, where is each joint in world space?" - -## Mathematical Theory - -### Position Computation - -Starting from the base at the origin, the position of joint $i + 1$ is: - -$$p_{i+1} = p_i + R_{\text{world} \to i} \cdot r_{i \to i+1}$$ - -where: - -- $p_i$ is the position of joint $i$ in world coordinates -- $R_{\text{world} \to i} = \prod_{k=0}^{i} R_k(q_k)$ is the accumulated rotation from world to link $i$ -- $r_{i \to i+1}$ is the offset from joint $i$ to joint $i + 1$ in the link $i$ frame - -Each rotation $R_k(q_k)$ is computed via Rodrigues' formula from the joint axis and angle. - -### End-Effector Approximation - -For the last link, the offset to the end-effector is approximated as $2 \cdot r_{j \to \text{CoM}}$ (assuming the center of mass is at the midpoint of the link). - -## Complexity Analysis - -| Case | Time | Space | Notes | -|------|--------|--------|------------------------| -| All | $O(n)$ | $O(n)$ | Single pass over chain | - -## Variants & Generalizations - -- **Planar vs. spatial chains**: In purely planar manipulators, all joint axes are parallel and rotations reduce to 2D trigonometry, but the algorithmic structure (one pass accumulating transforms) remains the same. -- **Prismatic joints**: For prismatic joints, $R_k(q_k)$ stays constant while $r_{i \to i+1}$ becomes a function of the joint displacement; the same forward sweep still applies. -- **Different parameterizations**: Denavit–Hartenberg, modified DH, or product-of-exponentials (PoE) formulations all map to the same core idea: recursively compose transforms along the chain. -- **Multiple end-effectors**: For branched chains, the same routine can be run per branch by choosing different terminal joints while reusing shared prefixes. - -## Applications - -- **Visualization**: Rendering joint frames and links in 3D for debugging controllers, planners, and estimators. -- **Collision detection & workspace analysis**: Computing link poses to test against environment geometry and to sample reachable workspaces. -- **Control & planning**: Providing end-effector pose and intermediate joint positions to inverse kinematics solvers, trajectory planners, and constraint checkers. -- **Dynamics algorithms**: Supplying link transforms to dynamics routines such as ABA and RNEA that require consistent kinematic states. -## Step-by-Step Walkthrough - -Consider a 2-link planar arm with link lengths $L_1 = 1$, $L_2 = 0.8$, Y-axis joints, and $q = [\pi/4, -\pi/6]$. - -1. $p_0 = [0, 0, 0]^T$ (base at origin) -2. $R_0 = R_y(\pi/4)$, link extent $= [1, 0, 0]^T$, so $p_1 = R_0 \cdot [1, 0, 0]^T = [0.707, 0, 0.707]^T$ -3. $R_{01} = R_0 \cdot R_y(-\pi/6)$, link extent $= [0.8, 0, 0]^T$, so $p_2 = p_1 + R_{01} \cdot [0.8, 0, 0]^T$ - -## Pitfalls & Edge Cases - -- **Zero-length links**: If `parentToJoint` and `jointToCoM` are both zero, consecutive joints collapse to the same position. -- **Floating-point only**: Uses trigonometric functions, unsuitable for fixed-point types. -- **Approximation at tip**: The end-effector position uses `jointToCoM * 2`, which is exact only for uniform-density links with the CoM at the geometric center. - -## Connections to Other Algorithms - -- [Articulated Body Algorithm](../dynamics/ArticulatedBodyAlgorithm.md): ABA's first pass performs a similar forward kinematics sweep to compute velocities and rotation matrices. -- [Recursive Newton-Euler](../dynamics/RecursiveNewtonEuler.md): RNEA's forward pass also propagates rotations along the chain. -- The shared rotation computation uses `math::RotationAboutAxis` from [Geometry3D](../math/Geometry3D.md). - -## References & Further Reading - -- Craig, J.J. (2005). *Introduction to Robotics: Mechanics and Control*. 3rd ed. Chapters 2–3. -- Siciliano, B. et al. (2009). *Robotics: Modelling, Planning and Control*. Chapter 2. diff --git a/doc/kinematics/InverseKinematics.md b/doc/kinematics/InverseKinematics.md deleted file mode 100644 index 307a871d..00000000 --- a/doc/kinematics/InverseKinematics.md +++ /dev/null @@ -1,122 +0,0 @@ -# Inverse Kinematics (Damped Least Squares) - -## Overview & Motivation - -Inverse Kinematics (IK) solves the problem dual to Forward Kinematics: given a desired 3D end-effector position $p_{\text{target}}$, find joint angles $q$ such that the end-effector reaches that position. This is the core computational problem in robot arm control, motion planning, and teleoperation. - -The **Damped Least Squares** (DLS) method, also known as the Levenberg-Marquardt approach, iteratively refines a joint-angle estimate by solving a small linear system at each step. Unlike the plain pseudoinverse, DLS adds a damping term $\lambda^2 I$ that keeps the system numerically stable even when the manipulator is near a singular configuration (such as a fully extended arm). - -## Mathematical Theory - -### Jacobian of a Serial Chain - -For a serial revolute-joint chain with $n$ joints, the **geometric Jacobian** $J \in \mathbb{R}^{3 \times n}$ maps joint velocities $\dot{q}$ to end-effector linear velocity: - -$$\dot{p}_{\text{ee}} = J(q) \, \dot{q}$$ - -Column $i$ of $J$ is the linear velocity contribution of joint $i$: - -$$J_i = a_i^w \times (p_{\text{ee}} - p_i)$$ - -where: -- $a_i^w = R_{0 \to i} \, a_i^{\text{link}}$ is the joint axis in world frame (accumulated rotation applied to the link-frame axis) -- $p_i$ is the world-frame position of joint $i$ (from Forward Kinematics) -- $p_{\text{ee}}$ is the current end-effector position - -### DLS Update Rule - -Define the position error: - -$$e = p_{\text{target}} - p_{\text{ee}}(q) \in \mathbb{R}^3$$ - -The DLS joint update per iteration is: - -$$\Delta q = J^\top \bigl(J J^\top + \lambda^2 I\bigr)^{-1} e$$ - -This is equivalent to solving the $3 \times 3$ linear system: - -$$\underbrace{\bigl(J J^\top + \lambda^2 I\bigr)}_{A} \, y = e, \quad \Delta q = J^\top y$$ - -where $A \in \mathbb{R}^{3 \times 3}$ is always symmetric positive definite (the damping $\lambda^2 I$ guarantees this), so `GaussianElimination` solves it reliably. - -The updated joint angles are: - -$$q_{k+1} = q_k + \Delta q$$ - -### Convergence - -The loop terminates when: -- $\|e\| < \epsilon_{\text{tol}}$ (success), or -- The maximum iteration count is reached (failure to converge — target unreachable or tolerance too tight) - -### Why DLS over Plain Pseudoinverse - -At a kinematic singularity (e.g., fully extended arm), $J J^\top$ becomes rank-deficient. The pseudoinverse $J^+ = J^\top (J J^\top)^{-1}$ would produce infinite joint velocities. The damping term $\lambda^2 I$ bounds the solution: - -$$\|\Delta q\| \leq \frac{\|e\|}{\lambda}$$ - -The cost is a small positional bias proportional to $\lambda$: the algorithm converges to within approximately $\lambda^2 \|e\|$ rather than exact zero. Choosing $\lambda \in [0.01, 0.2]$ balances stability and accuracy for typical robot geometries. - -## Complexity Analysis - -| Case | Time | Space | Notes | -|---------------|----------------|--------|----------------------------------------| -| Per iteration | $O(n)$ | $O(n)$ | FK sweep + Jacobian + $3\times3$ solve | -| Total | $O(k \cdot n)$ | $O(n)$ | $k$ = iterations (typically 10–50) | - -The $3 \times 3$ linear solve is $O(1)$ (constant-size), independent of the number of joints $n$. - -## Step-by-Step Walkthrough - -Consider a 2-link planar arm with $l_1 = 1.0$, $l_2 = 0.8$, both joints rotating about the Z-axis, target $p_{\text{target}} = (0.5, 1.0, 0)$, initial guess $q = (0, 0)$, $\lambda = 0.1$. - -**Iteration 1:** - -1. FK gives $p_{\text{ee}} = (1.8, 0, 0)$ (fully extended along X) -2. Error: $e = (-1.3, 1.0, 0)$, $\|e\| \approx 1.64$ -3. Jacobian (Z-axis joints, cross-product form): - - $J_1 = [0,0,1]^\top \times (p_{\text{ee}} - p_0) = [0,0,1]^\top \times [1.8, 0, 0]^\top = [0, 1.8, 0]^\top$ - - $J_2 = [0,0,1]^\top \times (p_{\text{ee}} - p_1) = [0,0,1]^\top \times [0.8, 0, 0]^\top = [0, 0.8, 0]^\top$ - - $J = \begin{bmatrix} 0 & 0 \\ 1.8 & 0.8 \\ 0 & 0 \end{bmatrix}$ (only Y-row is non-zero for planar X motion) -4. $A = J J^\top + 0.01 I$ — a $3 \times 3$ SPD matrix -5. Solve $Ay = e$, compute $\Delta q = J^\top y$ -6. Update both joint angles toward a bent configuration - -After ~40 iterations $q$ converges to joints that produce $p_{\text{ee}} \approx (0.5, 1.0, 0)$. - -## Pitfalls & Edge Cases - -- **Unreachable targets**: If $\|p_{\text{target}}\|$ exceeds the total arm length, the algorithm reaches maximum iterations without converging. The returned result has `converged = false`; the end-effector will be as close as geometrically possible. -- **Singularity handling**: Near singularities (e.g., fully extended arm), the damping $\lambda$ prevents numerical blow-up but introduces a small steady-state error. Reduce $\lambda$ for higher accuracy away from singularities. -- **Local minima in redundant chains**: For $n > 3$ (redundant), DLS finds *a* solution (minimum-norm joint motion) but not necessarily the one with the best posture. Null-space techniques (projecting secondary objectives into $\ker(J)$) can improve this. -- **Initial guess sensitivity**: Starting closer to the expected solution reduces iterations and avoids winding through joint-limit regions. For repeated calls (e.g., tracking), use the previous solution as the initial guess. -- **Float-only**: Uses `std::cos`, `std::sin`, and `std::sqrt`; Q15/Q31 fixed-point types are not supported. -- **Configuration preconditions**: `dampingFactor` and `tolerance` must both be strictly positive. The damping term $\lambda^2 I$ ensures $A$ is symmetric positive definite only when $\lambda > 0$; both values are asserted in debug builds via `really_assert`. - -## Variants & Generalizations - -- **Jacobian Transpose**: $\Delta q = \alpha J^\top e$ — no matrix solve, but slower convergence and step-size sensitive. Suitable for very constrained embedded targets. -- **Pseudoinverse (Moore-Penrose)**: DLS with $\lambda = 0$ — exact minimum-norm solution but numerically unstable at singularities. -- **FABRIK**: Forward-And-Backward Reaching IK — purely geometric, position-only, no Jacobian required. Very fast per iteration; preferred when orientation control is not needed. -- **Cyclic Coordinate Descent (CCD)**: Each joint independently minimizes error; simple but produces unnatural motion. -- **Full 6-DOF pose IK**: Extend $J$ to $6 \times n$ (stacking angular velocity rows) and $e$ to $\mathbb{R}^6$ to control both position and orientation. - -## Applications - -- **Robot arm target tracking**: Drive the end-effector to follow a moving 3D target in real-time. -- **Teleoperation**: Map operator hand position to robot joint angles continuously. -- **Motion planning**: Compute joint-space waypoints from Cartesian-space paths. -- **Animation & simulation**: Pose limbs in a physically plausible configuration to reach a point of interest. - -## Connections to Other Algorithms - -- [Forward Kinematics](ForwardKinematics.md): Called internally every iteration to compute current end-effector position and joint positions for the Jacobian. -- [GaussianElimination](../solvers/GaussianElimination.md): Solves the $3 \times 3$ DLS system $Ay = e$ at the core of each iteration. -- `numerical/math/Geometry3D.hpp`: Provides `CrossProduct` for Jacobian column computation, `RotationAboutAxis` for accumulated rotation, and `VectorNorm` for convergence check. - -## References & Further Reading - -- Buss, S.R. (2004). *Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods*. University of California, San Diego. [PDF](https://mathweb.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf) -- Nakamura, Y. and Hanafusa, H. (1986). "Inverse kinematic solutions with singularity robustness for robot manipulator control". *Journal of Dynamic Systems, Measurement, and Control*, 108(3). -- Siciliano, B. et al. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 3. -- Craig, J.J. (2005). *Introduction to Robotics: Mechanics and Control*. 3rd ed. Chapter 4. diff --git a/doc/kinematics/README.md b/doc/kinematics/README.md deleted file mode 100644 index 4393d6fa..00000000 --- a/doc/kinematics/README.md +++ /dev/null @@ -1,10 +0,0 @@ -# Kinematics - -Geometry and motion algorithms for serial kinematic chains. - -## Algorithms - -| Algorithm | Description | -|--------------------------------------------|-------------------------------------------------------------------------------------------------------------| -| [Forward Kinematics](ForwardKinematics.md) | Computes 3D joint positions from joint angles for serial chains — $O(n)$ complexity | -| [Inverse Kinematics](InverseKinematics.md) | Solves joint angles from a target end-effector position via Damped Least Squares — $O(k \cdot n)$ iterative | diff --git a/numerical/CMakeLists.txt b/numerical/CMakeLists.txt index 4b5d8f0d..1d4650c1 100644 --- a/numerical/CMakeLists.txt +++ b/numerical/CMakeLists.txt @@ -1,9 +1,7 @@ add_subdirectory(analysis) add_subdirectory(control_analysis) add_subdirectory(controllers) -add_subdirectory(dynamics) add_subdirectory(estimators) -add_subdirectory(kinematics) add_subdirectory(filters) add_subdirectory(math) add_subdirectory(neural_network) diff --git a/numerical/dynamics/ArticulatedBodyAlgorithm.cpp b/numerical/dynamics/ArticulatedBodyAlgorithm.cpp deleted file mode 100644 index 267d20e6..00000000 --- a/numerical/dynamics/ArticulatedBodyAlgorithm.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "numerical/dynamics/ArticulatedBodyAlgorithm.hpp" - -namespace dynamics -{ - template class ArticulatedBodyAlgorithm; - template class ArticulatedBodyAlgorithm; - template class ArticulatedBodyAlgorithm; -} diff --git a/numerical/dynamics/ArticulatedBodyAlgorithm.hpp b/numerical/dynamics/ArticulatedBodyAlgorithm.hpp deleted file mode 100644 index 83eea7dc..00000000 --- a/numerical/dynamics/ArticulatedBodyAlgorithm.hpp +++ /dev/null @@ -1,402 +0,0 @@ -#pragma once - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -#include "numerical/dynamics/RevoluteJointLink.hpp" -#include "numerical/math/CompilerOptimizations.hpp" -#include "numerical/math/Geometry3D.hpp" -#include "numerical/math/Matrix.hpp" -#include - -namespace dynamics -{ - // Articulated Body Algorithm (ABA): O(n) forward dynamics for serial chains. - // Given joint positions, velocities, and applied torques, computes joint accelerations - // directly without forming or inverting the mass matrix. - template - class ArticulatedBodyAlgorithm - { - static_assert(std::is_floating_point_v, - "ArticulatedBodyAlgorithm only supports floating-point types"); - static_assert(NumLinks > 0, "Number of links must be positive"); - - public: - using Vector3 = math::Vector; - using Matrix3 = math::SquareMatrix; - using JointVector = math::Vector; - using LinkArray = std::array, NumLinks>; - - ArticulatedBodyAlgorithm() = default; - - OPTIMIZE_FOR_SPEED JointVector ForwardDynamics(const LinkArray& links, - const JointVector& q, const JointVector& qDot, const JointVector& tau, - const Vector3& gravity) const; - - private: - struct LinkKinematics - { - Vector3 omega; - Vector3 linVelJ; - Matrix3 R; - Vector3 cJ; - Vector3 cJLin; - }; - - struct SpatialInertia - { - Matrix3 rot; - Matrix3 cross; - Matrix3 lin; - }; - - struct BiasWrench - { - Vector3 force; - Vector3 torque; - }; - - struct JointProjection - { - T D; - T u; - Vector3 Ua; - Vector3 UaLin; - }; - - // ── Pass 1: Forward kinematics ── - - static std::array ComputeForwardKinematics( - const LinkArray& links, const JointVector& q, const JointVector& qDot); - - static void PropagateKinematicsFromParent( - LinkKinematics& current, const LinkKinematics& parent, - const RevoluteJointLink& link, T qDot_i); - - // ── Pass 2: Backward articulated-body inertias ── - - static SpatialInertia ComputeRigidBodyInertia( - const RevoluteJointLink& link); - - static BiasWrench ComputeBiasWrench( - const RevoluteJointLink& link, - const LinkKinematics& kin); - - static JointProjection ComputeJointProjection( - const Vector3& axis, const SpatialInertia& Ia, - const BiasWrench& pA, T tau); - - static SpatialInertia ArticulatedBodyDowndate( - const SpatialInertia& Ia, const JointProjection& proj); - - static BiasWrench ComputeArticulatedBiasWrench( - const BiasWrench& pA, const SpatialInertia& Ia, - const LinkKinematics& kin, const JointProjection& proj); - - static SpatialInertia TransformInertiaToParent( - const SpatialInertia& Ia, const Matrix3& R, - const Vector3& parentToJoint); - - static BiasWrench TransformWrenchToParent( - const BiasWrench& wrench, const Matrix3& R, - const Vector3& parentToJoint); - - static void AccumulateChildContribution( - const LinkArray& links, std::size_t childIndex, - std::array& Ia, - std::array& pA, - const std::array& kin, - const std::array& proj); - - static void ComputeArticulatedBodyInertias( - const LinkArray& links, const JointVector& tau, - const std::array& kin, - std::array& Ia, - std::array& pA, - std::array& proj); - - // ── Pass 3: Forward accelerations ── - - static T ComputeJointAcceleration( - const JointProjection& proj, - const Vector3& aHatAng, const Vector3& aHatLin); - - static JointVector ComputeJointAccelerations( - const LinkArray& links, const Vector3& gravity, - const std::array& kin, - const std::array& proj); - }; - - template - void ArticulatedBodyAlgorithm::PropagateKinematicsFromParent( - LinkKinematics& current, const LinkKinematics& parent, - const RevoluteJointLink& link, T qDot_i) - { - auto Rt = current.R.Transpose(); - auto qDotAxis = link.jointAxis * qDot_i; - - current.omega = Rt * parent.omega + qDotAxis; - - 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); - } - - template - std::array::LinkKinematics, NumLinks> - ArticulatedBodyAlgorithm::ComputeForwardKinematics( - const LinkArray& links, const JointVector& q, const JointVector& qDot) - { - std::array kin{}; - - for (std::size_t i = 0; i < NumLinks; ++i) - { - kin[i].R = math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); - - 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)); - } - - return kin; - } - - template - typename ArticulatedBodyAlgorithm::SpatialInertia - ArticulatedBodyAlgorithm::ComputeRigidBodyInertia( - const RevoluteJointLink& link) - { - auto rSkew = math::SkewSymmetric(link.jointToCoM); - auto rSkewT = rSkew.Transpose(); - - return SpatialInertia{ - link.inertia + rSkewT * rSkew * link.mass, - rSkew * link.mass, - math::ScaledIdentity(link.mass) - }; - } - - template - typename ArticulatedBodyAlgorithm::BiasWrench - ArticulatedBodyAlgorithm::ComputeBiasWrench( - const RevoluteJointLink& link, - const LinkKinematics& kin) - { - auto rCoM = link.jointToCoM; - auto velCoM = kin.linVelJ + math::CrossProduct(kin.omega, rCoM); - auto biasForce = math::CrossProduct(kin.omega, velCoM) * link.mass; - auto Iw = link.inertia * kin.omega; - auto biasTorque = math::CrossProduct(kin.omega, Iw); - - return BiasWrench{ - biasForce, - biasTorque + math::CrossProduct(rCoM, biasForce) - }; - } - - template - typename ArticulatedBodyAlgorithm::JointProjection - ArticulatedBodyAlgorithm::ComputeJointProjection( - const Vector3& axis, const SpatialInertia& Ia, - const BiasWrench& pA, T tau) - { - JointProjection proj{}; - proj.Ua = Ia.rot * axis; - proj.UaLin = Ia.cross.Transpose() * axis; - - auto dVec = axis.Transpose() * proj.Ua; - proj.D = dVec.at(0, 0); - - auto sTpA = axis.Transpose() * pA.torque; - proj.u = tau - sTpA.at(0, 0); - - return proj; - } - - template - typename ArticulatedBodyAlgorithm::SpatialInertia - ArticulatedBodyAlgorithm::ArticulatedBodyDowndate( - const SpatialInertia& Ia, const JointProjection& proj) - { - T invD = T(1) / proj.D; - - return SpatialInertia{ - Ia.rot - math::OuterProduct(proj.Ua, proj.Ua) * invD, - Ia.cross - math::OuterProduct(proj.Ua, proj.UaLin) * invD, - Ia.lin - math::OuterProduct(proj.UaLin, proj.UaLin) * invD - }; - } - - template - typename ArticulatedBodyAlgorithm::BiasWrench - ArticulatedBodyAlgorithm::ComputeArticulatedBiasWrench( - const BiasWrench& pA, const SpatialInertia& Ia, - const LinkKinematics& kin, const JointProjection& proj) - { - auto uScaled = proj.u / proj.D; - - return BiasWrench{ - pA.force + Ia.cross.Transpose() * kin.cJ + Ia.lin * kin.cJLin + proj.UaLin * uScaled, - pA.torque + Ia.rot * kin.cJ + Ia.cross * kin.cJLin + proj.Ua * uScaled - }; - } - - template - typename ArticulatedBodyAlgorithm::SpatialInertia - ArticulatedBodyAlgorithm::TransformInertiaToParent( - const SpatialInertia& Ia, const Matrix3& R, - const Vector3& parentToJoint) - { - auto Rt = R.Transpose(); - auto IaRotP = R * Ia.rot * Rt; - auto IaCrossP = R * Ia.cross * Rt; - auto IaLinP = R * Ia.lin * Rt; - - auto rSkew = math::SkewSymmetric(parentToJoint); - - return SpatialInertia{ - IaRotP - IaCrossP * rSkew + rSkew * IaCrossP.Transpose() - rSkew * IaLinP * rSkew, - IaCrossP + rSkew * IaLinP, - IaLinP - }; - } - - template - typename ArticulatedBodyAlgorithm::BiasWrench - ArticulatedBodyAlgorithm::TransformWrenchToParent( - const BiasWrench& wrench, const Matrix3& R, - const Vector3& parentToJoint) - { - auto forceP = R * wrench.force; - auto torqueP = R * wrench.torque; - - return BiasWrench{ - forceP, - torqueP + math::CrossProduct(parentToJoint, forceP) - }; - } - - template - T ArticulatedBodyAlgorithm::ComputeJointAcceleration( - const JointProjection& proj, - const Vector3& aPrimeAng, const Vector3& aPrimeLin) - { - auto UaTa = proj.Ua.Transpose() * aPrimeAng + proj.UaLin.Transpose() * aPrimeLin; - return (proj.u - UaTa.at(0, 0)) / proj.D; - } - - template - void ArticulatedBodyAlgorithm::AccumulateChildContribution( - const LinkArray& links, std::size_t childIndex, - std::array& Ia, - std::array& pA, - const std::array& kin, - const std::array& proj) - { - auto parentIndex = childIndex - 1; - - 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); - - Ia[parentIndex].rot = Ia[parentIndex].rot + IaParent.rot; - Ia[parentIndex].cross = Ia[parentIndex].cross + IaParent.cross; - Ia[parentIndex].lin = Ia[parentIndex].lin + IaParent.lin; - pA[parentIndex].force = pA[parentIndex].force + pAParent.force; - pA[parentIndex].torque = pA[parentIndex].torque + pAParent.torque; - } - - template - void ArticulatedBodyAlgorithm::ComputeArticulatedBodyInertias( - const LinkArray& links, const JointVector& tau, - const std::array& kin, - std::array& Ia, - std::array& pA, - std::array& proj) - { - for (std::size_t i = 0; i < NumLinks; ++i) - { - Ia[i] = ComputeRigidBodyInertia(links[i]); - pA[i] = ComputeBiasWrench(links[i], kin[i]); - } - - 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)); - - if (i > 0) - AccumulateChildContribution(links, i, Ia, pA, kin, proj); - } - } - - template - typename ArticulatedBodyAlgorithm::JointVector - ArticulatedBodyAlgorithm::ComputeJointAccelerations( - const LinkArray& links, const Vector3& gravity, - const std::array& kin, - const std::array& proj) - { - JointVector qDDot; - std::array angAcc; - std::array linAcc; - - 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; - - qDDot.at(i, 0) = ComputeJointAcceleration(proj[i], aHatAng, aHatLin); - - angAcc[i] = aHatAng + links[i].jointAxis * qDDot.at(i, 0); - linAcc[i] = aHatLin; - } - - return qDDot; - } - - template - OPTIMIZE_FOR_SPEED - typename ArticulatedBodyAlgorithm::JointVector - ArticulatedBodyAlgorithm::ForwardDynamics(const LinkArray& links, - const JointVector& q, const JointVector& qDot, const JointVector& tau, - const Vector3& gravity) const - { - auto kin = ComputeForwardKinematics(links, q, qDot); - - std::array Ia; - std::array pA; - std::array proj; - ComputeArticulatedBodyInertias(links, tau, kin, Ia, pA, proj); - - return ComputeJointAccelerations(links, gravity, kin, proj); - } - -#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD - extern template class ArticulatedBodyAlgorithm; - extern template class ArticulatedBodyAlgorithm; - extern template class ArticulatedBodyAlgorithm; -#endif -} diff --git a/numerical/dynamics/CMakeLists.txt b/numerical/dynamics/CMakeLists.txt deleted file mode 100644 index cfe0f483..00000000 --- a/numerical/dynamics/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -numerical_add_header_library(numerical.dynamics) - -target_include_directories(numerical.dynamics ${NUMERICAL_VISIBILITY} - "$" - "$" -) - -target_link_libraries(numerical.dynamics ${NUMERICAL_VISIBILITY} - infra.util - numerical.math - numerical.solver -) - -target_sources(numerical.dynamics PRIVATE - ArticulatedBodyAlgorithm.hpp - EulerLagrangeDynamics.hpp - EulerLagrangeSolver.hpp - NewtonEulerBody.hpp - NewtonEulerSolver.hpp - RecursiveNewtonEuler.hpp - RevoluteJointLink.hpp -) - -numerical_add_coverage_sources(numerical.dynamics - ArticulatedBodyAlgorithm.cpp - EulerLagrangeSolver.cpp - NewtonEulerSolver.cpp - RecursiveNewtonEuler.cpp -) - -add_subdirectory(test) diff --git a/numerical/dynamics/EulerLagrangeDynamics.hpp b/numerical/dynamics/EulerLagrangeDynamics.hpp deleted file mode 100644 index aefccb51..00000000 --- a/numerical/dynamics/EulerLagrangeDynamics.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#pragma once - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -#include "numerical/math/Matrix.hpp" - -namespace dynamics -{ - template - class EulerLagrangeDynamics - { - static_assert(std::is_floating_point_v, - "EulerLagrangeDynamics only supports floating-point types"); - static_assert(math::detail::is_valid_dimensions_v, - "Degrees of freedom must be positive"); - - public: - using StateVector = math::Vector; - using MassMatrix = math::SquareMatrix; - - virtual ~EulerLagrangeDynamics() = default; - - virtual MassMatrix ComputeMassMatrix(const StateVector& q) const = 0; - virtual StateVector ComputeCoriolisTerms(const StateVector& q, const StateVector& qDot) const = 0; - virtual StateVector ComputeGravityTerms(const StateVector& q) const = 0; - }; -} diff --git a/numerical/dynamics/EulerLagrangeSolver.cpp b/numerical/dynamics/EulerLagrangeSolver.cpp deleted file mode 100644 index 5ddd840b..00000000 --- a/numerical/dynamics/EulerLagrangeSolver.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "numerical/dynamics/EulerLagrangeSolver.hpp" - -namespace dynamics -{ - template class EulerLagrangeSolver; - template class EulerLagrangeSolver; - template class EulerLagrangeSolver; -} diff --git a/numerical/dynamics/EulerLagrangeSolver.hpp b/numerical/dynamics/EulerLagrangeSolver.hpp deleted file mode 100644 index bb336a30..00000000 --- a/numerical/dynamics/EulerLagrangeSolver.hpp +++ /dev/null @@ -1,71 +0,0 @@ -#pragma once - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -#include "numerical/dynamics/EulerLagrangeDynamics.hpp" -#include "numerical/math/CompilerOptimizations.hpp" -#include "numerical/math/Matrix.hpp" -#include "numerical/solvers/GaussianElimination.hpp" - -namespace dynamics -{ - template - class EulerLagrangeSolver - { - static_assert(std::is_floating_point_v, - "EulerLagrangeSolver only supports floating-point types"); - static_assert(math::detail::is_valid_dimensions_v, - "Degrees of freedom must be positive"); - - public: - using StateVector = math::Vector; - using MassMatrix = math::SquareMatrix; - - EulerLagrangeSolver() = default; - - // Forward dynamics: computes qDDot = M(q)^{-1} * (tau - C(q, qDot) - g(q)) - OPTIMIZE_FOR_SPEED StateVector ForwardDynamics(const EulerLagrangeDynamics& model, - const StateVector& q, const StateVector& qDot, const StateVector& tau) const; - - // Inverse dynamics: computes tau = M(q) * qDDot + C(q, qDot) + g(q) - OPTIMIZE_FOR_SPEED StateVector InverseDynamics(const EulerLagrangeDynamics& model, - const StateVector& q, const StateVector& qDot, const StateVector& qDDot) const; - }; - - template - OPTIMIZE_FOR_SPEED - typename EulerLagrangeSolver::StateVector - EulerLagrangeSolver::ForwardDynamics(const EulerLagrangeDynamics& model, - const StateVector& q, const StateVector& qDot, const StateVector& tau) const - { - auto M = model.ComputeMassMatrix(q); - auto C = model.ComputeCoriolisTerms(q, qDot); - auto g = model.ComputeGravityTerms(q); - - auto rhs = tau - C - g; - - solvers::GaussianElimination solver; - return solver.Solve(M, rhs); - } - - template - OPTIMIZE_FOR_SPEED - typename EulerLagrangeSolver::StateVector - EulerLagrangeSolver::InverseDynamics(const EulerLagrangeDynamics& model, - const StateVector& q, const StateVector& qDot, const StateVector& qDDot) const - { - auto M = model.ComputeMassMatrix(q); - auto C = model.ComputeCoriolisTerms(q, qDot); - auto g = model.ComputeGravityTerms(q); - - return M * qDDot + C + g; - } - -#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD - extern template class EulerLagrangeSolver; - extern template class EulerLagrangeSolver; - extern template class EulerLagrangeSolver; -#endif -} diff --git a/numerical/dynamics/NewtonEulerBody.hpp b/numerical/dynamics/NewtonEulerBody.hpp deleted file mode 100644 index 700b9af3..00000000 --- a/numerical/dynamics/NewtonEulerBody.hpp +++ /dev/null @@ -1,26 +0,0 @@ -#pragma once - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -#include "numerical/math/Matrix.hpp" - -namespace dynamics -{ - template - class NewtonEulerBody - { - static_assert(std::is_floating_point_v, - "NewtonEulerBody only supports floating-point types"); - - public: - using Vector3 = math::Vector; - using InertiaMatrix = math::SquareMatrix; - - virtual ~NewtonEulerBody() = default; - - virtual T ComputeMass() const = 0; - virtual InertiaMatrix ComputeInertia() const = 0; - }; -} diff --git a/numerical/dynamics/NewtonEulerSolver.cpp b/numerical/dynamics/NewtonEulerSolver.cpp deleted file mode 100644 index 9b27343f..00000000 --- a/numerical/dynamics/NewtonEulerSolver.cpp +++ /dev/null @@ -1,6 +0,0 @@ -#include "numerical/dynamics/NewtonEulerSolver.hpp" - -namespace dynamics -{ - template class NewtonEulerSolver; -} diff --git a/numerical/dynamics/NewtonEulerSolver.hpp b/numerical/dynamics/NewtonEulerSolver.hpp deleted file mode 100644 index 82a4a48d..00000000 --- a/numerical/dynamics/NewtonEulerSolver.hpp +++ /dev/null @@ -1,116 +0,0 @@ -#pragma once - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -#include "numerical/dynamics/NewtonEulerBody.hpp" -#include "numerical/math/CompilerOptimizations.hpp" -#include "numerical/math/Matrix.hpp" -#include "numerical/solvers/GaussianElimination.hpp" - -namespace dynamics -{ - template - struct SpatialAcceleration - { - math::Vector linear; - math::Vector angular; - }; - - template - struct SpatialForce - { - math::Vector force; - math::Vector torque; - }; - - template - class NewtonEulerSolver - { - static_assert(std::is_floating_point_v, - "NewtonEulerSolver only supports floating-point types"); - - public: - using Vector3 = math::Vector; - using InertiaMatrix = math::SquareMatrix; - - NewtonEulerSolver() = default; - - // Forward dynamics (body-frame formulation): - // linearAccel = force / mass - angularVelocity x linearVelocity - // angularAccel = I^{-1} (torque - angularVelocity x (I * angularVelocity)) - OPTIMIZE_FOR_SPEED SpatialAcceleration ForwardDynamics(const NewtonEulerBody& body, - const Vector3& force, const Vector3& torque, - const Vector3& linearVelocity, const Vector3& angularVelocity) const; - - // Inverse dynamics (body-frame formulation): - // force = mass * (linearAccel + angularVelocity x linearVelocity) - // torque = I * angularAccel + angularVelocity x (I * angularVelocity) - OPTIMIZE_FOR_SPEED SpatialForce InverseDynamics(const NewtonEulerBody& body, - const Vector3& linearAcceleration, const Vector3& angularAcceleration, - const Vector3& linearVelocity, const Vector3& angularVelocity) const; - - private: - static Vector3 CrossProduct(const Vector3& a, const Vector3& b); - }; - - template - typename NewtonEulerSolver::Vector3 - NewtonEulerSolver::CrossProduct(const Vector3& a, const Vector3& b) - { - return Vector3{ - a.at(1, 0) * b.at(2, 0) - a.at(2, 0) * b.at(1, 0), - a.at(2, 0) * b.at(0, 0) - a.at(0, 0) * b.at(2, 0), - a.at(0, 0) * b.at(1, 0) - a.at(1, 0) * b.at(0, 0) - }; - } - - template - OPTIMIZE_FOR_SPEED - SpatialAcceleration - NewtonEulerSolver::ForwardDynamics(const NewtonEulerBody& body, - const Vector3& force, const Vector3& torque, - const Vector3& linearVelocity, const Vector3& angularVelocity) const - { - auto mass = body.ComputeMass(); - auto I = body.ComputeInertia(); - - auto Iw = I * angularVelocity; - auto gyroscopic = CrossProduct(angularVelocity, Iw); - auto rhs = torque - gyroscopic; - - solvers::GaussianElimination solver; - auto angularAccel = solver.Solve(I, rhs); - - auto wCrossV = CrossProduct(angularVelocity, linearVelocity); - T inverseMass = T(1.0f) / mass; - auto linearAccel = force * inverseMass - wCrossV; - - return SpatialAcceleration{ linearAccel, angularAccel }; - } - - template - OPTIMIZE_FOR_SPEED - SpatialForce - NewtonEulerSolver::InverseDynamics(const NewtonEulerBody& body, - const Vector3& linearAcceleration, const Vector3& angularAcceleration, - const Vector3& linearVelocity, const Vector3& angularVelocity) const - { - auto mass = body.ComputeMass(); - auto I = body.ComputeInertia(); - - auto wCrossV = CrossProduct(angularVelocity, linearVelocity); - auto resultForce = (linearAcceleration + wCrossV) * mass; - - auto Iw = I * angularVelocity; - auto gyroscopic = CrossProduct(angularVelocity, Iw); - auto resultTorque = I * angularAcceleration + gyroscopic; - - return SpatialForce{ resultForce, resultTorque }; - } - -#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD - extern template class NewtonEulerSolver; -#endif -} diff --git a/numerical/dynamics/RecursiveNewtonEuler.cpp b/numerical/dynamics/RecursiveNewtonEuler.cpp deleted file mode 100644 index 9d78bb01..00000000 --- a/numerical/dynamics/RecursiveNewtonEuler.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "numerical/dynamics/RecursiveNewtonEuler.hpp" - -namespace dynamics -{ - template class RecursiveNewtonEuler; - template class RecursiveNewtonEuler; - template class RecursiveNewtonEuler; -} diff --git a/numerical/dynamics/RecursiveNewtonEuler.hpp b/numerical/dynamics/RecursiveNewtonEuler.hpp deleted file mode 100644 index 766ca42c..00000000 --- a/numerical/dynamics/RecursiveNewtonEuler.hpp +++ /dev/null @@ -1,205 +0,0 @@ -#pragma once - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -#include "numerical/dynamics/RevoluteJointLink.hpp" -#include "numerical/math/CompilerOptimizations.hpp" -#include "numerical/math/Geometry3D.hpp" -#include "numerical/math/Matrix.hpp" -#include - -namespace dynamics -{ - template - class RecursiveNewtonEuler - { - static_assert(std::is_floating_point_v, - "RecursiveNewtonEuler only supports floating-point types"); - static_assert(NumLinks > 0, "Number of links must be positive"); - - public: - using Vector3 = math::Vector; - using Matrix3 = math::SquareMatrix; - using JointVector = math::Vector; - using LinkArray = std::array, NumLinks>; - - RecursiveNewtonEuler() = default; - - // Inverse dynamics: given joint positions, velocities, and accelerations, - // compute the required joint torques. O(n) complexity. - OPTIMIZE_FOR_SPEED JointVector InverseDynamics(const LinkArray& links, - const JointVector& q, const JointVector& qDot, const JointVector& qDDot, - const Vector3& gravity) const; - - private: - struct LinkState - { - Vector3 omega; - Vector3 omegaDot; - Vector3 linAccCoM; - Matrix3 R; - }; - - 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); - - static std::array ComputeForwardPass( - const LinkArray& links, const JointVector& q, - const JointVector& qDot, const JointVector& qDDot, - const Vector3& gravity); - - static void ComputeLinkWrench( - Vector3& force, Vector3& torque, - const RevoluteJointLink& link, const LinkState& state); - - static void AccumulateChildWrench( - Vector3& parentForce, Vector3& parentTorque, - const Vector3& childForce, const Vector3& childTorque, - const Matrix3& childR, const Vector3& rToChild); - - static JointVector ComputeBackwardPass( - const LinkArray& links, const std::array& states); - }; - - 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) - { - 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)); - - auto rToJoint = link.parentToJoint; - auto accAtParentEnd = parentJointAcc + math::CrossProduct(parent.omegaDot, rToJoint) + math::CrossProduct(parent.omega, math::CrossProduct(parent.omega, rToJoint)); - - 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)); - } - - template - std::array::LinkState, NumLinks> - RecursiveNewtonEuler::ComputeForwardPass( - const LinkArray& links, const JointVector& q, - const JointVector& qDot, const JointVector& qDDot, - const Vector3& gravity) - { - std::array states{}; - - for (std::size_t i = 0; i < NumLinks; ++i) - { - states[i].R = math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); - - 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)); - } - - return states; - } - - template - void RecursiveNewtonEuler::ComputeLinkWrench( - Vector3& force, Vector3& torque, - const RevoluteJointLink& link, const LinkState& state) - { - force = state.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); - } - - template - void RecursiveNewtonEuler::AccumulateChildWrench( - Vector3& parentForce, Vector3& parentTorque, - const Vector3& childForce, const Vector3& childTorque, - const Matrix3& childR, const Vector3& rToChild) - { - auto childForceInParent = childR * childForce; - auto childTorqueInParent = childR * childTorque; - - parentForce = parentForce + childForceInParent; - parentTorque = parentTorque + childTorqueInParent + math::CrossProduct(rToChild, childForceInParent); - } - - template - typename RecursiveNewtonEuler::JointVector - RecursiveNewtonEuler::ComputeBackwardPass( - const LinkArray& links, const std::array& states) - { - std::array force; - std::array torque; - - for (std::size_t j = NumLinks; j > 0; --j) - { - std::size_t i = j - 1; - - ComputeLinkWrench(force[i], torque[i], links[i], states[i]); - - if (i + 1 < NumLinks) - AccumulateChildWrench(force[i], torque[i], force[i + 1], torque[i + 1], - states[i + 1].R, links[i + 1].parentToJoint); - } - - JointVector tau; - for (std::size_t i = 0; i < NumLinks; ++i) - { - auto dotProduct = links[i].jointAxis.Transpose() * torque[i]; - tau.at(i, 0) = dotProduct.at(0, 0); - } - - return tau; - } - - template - OPTIMIZE_FOR_SPEED - typename RecursiveNewtonEuler::JointVector - RecursiveNewtonEuler::InverseDynamics(const LinkArray& links, - const JointVector& q, const JointVector& qDot, const JointVector& qDDot, - const Vector3& gravity) const - { - auto states = ComputeForwardPass(links, q, qDot, qDDot, gravity); - return ComputeBackwardPass(links, states); - } - -#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD - extern template class RecursiveNewtonEuler; - extern template class RecursiveNewtonEuler; - extern template class RecursiveNewtonEuler; -#endif -} diff --git a/numerical/dynamics/RevoluteJointLink.hpp b/numerical/dynamics/RevoluteJointLink.hpp deleted file mode 100644 index c3ab2a36..00000000 --- a/numerical/dynamics/RevoluteJointLink.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -#include "numerical/math/Matrix.hpp" -#include - -namespace dynamics -{ - template - struct RevoluteJointLink - { - static_assert(std::is_floating_point_v, - "RevoluteJointLink only supports floating-point types"); - - T mass; - math::SquareMatrix inertia; // inertia tensor at center of mass, in link frame - math::Vector jointAxis; // joint rotation axis in link frame (unit vector) - math::Vector parentToJoint; // position of this joint origin in parent frame - math::Vector jointToCoM; // position of center of mass in link frame - }; -} diff --git a/numerical/dynamics/test/CMakeLists.txt b/numerical/dynamics/test/CMakeLists.txt deleted file mode 100644 index 152e062b..00000000 --- a/numerical/dynamics/test/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -add_executable(numerical.dynamics_test) -emil_build_for(numerical.dynamics_test BOOL NUMERICAL_TOOLBOX_BUILD_TESTS) -emil_add_test(numerical.dynamics_test) - -target_link_libraries(numerical.dynamics_test PUBLIC - gmock_main - numerical.dynamics -) - -target_sources(numerical.dynamics_test PRIVATE - TestArticulatedBodyAlgorithm.cpp - TestEulerLagrangeSolver.cpp - TestNewtonEulerSolver.cpp - TestRecursiveNewtonEuler.cpp -) diff --git a/numerical/dynamics/test/TestArticulatedBodyAlgorithm.cpp b/numerical/dynamics/test/TestArticulatedBodyAlgorithm.cpp deleted file mode 100644 index 4e61ebdc..00000000 --- a/numerical/dynamics/test/TestArticulatedBodyAlgorithm.cpp +++ /dev/null @@ -1,277 +0,0 @@ -#include "numerical/dynamics/ArticulatedBodyAlgorithm.hpp" -#include -#include - -namespace -{ - constexpr float gravity = 9.81f; - const math::Vector gravityVec{ 0.0f, 0.0f, -gravity }; - - dynamics::RevoluteJointLink MakePendulumLink(float mass, float length) - { - float I = mass * length * length / 12.0f; - - return dynamics::RevoluteJointLink{ - mass, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ length / 2.0f, 0.0f, 0.0f } - }; - } - - std::array, 2> MakeTwoLinkArm( - float m1, float l1, float m2, float l2) - { - float I1 = m1 * l1 * l1 / 12.0f; - float I2 = m2 * l2 * l2 / 12.0f; - - dynamics::RevoluteJointLink link1{ - m1, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I1, 0.0f }, - { 0.0f, 0.0f, I1 } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ l1 / 2.0f, 0.0f, 0.0f } - }; - - dynamics::RevoluteJointLink link2{ - m2, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I2, 0.0f }, - { 0.0f, 0.0f, I2 } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{ l1, 0.0f, 0.0f }, - math::Vector{ l2 / 2.0f, 0.0f, 0.0f } - }; - - return { link1, link2 }; - } - - class TestArticulatedBodyAlgorithm : public ::testing::Test - { - protected: - dynamics::ArticulatedBodyAlgorithm aba1; - dynamics::ArticulatedBodyAlgorithm aba2; - }; -} - -TEST_F(TestArticulatedBodyAlgorithm, single_link_zero_torque_no_gravity_gives_zero_acceleration) -{ - auto link = MakePendulumLink(1.0f, 1.0f); - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector tau{}; - math::Vector zeroGravity{}; - - auto qDDot = aba1.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), 0.0f, 1e-5f); -} - -TEST_F(TestArticulatedBodyAlgorithm, single_link_pure_torque_no_gravity) -{ - float mass = 2.0f; - float length = 1.0f; - auto link = MakePendulumLink(mass, length); - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector zeroGravity{}; - - // Apply torque = I_end to get qDDot = 1 rad/s^2 - float I_end = mass * length * length / 3.0f; - math::Vector tau{ I_end }; - - auto qDDot = aba1.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), 1.0f, 1e-3f); -} - -TEST_F(TestArticulatedBodyAlgorithm, single_link_rnea_aba_roundtrip_no_gravity) -{ - auto link = MakePendulumLink(2.0f, 1.0f); - std::array, 1> links = { link }; - - math::Vector q{ 0.5f }; - math::Vector qDot{ 1.5f }; - math::Vector qDDotExpected{ 3.0f }; - math::Vector zeroGravity{}; - - math::Vector tau{ 2.0f }; - - auto qDDotRecovered = aba1.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDotRecovered.at(0, 0), qDDotExpected.at(0, 0), 1e-3f); -} - -TEST_F(TestArticulatedBodyAlgorithm, single_link_rnea_aba_roundtrip_with_gravity) -{ - auto link = MakePendulumLink(2.0f, 1.0f); - std::array, 1> links = { link }; - - math::Vector q{ 0.3f }; - math::Vector qDot{ 0.0f }; - math::Vector qDDotExpected{ 2.0f }; - - math::Vector tau{ 1.3333333731f }; - auto qDDotRecovered = aba1.ForwardDynamics(links, q, qDot, tau, gravityVec); - - EXPECT_NEAR(qDDotRecovered.at(0, 0), qDDotExpected.at(0, 0), 1e-2f); -} - -TEST_F(TestArticulatedBodyAlgorithm, two_link_zero_torque_no_gravity_gives_zero_acceleration) -{ - auto links = MakeTwoLinkArm(1.0f, 1.0f, 1.0f, 1.0f); - - math::Vector q{ 0.0f, 0.0f }; - math::Vector qDot{}; - math::Vector tau{}; - math::Vector zeroGravity{}; - - auto qDDot = aba2.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(qDDot.at(1, 0), 0.0f, 1e-4f); -} - -TEST_F(TestArticulatedBodyAlgorithm, two_link_analytical_torque_produces_known_acceleration) -{ - // Textbook 2R planar arm (z-axis joints, uniform rods): - // M(q) = | m1*l1^2/3 + m2*(l1^2 + l2^2/3 + l1*l2*cos(q2)) m2*(l2^2/3 + l1*l2*cos(q2)/2) | - // | m2*(l2^2/3 + l1*l2*cos(q2)/2) m2*l2^2/3 | - // C(q,qd)*qd_0 = h*(2*qd1*qd2 + qd2^2)/2, h = -m2*l1*l2*sin(q2) - // C(q,qd)*qd_1 = -h*qd1^2/2 - // G = 0 (z-axis rotation with z-gravity => no gravitational torque) - // - // tau = M*qDDot + C*qDot => qDDot = M^{-1}*(tau - C*qDot) - - float m1 = 1.0f, l1 = 1.0f, m2 = 1.0f, l2 = 1.0f; - float q2 = -0.5f; - float qd1 = 1.0f, qd2 = -0.5f; - float qdd1 = 2.0f, qdd2 = -1.0f; - - float c2 = std::cos(q2); - float s2 = std::sin(q2); - float h = -m2 * l1 * l2 * s2; - - float M00 = m1 * l1 * l1 / 3.0f + m2 * (l1 * l1 + l2 * l2 / 3.0f + l1 * l2 * c2); - float M01 = m2 * (l2 * l2 / 3.0f + l1 * l2 * c2 / 2.0f); - float M11 = m2 * l2 * l2 / 3.0f; - - float Cqd0 = h * (2.0f * qd1 * qd2 + qd2 * qd2) / 2.0f; - float Cqd1 = -h * qd1 * qd1 / 2.0f; - - float tau0 = M00 * qdd1 + M01 * qdd2 + Cqd0; - float tau1 = M01 * qdd1 + M11 * qdd2 + Cqd1; - - auto links = MakeTwoLinkArm(m1, l1, m2, l2); - math::Vector q{ 0.3f, q2 }; - math::Vector qDot{ qd1, qd2 }; - math::Vector tau{ tau0, tau1 }; - math::Vector zeroGravity{}; - - auto qDDot = aba2.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), qdd1, 1e-4f); - EXPECT_NEAR(qDDot.at(1, 0), qdd2, 1e-4f); -} - -TEST_F(TestArticulatedBodyAlgorithm, two_link_coriolis_torque_gives_zero_acceleration) -{ - // If tau = C(q,qDot)*qDot exactly, then M*qDDot = 0 => qDDot = 0 - float m1 = 1.0f, l1 = 1.0f, m2 = 1.0f, l2 = 1.0f; - float q2 = -0.5f; - float qd1 = 1.0f, qd2 = -0.5f; - - float h = -m2 * l1 * l2 * std::sin(q2); - float tau0 = h * (2.0f * qd1 * qd2 + qd2 * qd2) / 2.0f; - float tau1 = -h * qd1 * qd1 / 2.0f; - - auto links = MakeTwoLinkArm(m1, l1, m2, l2); - math::Vector q{ 0.3f, q2 }; - math::Vector qDot{ qd1, qd2 }; - math::Vector tau{ tau0, tau1 }; - math::Vector zeroGravity{}; - - auto qDDot = aba2.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDot.at(0, 0), 0.0f, 1e-5f); - EXPECT_NEAR(qDDot.at(1, 0), 0.0f, 1e-5f); -} - -TEST_F(TestArticulatedBodyAlgorithm, two_link_rnea_aba_roundtrip_no_gravity) -{ - auto links = MakeTwoLinkArm(1.0f, 1.0f, 1.0f, 1.0f); - - math::Vector q{ 0.3f, -0.5f }; - math::Vector qDot{ 1.0f, -0.5f }; - math::Vector qDDotExpected{ 2.0f, -1.0f }; - math::Vector zeroGravity{}; - - math::Vector tau{ 4.1365890503f, 0.9712030888f }; - auto qDDotRecovered = aba2.ForwardDynamics(links, q, qDot, tau, zeroGravity); - - EXPECT_NEAR(qDDotRecovered.at(0, 0), qDDotExpected.at(0, 0), 1e-4f); - EXPECT_NEAR(qDDotRecovered.at(1, 0), qDDotExpected.at(1, 0), 1e-4f); -} - -TEST_F(TestArticulatedBodyAlgorithm, two_link_rnea_aba_roundtrip_with_gravity) -{ - auto links = MakeTwoLinkArm(2.0f, 0.5f, 1.0f, 0.8f); - - math::Vector q{ 0.1f, -0.2f }; - math::Vector qDot{ 0.5f, -0.3f }; - math::Vector qDDotExpected{ 1.0f, -2.0f }; - - math::Vector tau{ 0.1949892044f, -0.0272534918f }; - auto qDDotRecovered = aba2.ForwardDynamics(links, q, qDot, tau, gravityVec); - - EXPECT_NEAR(qDDotRecovered.at(0, 0), qDDotExpected.at(0, 0), 1e-4f); - EXPECT_NEAR(qDDotRecovered.at(1, 0), qDDotExpected.at(1, 0), 1e-4f); -} - -TEST_F(TestArticulatedBodyAlgorithm, single_link_free_fall_under_gravity) -{ - // y-axis pendulum at q=0: joint axis = y, CoM at [L/2, 0, 0]. - // Gravity = [0, 0, -g]. At q=0, link frame = world frame. - // Potential energy: V = -mgL*sin(q)/2 (positive y-rotation moves CoM toward -z) - // G(q) = dV/dq = -mgL*cos(q)/2 - // At q=0: G = -mgL/2 - // EoM: I_pivot * qDDot + G = 0 => qDDot = -G / I_pivot = +mgL / (2*I_pivot) - // I_pivot = mL^2/3 - // qDDot = +3g/(2L) - float mass = 1.0f; - float length = 1.0f; - float I = mass * length * length / 12.0f; - - dynamics::RevoluteJointLink link{ - mass, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{}, - math::Vector{ length / 2.0f, 0.0f, 0.0f } - }; - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector tau{}; - - auto qDDot = aba1.ForwardDynamics(links, q, qDot, tau, gravityVec); - - float expected = 3.0f * gravity / (2.0f * length); - EXPECT_NEAR(qDDot.at(0, 0), expected, 1e-4f); -} diff --git a/numerical/dynamics/test/TestEulerLagrangeSolver.cpp b/numerical/dynamics/test/TestEulerLagrangeSolver.cpp deleted file mode 100644 index a2a053c0..00000000 --- a/numerical/dynamics/test/TestEulerLagrangeSolver.cpp +++ /dev/null @@ -1,209 +0,0 @@ -#include "numerical/dynamics/EulerLagrangeDynamics.hpp" -#include "numerical/dynamics/EulerLagrangeSolver.hpp" -#include -#include - -namespace -{ - // Simple pendulum: 1-DOF - // M(q) = m * l^2 - // C(q, qDot) = 0 - // g(q) = m * gravity * l * sin(q) - class SimplePendulum : public dynamics::EulerLagrangeDynamics - { - public: - static constexpr float mass = 1.0f; - static constexpr float length = 1.0f; - static constexpr float gravity = 9.81f; - - MassMatrix ComputeMassMatrix(const StateVector& /* q */) const override - { - return MassMatrix{ mass * length * length }; - } - - StateVector ComputeCoriolisTerms(const StateVector& /* q */, const StateVector& /* qDot */) const override - { - return StateVector{}; - } - - StateVector ComputeGravityTerms(const StateVector& q) const override - { - return StateVector{ mass * gravity * length * std::sin(q.at(0, 0)) }; - } - }; - - // Two-link planar arm: 2-DOF - // m1, m2: link masses - // l1, l2: link lengths - // lc1, lc2: center-of-mass distances - // I1, I2: link inertias - class TwoLinkPlanarArm : public dynamics::EulerLagrangeDynamics - { - public: - static constexpr float m1 = 1.0f; - static constexpr float m2 = 1.0f; - static constexpr float l1 = 1.0f; - static constexpr float lc1 = 0.5f; - static constexpr float lc2 = 0.5f; - static constexpr float I1 = 0.083f; // m1 * l1^2 / 12 - static constexpr float I2 = 0.083f; - static constexpr float gravity = 9.81f; - - MassMatrix ComputeMassMatrix(const StateVector& q) const override - { - float c2 = std::cos(q.at(1, 0)); - - float d11 = I1 + I2 + m1 * lc1 * lc1 + m2 * (l1 * l1 + lc2 * lc2 + 2.0f * l1 * lc2 * c2); - float d12 = I2 + m2 * (lc2 * lc2 + l1 * lc2 * c2); - float d22 = I2 + m2 * lc2 * lc2; - - return MassMatrix{ - { d11, d12 }, - { d12, d22 } - }; - } - - StateVector ComputeCoriolisTerms(const StateVector& q, const StateVector& qDot) const override - { - float s2 = std::sin(q.at(1, 0)); - float h = m2 * l1 * lc2 * s2; - - float c1 = -h * qDot.at(1, 0) * (2.0f * qDot.at(0, 0) + qDot.at(1, 0)); - float c2Term = h * qDot.at(0, 0) * qDot.at(0, 0); - - return StateVector{ c1, c2Term }; - } - - StateVector ComputeGravityTerms(const StateVector& q) const override - { - float s1 = std::sin(q.at(0, 0)); - float s12 = std::sin(q.at(0, 0) + q.at(1, 0)); - - float g1 = (m1 * lc1 + m2 * l1) * gravity * s1 + m2 * lc2 * gravity * s12; - float g2 = m2 * lc2 * gravity * s12; - - return StateVector{ g1, g2 }; - } - }; - - class TestEulerLagrangeSolver : public ::testing::Test - { - protected: - dynamics::EulerLagrangeSolver solver1Dof; - dynamics::EulerLagrangeSolver solver2Dof; - SimplePendulum pendulum; - TwoLinkPlanarArm twoLinkArm; - }; -} - -TEST_F(TestEulerLagrangeSolver, forward_dynamics_at_rest_returns_gravity_acceleration) -{ - math::Vector q{ 0.5f }; // 0.5 rad from vertical - math::Vector qDot{}; - math::Vector tau{}; - - auto qDDot = solver1Dof.ForwardDynamics(pendulum, q, qDot, tau); - - // qDDot = -g * sin(q) / l = -9.81 * sin(0.5) - float expected = -SimplePendulum::gravity * std::sin(0.5f) / SimplePendulum::length; - EXPECT_NEAR(qDDot.at(0, 0), expected, 1e-4f); -} - -TEST_F(TestEulerLagrangeSolver, inverse_dynamics_at_rest_returns_gravity_compensation) -{ - math::Vector q{ 0.5f }; - math::Vector qDot{}; - math::Vector qDDot{}; - - auto tau = solver1Dof.InverseDynamics(pendulum, q, qDot, qDDot); - - // tau = g(q) = m * gravity * l * sin(q) - float expected = SimplePendulum::mass * SimplePendulum::gravity * SimplePendulum::length * std::sin(0.5f); - EXPECT_NEAR(tau.at(0, 0), expected, 1e-4f); -} - -TEST_F(TestEulerLagrangeSolver, forward_inverse_roundtrip_consistency) -{ - math::Vector q{ 1.0f }; - math::Vector qDot{ 0.3f }; - math::Vector tau{ 2.5f }; - - auto qDDot = solver1Dof.ForwardDynamics(pendulum, q, qDot, tau); - auto tauRecovered = solver1Dof.InverseDynamics(pendulum, q, qDot, qDDot); - - EXPECT_NEAR(tauRecovered.at(0, 0), tau.at(0, 0), 1e-4f); -} - -TEST_F(TestEulerLagrangeSolver, identity_mass_matrix_forward_dynamics_reduces_to_subtraction) -{ - // A trivial model: M=I, C=0, g=0 - class TrivialModel : public dynamics::EulerLagrangeDynamics - { - public: - MassMatrix ComputeMassMatrix(const StateVector& /* q */) const override - { - return MassMatrix::Identity(); - } - - StateVector ComputeCoriolisTerms(const StateVector& /* q */, const StateVector& /* qDot */) const override - { - return StateVector{}; - } - - StateVector ComputeGravityTerms(const StateVector& /* q */) const override - { - return StateVector{}; - } - }; - - TrivialModel model; - math::Vector q{ 1.0f, 2.0f }; - math::Vector qDot{ 0.5f, 0.5f }; - math::Vector tau{ 3.0f, 4.0f }; - - auto qDDot = solver2Dof.ForwardDynamics(model, q, qDot, tau); - - EXPECT_NEAR(qDDot.at(0, 0), tau.at(0, 0), 1e-3f); - EXPECT_NEAR(qDDot.at(1, 0), tau.at(1, 0), 1e-3f); -} - -TEST_F(TestEulerLagrangeSolver, two_link_forward_inverse_roundtrip) -{ - math::Vector q{ 0.3f, 0.7f }; - math::Vector qDot{ 0.1f, -0.2f }; - math::Vector tau{ 1.5f, 0.8f }; - - auto qDDot = solver2Dof.ForwardDynamics(twoLinkArm, q, qDot, tau); - auto tauRecovered = solver2Dof.InverseDynamics(twoLinkArm, q, qDot, qDDot); - - EXPECT_NEAR(tauRecovered.at(0, 0), tau.at(0, 0), 1e-3f); - EXPECT_NEAR(tauRecovered.at(1, 0), tau.at(1, 0), 1e-3f); -} - -TEST_F(TestEulerLagrangeSolver, two_link_at_rest_returns_gravity_torques) -{ - math::Vector q{ 0.0f, 0.0f }; // hanging straight down - math::Vector qDot{}; - math::Vector tau{}; - - auto qDDot = solver2Dof.ForwardDynamics(twoLinkArm, q, qDot, tau); - - // At q = [0, 0], sin(q1)=0, sin(q1+q2)=0 → g=[0,0] → qDDot=[0,0] - EXPECT_NEAR(qDDot.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(qDDot.at(1, 0), 0.0f, 1e-4f); -} - -TEST_F(TestEulerLagrangeSolver, inverse_dynamics_zero_acceleration_returns_only_coriolis_and_gravity) -{ - math::Vector q{ 0.5f, 0.3f }; - math::Vector qDot{ 1.0f, -0.5f }; - math::Vector qDDot{}; - - auto tau = solver2Dof.InverseDynamics(twoLinkArm, q, qDot, qDDot); - - auto C = twoLinkArm.ComputeCoriolisTerms(q, qDot); - auto g = twoLinkArm.ComputeGravityTerms(q); - - EXPECT_NEAR(tau.at(0, 0), C.at(0, 0) + g.at(0, 0), 1e-4f); - EXPECT_NEAR(tau.at(1, 0), C.at(1, 0) + g.at(1, 0), 1e-4f); -} diff --git a/numerical/dynamics/test/TestNewtonEulerSolver.cpp b/numerical/dynamics/test/TestNewtonEulerSolver.cpp deleted file mode 100644 index 56ad5f7d..00000000 --- a/numerical/dynamics/test/TestNewtonEulerSolver.cpp +++ /dev/null @@ -1,190 +0,0 @@ -#include "numerical/dynamics/NewtonEulerBody.hpp" -#include "numerical/dynamics/NewtonEulerSolver.hpp" -#include -#include - -namespace -{ - // Uniform sphere: I = (2/5) * m * r^2 * Identity - class UniformSphere : public dynamics::NewtonEulerBody - { - public: - static constexpr float mass = 2.0f; - static constexpr float radius = 0.5f; - - float ComputeMass() const override - { - return mass; - } - - InertiaMatrix ComputeInertia() const override - { - float i = 0.4f * mass * radius * radius; // (2/5) * m * r^2 - return InertiaMatrix{ - { i, 0.0f, 0.0f }, - { 0.0f, i, 0.0f }, - { 0.0f, 0.0f, i } - }; - } - }; - - // Asymmetric body with distinct principal moments of inertia - class AsymmetricBody : public dynamics::NewtonEulerBody - { - public: - static constexpr float mass = 3.0f; - - float ComputeMass() const override - { - return mass; - } - - InertiaMatrix ComputeInertia() const override - { - return InertiaMatrix{ - { 1.0f, 0.0f, 0.0f }, - { 0.0f, 2.0f, 0.0f }, - { 0.0f, 0.0f, 3.0f } - }; - } - }; - - class TestNewtonEulerSolver : public ::testing::Test - { - protected: - dynamics::NewtonEulerSolver solver; - UniformSphere sphere; - AsymmetricBody asymmetricBody; - - math::Vector zero3{}; - }; -} - -TEST_F(TestNewtonEulerSolver, forward_dynamics_pure_translation_no_rotation) -{ - math::Vector force{ 6.0f, 0.0f, 0.0f }; - math::Vector torque{}; - - auto result = solver.ForwardDynamics(sphere, force, torque, zero3, zero3); - - // a = F / m = 6 / 2 = 3 - EXPECT_NEAR(result.linear.at(0, 0), 3.0f, 1e-4f); - EXPECT_NEAR(result.linear.at(1, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.linear.at(2, 0), 0.0f, 1e-4f); - - EXPECT_NEAR(result.angular.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.angular.at(1, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.angular.at(2, 0), 0.0f, 1e-4f); -} - -TEST_F(TestNewtonEulerSolver, forward_dynamics_pure_rotation_no_translation) -{ - math::Vector force{}; - math::Vector torque{ 0.0f, 0.0f, 1.0f }; - - auto result = solver.ForwardDynamics(sphere, force, torque, zero3, zero3); - - // alpha = I^{-1} * torque, I = 0.2 * Identity → alpha_z = 1.0 / 0.2 = 5.0 - float inertia = 0.4f * UniformSphere::mass * UniformSphere::radius * UniformSphere::radius; - EXPECT_NEAR(result.angular.at(2, 0), 1.0f / inertia, 1e-3f); - EXPECT_NEAR(result.linear.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.linear.at(1, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.linear.at(2, 0), 0.0f, 1e-4f); -} - -TEST_F(TestNewtonEulerSolver, inverse_dynamics_at_rest_returns_zero) -{ - auto result = solver.InverseDynamics(sphere, zero3, zero3, zero3, zero3); - - EXPECT_NEAR(result.force.at(0, 0), 0.0f, 1e-6f); - EXPECT_NEAR(result.force.at(1, 0), 0.0f, 1e-6f); - EXPECT_NEAR(result.force.at(2, 0), 0.0f, 1e-6f); - EXPECT_NEAR(result.torque.at(0, 0), 0.0f, 1e-6f); - EXPECT_NEAR(result.torque.at(1, 0), 0.0f, 1e-6f); - EXPECT_NEAR(result.torque.at(2, 0), 0.0f, 1e-6f); -} - -TEST_F(TestNewtonEulerSolver, forward_inverse_roundtrip_consistency) -{ - math::Vector force{ 1.0f, -2.0f, 3.0f }; - math::Vector torque{ 0.5f, -0.3f, 0.8f }; - math::Vector linearVel{ 0.1f, 0.2f, -0.1f }; - math::Vector angularVel{ 0.3f, -0.5f, 0.2f }; - - auto accel = solver.ForwardDynamics(sphere, force, torque, linearVel, angularVel); - auto recovered = solver.InverseDynamics(sphere, accel.linear, accel.angular, linearVel, angularVel); - - EXPECT_NEAR(recovered.force.at(0, 0), force.at(0, 0), 1e-3f); - EXPECT_NEAR(recovered.force.at(1, 0), force.at(1, 0), 1e-3f); - EXPECT_NEAR(recovered.force.at(2, 0), force.at(2, 0), 1e-3f); - EXPECT_NEAR(recovered.torque.at(0, 0), torque.at(0, 0), 1e-3f); - EXPECT_NEAR(recovered.torque.at(1, 0), torque.at(1, 0), 1e-3f); - EXPECT_NEAR(recovered.torque.at(2, 0), torque.at(2, 0), 1e-3f); -} - -TEST_F(TestNewtonEulerSolver, gyroscopic_effect_on_asymmetric_body) -{ - // Spinning about z-axis with no external torque → gyroscopic coupling - math::Vector force{}; - math::Vector torque{}; - math::Vector angularVel{ 0.0f, 0.0f, 1.0f }; - - auto result = solver.ForwardDynamics(asymmetricBody, force, torque, zero3, angularVel); - - // omega x (I * omega) = [0,0,1] x [0,0,3] = [0*3 - 1*0, 1*0 - 0*3, 0*0 - 0*0] = [0,0,0] - // When omega is aligned with a principal axis, gyroscopic term vanishes - EXPECT_NEAR(result.angular.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.angular.at(1, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.angular.at(2, 0), 0.0f, 1e-4f); -} - -TEST_F(TestNewtonEulerSolver, gyroscopic_effect_off_principal_axis) -{ - // Spinning about combined axes on asymmetric body → non-zero gyroscopic term - math::Vector force{}; - math::Vector torque{}; - math::Vector angularVel{ 1.0f, 1.0f, 0.0f }; - - auto result = solver.ForwardDynamics(asymmetricBody, force, torque, zero3, angularVel); - - // I*omega = [1*1, 2*1, 0] = [1, 2, 0] - // omega x I*omega = [1,1,0] x [1,2,0] = [0-0, 0-0, 2-1] = [0, 0, 1] - // alpha = I^{-1} * (0 - [0,0,1]) = [0, 0, -1/3] - EXPECT_NEAR(result.angular.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.angular.at(1, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.angular.at(2, 0), -1.0f / 3.0f, 1e-3f); -} - -TEST_F(TestNewtonEulerSolver, body_frame_coriolis_effect) -{ - // Linear velocity + angular velocity → omega x v contributes to forward dynamics - math::Vector force{}; - math::Vector torque{}; - math::Vector linearVel{ 1.0f, 0.0f, 0.0f }; - math::Vector angularVel{ 0.0f, 0.0f, 1.0f }; - - auto result = solver.ForwardDynamics(sphere, force, torque, linearVel, angularVel); - - // a = F/m - omega x v = 0 - [0,0,1] x [1,0,0] = -[0,0,0 x 1,0,0] = -[0*0-1*0, 1*1-0*0, 0*0-0*1] = -[0, 1, 0] - EXPECT_NEAR(result.linear.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(result.linear.at(1, 0), -1.0f, 1e-4f); - EXPECT_NEAR(result.linear.at(2, 0), 0.0f, 1e-4f); -} - -TEST_F(TestNewtonEulerSolver, asymmetric_body_forward_inverse_roundtrip) -{ - math::Vector force{ 3.0f, -1.0f, 2.0f }; - math::Vector torque{ 1.0f, 0.5f, -0.7f }; - math::Vector linearVel{ 0.5f, -0.3f, 0.8f }; - math::Vector angularVel{ 0.2f, 0.4f, -0.6f }; - - auto accel = solver.ForwardDynamics(asymmetricBody, force, torque, linearVel, angularVel); - auto recovered = solver.InverseDynamics(asymmetricBody, accel.linear, accel.angular, linearVel, angularVel); - - EXPECT_NEAR(recovered.force.at(0, 0), force.at(0, 0), 1e-3f); - EXPECT_NEAR(recovered.force.at(1, 0), force.at(1, 0), 1e-3f); - EXPECT_NEAR(recovered.force.at(2, 0), force.at(2, 0), 1e-3f); - EXPECT_NEAR(recovered.torque.at(0, 0), torque.at(0, 0), 1e-3f); - EXPECT_NEAR(recovered.torque.at(1, 0), torque.at(1, 0), 1e-3f); - EXPECT_NEAR(recovered.torque.at(2, 0), torque.at(2, 0), 1e-3f); -} diff --git a/numerical/dynamics/test/TestRecursiveNewtonEuler.cpp b/numerical/dynamics/test/TestRecursiveNewtonEuler.cpp deleted file mode 100644 index e6f72902..00000000 --- a/numerical/dynamics/test/TestRecursiveNewtonEuler.cpp +++ /dev/null @@ -1,206 +0,0 @@ -#include "numerical/dynamics/RecursiveNewtonEuler.hpp" -#include -#include - -namespace -{ - constexpr float gravity = 9.81f; - const math::Vector gravityVec{ 0.0f, 0.0f, -gravity }; - - // 1-DOF simple pendulum: single link rotating about z-axis - // Link hangs along x-axis, joint at origin, CoM at (l/2, 0, 0) - dynamics::RevoluteJointLink MakePendulumLink(float mass, float length) - { - float I = mass * length * length / 12.0f; // thin rod about CoM - - return dynamics::RevoluteJointLink{ - mass, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, // z-axis rotation - math::Vector{}, // joint at parent origin - math::Vector{ length / 2.0f, 0.0f, 0.0f } // CoM at half-length - }; - } - - // 2-DOF planar arm: two links rotating about z-axis in x-y plane - std::array, 2> MakeTwoLinkArm( - float m1, float l1, float m2, float l2) - { - float I1 = m1 * l1 * l1 / 12.0f; - float I2 = m2 * l2 * l2 / 12.0f; - - dynamics::RevoluteJointLink link1{ - m1, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I1, 0.0f }, - { 0.0f, 0.0f, I1 } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ l1 / 2.0f, 0.0f, 0.0f } - }; - - dynamics::RevoluteJointLink link2{ - m2, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I2, 0.0f }, - { 0.0f, 0.0f, I2 } }, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{ l1, 0.0f, 0.0f }, // joint 2 at end of link 1 - math::Vector{ l2 / 2.0f, 0.0f, 0.0f } - }; - - return { link1, link2 }; - } - - class TestRecursiveNewtonEuler : public ::testing::Test - { - protected: - dynamics::RecursiveNewtonEuler rnea1; - dynamics::RecursiveNewtonEuler rnea2; - }; -} - -TEST_F(TestRecursiveNewtonEuler, single_link_gravity_torque_at_horizontal) -{ - // Pendulum rotating about y-axis in x-z plane, horizontal at q=0 (link along x) - float mass = 1.0f; - float length = 1.0f; - float I = mass * length * length / 12.0f; - - dynamics::RevoluteJointLink link{ - mass, - math::SquareMatrix{ - { 0.0f, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - math::Vector{ 0.0f, 1.0f, 0.0f }, // y-axis rotation - math::Vector{}, // joint at parent origin - math::Vector{ length / 2.0f, 0.0f, 0.0f } // CoM at half-length - }; - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{}; - - auto tau = rnea1.InverseDynamics(links, q, qDot, qDDot, gravityVec); - - // At q=0: link along x, gravity in -z, rotating about y-axis - // Fictitious base acceleration = [0, 0, g], force on CoM = [0, 0, mg] - // Torque = rCoM x force = [l/2, 0, 0] x [0, 0, mg] = [0, -mg*l/2, 0] - // Projected on y-axis: tau = -m*g*l/2 - float expected = -mass * gravity * (length / 2.0f); - EXPECT_NEAR(tau.at(0, 0), expected, 0.01f); -} - -TEST_F(TestRecursiveNewtonEuler, single_link_zero_gravity_zero_motion_gives_zero_torque) -{ - auto link = MakePendulumLink(1.0f, 1.0f); - std::array, 1> links = { link }; - - math::Vector q{ 0.5f }; - math::Vector qDot{}; - math::Vector qDDot{}; - math::Vector zeroGravity{}; - - auto tau = rnea1.InverseDynamics(links, q, qDot, qDDot, zeroGravity); - - // No gravity, no motion → zero torque - EXPECT_NEAR(tau.at(0, 0), 0.0f, 1e-5f); -} - -TEST_F(TestRecursiveNewtonEuler, single_link_pure_acceleration_no_gravity) -{ - float mass = 2.0f; - float length = 1.0f; - auto link = MakePendulumLink(mass, length); - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{ 1.0f }; // 1 rad/s^2 - math::Vector zeroGravity{}; - - auto tau = rnea1.InverseDynamics(links, q, qDot, qDDot, zeroGravity); - - // For a thin rod about the end: I_end = m*l^2/3 - // Torque = I_end * qDDot = 2 * 1 / 3 * 1 = 0.6667 - float I_end = mass * length * length / 3.0f; - EXPECT_NEAR(tau.at(0, 0), I_end * 1.0f, 1e-3f); -} - -TEST_F(TestRecursiveNewtonEuler, two_link_zero_gravity_zero_motion_gives_zero_torque) -{ - auto links = MakeTwoLinkArm(1.0f, 1.0f, 1.0f, 1.0f); - - math::Vector q{ 0.0f, 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{}; - math::Vector zeroGravity{}; - - auto tau = rnea2.InverseDynamics(links, q, qDot, qDDot, zeroGravity); - - EXPECT_NEAR(tau.at(0, 0), 0.0f, 1e-4f); - EXPECT_NEAR(tau.at(1, 0), 0.0f, 1e-4f); -} - -TEST_F(TestRecursiveNewtonEuler, two_link_joint2_acceleration_only) -{ - float m1 = 1.0f, l1 = 1.0f, m2 = 1.0f, l2 = 1.0f; - auto links = MakeTwoLinkArm(m1, l1, m2, l2); - - math::Vector q{ 0.0f, 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{ 0.0f, 1.0f }; // only joint 2 accelerates - math::Vector zeroGravity{}; - - auto tau = rnea2.InverseDynamics(links, q, qDot, qDDot, zeroGravity); - - // Joint 2 torque = I2_end * qDDot2 = m2*l2^2/3 * 1 - float I2_end = m2 * l2 * l2 / 3.0f; - EXPECT_NEAR(tau.at(1, 0), I2_end, 1e-3f); - - // Joint 1 should also feel a reaction torque (coupling term) - EXPECT_TRUE(std::abs(tau.at(0, 0)) > 1e-4f); -} - -TEST_F(TestRecursiveNewtonEuler, single_link_centrifugal_term) -{ - // Link spinning at constant velocity → centrifugal force on CoM - float mass = 1.0f; - float length = 1.0f; - auto link = MakePendulumLink(mass, length); - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - math::Vector qDot{ 2.0f }; // spinning at 2 rad/s - math::Vector qDDot{}; // constant velocity - math::Vector zeroGravity{}; - - auto tau = rnea1.InverseDynamics(links, q, qDot, qDDot, zeroGravity); - - // For a single link rotating in a plane, centrifugal force is radial - // and doesn't produce torque about the rotation axis → tau ≈ 0 - EXPECT_NEAR(tau.at(0, 0), 0.0f, 1e-4f); -} - -TEST_F(TestRecursiveNewtonEuler, two_link_symmetry_check) -{ - // Two identical links, both at q=0, both accelerating equally - auto links = MakeTwoLinkArm(1.0f, 1.0f, 1.0f, 1.0f); - - math::Vector q{ 0.0f, 0.0f }; - math::Vector qDot{}; - math::Vector qDDot{ 1.0f, 1.0f }; - math::Vector zeroGravity{}; - - auto tau = rnea2.InverseDynamics(links, q, qDot, qDDot, zeroGravity); - - // Joint 1 should require more torque than joint 2 (it carries the full chain) - EXPECT_GT(std::abs(tau.at(0, 0)), std::abs(tau.at(1, 0))); -} diff --git a/numerical/kinematics/CMakeLists.txt b/numerical/kinematics/CMakeLists.txt deleted file mode 100644 index effff4f2..00000000 --- a/numerical/kinematics/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -numerical_add_header_library(numerical.kinematics) - -target_include_directories(numerical.kinematics ${NUMERICAL_VISIBILITY} - "$" - "$" -) - -target_link_libraries(numerical.kinematics ${NUMERICAL_VISIBILITY} - numerical.dynamics - numerical.math - numerical.solver -) - -target_sources(numerical.kinematics PRIVATE - ForwardKinematics.hpp - InverseKinematics.hpp -) - -numerical_add_coverage_sources(numerical.kinematics - ForwardKinematics.cpp - InverseKinematics.cpp -) - -add_subdirectory(test) diff --git a/numerical/kinematics/ForwardKinematics.cpp b/numerical/kinematics/ForwardKinematics.cpp deleted file mode 100644 index 61f86caf..00000000 --- a/numerical/kinematics/ForwardKinematics.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "numerical/kinematics/ForwardKinematics.hpp" - -namespace kinematics -{ - template class ForwardKinematics; - template class ForwardKinematics; - template class ForwardKinematics; -} diff --git a/numerical/kinematics/ForwardKinematics.hpp b/numerical/kinematics/ForwardKinematics.hpp deleted file mode 100644 index 74a024b7..00000000 --- a/numerical/kinematics/ForwardKinematics.hpp +++ /dev/null @@ -1,70 +0,0 @@ -#pragma once - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -#include "numerical/dynamics/RevoluteJointLink.hpp" -#include "numerical/math/CompilerOptimizations.hpp" -#include "numerical/math/Geometry3D.hpp" -#include "numerical/math/Matrix.hpp" -#include - -namespace kinematics -{ - template - class ForwardKinematics - { - static_assert(std::is_floating_point_v, - "ForwardKinematics only supports floating-point types"); - static_assert(NumLinks > 0, "Number of links must be positive"); - - public: - using Vector3 = math::Vector; - using Matrix3 = math::SquareMatrix; - using JointVector = math::Vector; - using LinkArray = std::array, NumLinks>; - using PositionArray = std::array; - - ForwardKinematics() = default; - - // Computes the 3D positions of all joints (including base at index 0 - // and end-effector at index NumLinks) given link descriptions and - // joint angles. - OPTIMIZE_FOR_SPEED PositionArray Compute(const LinkArray& links, - const JointVector& q) const; - }; - - template - OPTIMIZE_FOR_SPEED - typename ForwardKinematics::PositionArray - ForwardKinematics::Compute(const LinkArray& links, - const JointVector& q) const - { - PositionArray positions{}; - positions[0] = Vector3{}; - - auto R = Matrix3::Identity(); - - for (std::size_t i = 0; i < NumLinks; ++i) - { - R = R * math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); - - Vector3 linkExtent; - if (i + 1 < NumLinks) - linkExtent = links[i + 1].parentToJoint; - else - linkExtent = links[i].jointToCoM * T(2); - - positions[i + 1] = positions[i] + R * linkExtent; - } - - return positions; - } - -#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD - extern template class ForwardKinematics; - extern template class ForwardKinematics; - extern template class ForwardKinematics; -#endif -} diff --git a/numerical/kinematics/InverseKinematics.cpp b/numerical/kinematics/InverseKinematics.cpp deleted file mode 100644 index dbefe127..00000000 --- a/numerical/kinematics/InverseKinematics.cpp +++ /dev/null @@ -1,8 +0,0 @@ -#include "numerical/kinematics/InverseKinematics.hpp" - -namespace kinematics -{ - template class InverseKinematics; - template class InverseKinematics; - template class InverseKinematics; -} diff --git a/numerical/kinematics/InverseKinematics.hpp b/numerical/kinematics/InverseKinematics.hpp deleted file mode 100644 index db346739..00000000 --- a/numerical/kinematics/InverseKinematics.hpp +++ /dev/null @@ -1,173 +0,0 @@ -#pragma once - -#if defined(__GNUC__) || defined(__clang__) -#pragma GCC optimize("O3", "fast-math") -#endif - -#include "infra/util/ReallyAssert.hpp" -#include "numerical/dynamics/RevoluteJointLink.hpp" -#include "numerical/kinematics/ForwardKinematics.hpp" -#include "numerical/math/CompilerOptimizations.hpp" -#include "numerical/math/Geometry3D.hpp" -#include "numerical/math/Matrix.hpp" -#include "numerical/solvers/GaussianElimination.hpp" -#include -#include -#include - -namespace kinematics -{ - template - struct InverseKinematicsConfig - { - static_assert(std::is_floating_point_v, - "InverseKinematicsConfig only supports floating-point types"); - - T dampingFactor = T(0.1); - T tolerance = T(1e-4); - std::size_t maxIterations = 100; - }; - - template - struct InverseKinematicsResult - { - math::Vector q; - T finalError; - std::size_t iterations; - bool converged; - }; - - template - class InverseKinematics - { - static_assert(std::is_floating_point_v, - "InverseKinematics only supports floating-point types"); - static_assert(NumLinks > 0, "Number of links must be positive"); - - public: - using Vector3 = math::Vector3; - using Matrix3 = math::Matrix3; - using JointVector = math::Vector; - using Jacobian = math::Matrix; - using LinkArray = std::array, NumLinks>; - using PositionArray = std::array; - - explicit InverseKinematics(InverseKinematicsConfig cfg = {}); - - OPTIMIZE_FOR_SPEED InverseKinematicsResult Solve( - const LinkArray& links, - const Vector3& target, - const JointVector& initialQ) const; - - private: - Jacobian ComputeJacobian( - const LinkArray& links, - const JointVector& q, - const PositionArray& positions) const; - - InverseKinematicsConfig config; - ForwardKinematics fk; - mutable solvers::GaussianElimination solver; - }; - - template - InverseKinematics::InverseKinematics(InverseKinematicsConfig cfg) - : config(cfg) - { - really_assert(config.dampingFactor > T(0)); - really_assert(config.tolerance > T(0)); - } - - template - OPTIMIZE_FOR_SPEED - InverseKinematicsResult - InverseKinematics::Solve( - const LinkArray& links, - const Vector3& target, - const JointVector& initialQ) const - { - JointVector q = initialQ; - - std::size_t iter = 0; - T error = T(0); - - for (; iter < config.maxIterations; ++iter) - { - auto positions = fk.Compute(links, q); - const Vector3& eePos = positions[NumLinks]; - - Vector3 e = target - eePos; - error = math::VectorNorm(e); - - if (error < config.tolerance) - return { q, error, iter, true }; - - Jacobian J = ComputeJacobian(links, q, positions); - - math::SquareMatrix JJt{}; - for (std::size_t r = 0; r < 3; ++r) - for (std::size_t c = 0; c < 3; ++c) - { - T sum = T(0); - for (std::size_t k = 0; k < NumLinks; ++k) - sum += J.at(r, k) * J.at(c, k); - JJt.at(r, c) = sum; - } - - T lambda2 = config.dampingFactor * config.dampingFactor; - JJt.at(0, 0) += lambda2; - JJt.at(1, 1) += lambda2; - JJt.at(2, 2) += lambda2; - - Vector3 y = solver.Solve(JJt, e); - - for (std::size_t i = 0; i < NumLinks; ++i) - { - T dq = T(0); - for (std::size_t r = 0; r < 3; ++r) - dq += J.at(r, i) * y.at(r, 0); - q.at(i, 0) += dq; - } - } - - auto positions = fk.Compute(links, q); - Vector3 e = target - positions[NumLinks]; - error = math::VectorNorm(e); - - return { q, error, iter, false }; - } - - template - typename InverseKinematics::Jacobian - InverseKinematics::ComputeJacobian( - const LinkArray& links, - const JointVector& q, - const PositionArray& positions) const - { - const Vector3& eePos = positions[NumLinks]; - - Matrix3 R = Matrix3::Identity(); - Jacobian J{}; - - for (std::size_t i = 0; i < NumLinks; ++i) - { - Vector3 worldAxis = R * links[i].jointAxis; - Vector3 jointToEe = eePos - positions[i]; - Vector3 col = math::CrossProduct(worldAxis, jointToEe); - - J.at(0, i) = col.at(0, 0); - J.at(1, i) = col.at(1, 0); - J.at(2, i) = col.at(2, 0); - - R = R * math::RotationAboutAxis(links[i].jointAxis, q.at(i, 0)); - } - - return J; - } - -#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD - extern template class InverseKinematics; - extern template class InverseKinematics; - extern template class InverseKinematics; -#endif -} diff --git a/numerical/kinematics/test/CMakeLists.txt b/numerical/kinematics/test/CMakeLists.txt deleted file mode 100644 index 8a618827..00000000 --- a/numerical/kinematics/test/CMakeLists.txt +++ /dev/null @@ -1,13 +0,0 @@ -add_executable(numerical.kinematics_test) -emil_build_for(numerical.kinematics_test BOOL NUMERICAL_TOOLBOX_BUILD_TESTS) -emil_add_test(numerical.kinematics_test) - -target_link_libraries(numerical.kinematics_test PUBLIC - gmock_main - numerical.kinematics -) - -target_sources(numerical.kinematics_test PRIVATE - TestForwardKinematics.cpp - TestInverseKinematics.cpp -) diff --git a/numerical/kinematics/test/TestForwardKinematics.cpp b/numerical/kinematics/test/TestForwardKinematics.cpp deleted file mode 100644 index d307df63..00000000 --- a/numerical/kinematics/test/TestForwardKinematics.cpp +++ /dev/null @@ -1,300 +0,0 @@ -#include "numerical/kinematics/ForwardKinematics.hpp" -#include -#include -#include - -namespace -{ - constexpr float tolerance = 5e-4f; - constexpr float pi = std::numbers::pi_v; - - dynamics::RevoluteJointLink MakeLink(float mass, float length, - const math::Vector& axis, - const math::Vector& parentToJoint, - const math::Vector& jointToCoM) - { - float I = mass * length * length / 12.0f; - - return dynamics::RevoluteJointLink{ - mass, - math::SquareMatrix{ - { I, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - axis, - parentToJoint, - jointToCoM - }; - } - - dynamics::RevoluteJointLink MakePlanarLink(float mass, float length, - const math::Vector& parentToJoint) - { - return MakeLink(mass, length, - math::Vector{ 0.0f, 0.0f, 1.0f }, - parentToJoint, - math::Vector{ length / 2.0f, 0.0f, 0.0f }); - } - - class TestForwardKinematics : public ::testing::Test - { - protected: - kinematics::ForwardKinematics fk1; - kinematics::ForwardKinematics fk2; - kinematics::ForwardKinematics fk3; - }; -} - -TEST_F(TestForwardKinematics, single_link_at_zero_angle) -{ - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; - - math::Vector q{ 0.0f }; - auto positions = fk1.Compute(links, q); - - EXPECT_NEAR(positions[0].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[0].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[0].at(2, 0), 0.0f, tolerance); - - EXPECT_NEAR(positions[1].at(0, 0), length, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(2, 0), 0.0f, tolerance); -} - -TEST_F(TestForwardKinematics, single_link_rotated_90_degrees) -{ - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; - - math::Vector q{ pi / 2.0f }; - auto positions = fk1.Compute(links, q); - - EXPECT_NEAR(positions[1].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), length, tolerance); - EXPECT_NEAR(positions[1].at(2, 0), 0.0f, tolerance); -} - -TEST_F(TestForwardKinematics, single_link_rotated_180_degrees) -{ - float length = 2.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; - - math::Vector q{ pi }; - auto positions = fk1.Compute(links, q); - - EXPECT_NEAR(positions[1].at(0, 0), -length, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(2, 0), 0.0f, tolerance); -} - -TEST_F(TestForwardKinematics, two_link_planar_both_zero) -{ - float l1 = 1.0f, l2 = 0.5f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.5f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector q{ 0.0f, 0.0f }; - auto positions = fk2.Compute(links, q); - - EXPECT_NEAR(positions[0].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[2].at(0, 0), l1 + l2, tolerance); - EXPECT_NEAR(positions[2].at(1, 0), 0.0f, tolerance); -} - -TEST_F(TestForwardKinematics, two_link_planar_first_joint_90) -{ - float l1 = 1.0f, l2 = 1.0f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(1.0f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector q{ pi / 2.0f, 0.0f }; - auto positions = fk2.Compute(links, q); - - EXPECT_NEAR(positions[1].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), l1, tolerance); - - EXPECT_NEAR(positions[2].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[2].at(1, 0), l1 + l2, tolerance); -} - -TEST_F(TestForwardKinematics, two_link_planar_second_joint_90) -{ - float l1 = 1.0f, l2 = 1.0f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(1.0f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector q{ 0.0f, pi / 2.0f }; - auto positions = fk2.Compute(links, q); - - EXPECT_NEAR(positions[1].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); - - EXPECT_NEAR(positions[2].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[2].at(1, 0), l2, tolerance); -} - -TEST_F(TestForwardKinematics, two_link_folded_back) -{ - float l1 = 1.0f, l2 = 1.0f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(1.0f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector q{ 0.0f, pi }; - auto positions = fk2.Compute(links, q); - - EXPECT_NEAR(positions[1].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[2].at(0, 0), l1 - l2, tolerance); - EXPECT_NEAR(positions[2].at(1, 0), 0.0f, tolerance); -} - -TEST_F(TestForwardKinematics, y_axis_rotation_single_link) -{ - float length = 1.0f; - auto link = MakeLink(1.0f, length, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{}, - math::Vector{ length / 2.0f, 0.0f, 0.0f }); - std::array, 1> links = { link }; - - math::Vector q{ pi / 2.0f }; - auto positions = fk1.Compute(links, q); - - EXPECT_NEAR(positions[1].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(std::sqrt(positions[1].at(0, 0) * positions[1].at(0, 0) + - positions[1].at(2, 0) * positions[1].at(2, 0)), - length, tolerance); -} - -TEST_F(TestForwardKinematics, base_position_always_origin) -{ - float length = 1.5f; - auto link = MakePlanarLink(2.0f, length, math::Vector{}); - std::array, 1> links = { link }; - - for (float angle : { 0.0f, pi / 4.0f, pi / 2.0f, pi, -pi / 3.0f }) - { - math::Vector q{ angle }; - auto positions = fk1.Compute(links, q); - - EXPECT_NEAR(positions[0].at(0, 0), 0.0f, 1e-6f); - EXPECT_NEAR(positions[0].at(1, 0), 0.0f, 1e-6f); - EXPECT_NEAR(positions[0].at(2, 0), 0.0f, 1e-6f); - } -} - -TEST_F(TestForwardKinematics, end_effector_distance_equals_link_length) -{ - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; - - for (float angle : { 0.0f, pi / 6.0f, pi / 3.0f, pi / 2.0f, 2.0f * pi / 3.0f }) - { - math::Vector q{ angle }; - auto positions = fk1.Compute(links, q); - - float dx = positions[1].at(0, 0) - positions[0].at(0, 0); - float dy = positions[1].at(1, 0) - positions[0].at(1, 0); - float dz = positions[1].at(2, 0) - positions[0].at(2, 0); - float dist = std::sqrt(dx * dx + dy * dy + dz * dz); - - EXPECT_NEAR(dist, length, tolerance); - } -} - -TEST_F(TestForwardKinematics, three_link_all_zero) -{ - float l1 = 1.0f, l2 = 0.8f, l3 = 0.6f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - auto link3 = MakePlanarLink(0.6f, l3, math::Vector{ l2, 0.0f, 0.0f }); - std::array, 3> links = { link1, link2, link3 }; - - math::Vector q{ 0.0f, 0.0f, 0.0f }; - auto positions = fk3.Compute(links, q); - - EXPECT_NEAR(positions[0].at(0, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[1].at(0, 0), l1, tolerance); - EXPECT_NEAR(positions[2].at(0, 0), l1 + l2, tolerance); - EXPECT_NEAR(positions[3].at(0, 0), l1 + l2 + l3, tolerance); - - for (int i = 0; i <= 3; ++i) - { - EXPECT_NEAR(positions[i].at(1, 0), 0.0f, tolerance); - EXPECT_NEAR(positions[i].at(2, 0), 0.0f, tolerance); - } -} - -TEST_F(TestForwardKinematics, two_link_total_reach) -{ - float l1 = 1.0f, l2 = 0.5f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(1.0f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - for (float q1 : { 0.0f, pi / 4.0f, pi / 2.0f }) - { - for (float q2 : { 0.0f, pi / 3.0f, pi, -pi / 2.0f }) - { - math::Vector q{ q1, q2 }; - auto positions = fk2.Compute(links, q); - - float x = positions[2].at(0, 0); - float y = positions[2].at(1, 0); - float z = positions[2].at(2, 0); - float dist = std::sqrt(x * x + y * y + z * z); - - EXPECT_LE(dist, l1 + l2 + 1e-3f); - } - } -} - -TEST_F(TestForwardKinematics, spatial_3dof_vertical_base) -{ - float l1 = 0.5f, l2 = 0.4f, l3 = 0.3f; - - auto link1 = MakeLink(1.0f, l1, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ 0.0f, 0.0f, l1 / 2.0f }); - - auto link2 = MakeLink(0.8f, l2, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{ 0.0f, 0.0f, l1 }, - math::Vector{ l2 / 2.0f, 0.0f, 0.0f }); - - auto link3 = MakeLink(0.6f, l3, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{ l2, 0.0f, 0.0f }, - math::Vector{ l3 / 2.0f, 0.0f, 0.0f }); - - std::array, 3> links = { link1, link2, link3 }; - - math::Vector q{ 0.0f, 0.0f, 0.0f }; - auto positions = fk3.Compute(links, q); - - EXPECT_NEAR(positions[1].at(2, 0), l1, tolerance); - EXPECT_NEAR(positions[1].at(0, 0), 0.0f, tolerance); - - EXPECT_NEAR(positions[2].at(0, 0), l2, tolerance); - EXPECT_NEAR(positions[2].at(2, 0), l1, tolerance); - - EXPECT_NEAR(positions[3].at(0, 0), l2 + l3, tolerance); - EXPECT_NEAR(positions[3].at(2, 0), l1, tolerance); -} diff --git a/numerical/kinematics/test/TestInverseKinematics.cpp b/numerical/kinematics/test/TestInverseKinematics.cpp deleted file mode 100644 index 3937c109..00000000 --- a/numerical/kinematics/test/TestInverseKinematics.cpp +++ /dev/null @@ -1,270 +0,0 @@ -#include "numerical/kinematics/ForwardKinematics.hpp" -#include "numerical/kinematics/InverseKinematics.hpp" -#include -#include -#include - -namespace -{ - constexpr float tolerance = 1e-3f; - constexpr float pi = std::numbers::pi_v; - - dynamics::RevoluteJointLink MakeLink(float mass, float length, - const math::Vector& axis, - const math::Vector& parentToJoint, - const math::Vector& jointToCoM) - { - float I = mass * length * length / 12.0f; - - return dynamics::RevoluteJointLink{ - mass, - math::SquareMatrix{ - { I, 0.0f, 0.0f }, - { 0.0f, I, 0.0f }, - { 0.0f, 0.0f, I } }, - axis, - parentToJoint, - jointToCoM - }; - } - - dynamics::RevoluteJointLink MakePlanarLink(float mass, float length, - const math::Vector& parentToJoint) - { - return MakeLink(mass, length, - math::Vector{ 0.0f, 0.0f, 1.0f }, - parentToJoint, - math::Vector{ length / 2.0f, 0.0f, 0.0f }); - } - - class TestInverseKinematics : public ::testing::Test - { - protected: - kinematics::InverseKinematics ik1; - kinematics::InverseKinematics ik2; - kinematics::InverseKinematics ik3; - kinematics::ForwardKinematics fk1; - kinematics::ForwardKinematics fk2; - kinematics::ForwardKinematics fk3; - }; -} - -TEST_F(TestInverseKinematics, single_link_reaches_target_on_arc) -{ - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; - - math::Vector referenceQ{ pi / 3.0f }; - auto fkRef = fk1.Compute(links, referenceQ); - math::Vector target = fkRef[1]; - - math::Vector initialQ{ 0.0f }; - auto result = ik1.Solve(links, target, initialQ); - - EXPECT_TRUE(result.converged); - EXPECT_NEAR(result.q.at(0, 0), pi / 3.0f, 1e-3f); - - auto positions = fk1.Compute(links, result.q); - EXPECT_NEAR(positions[1].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[1].at(1, 0), target.at(1, 0), tolerance); - EXPECT_NEAR(positions[1].at(2, 0), target.at(2, 0), tolerance); -} - -TEST_F(TestInverseKinematics, starts_at_solution_converges_immediately) -{ - float length = 1.0f; - auto link = MakePlanarLink(1.0f, length, math::Vector{}); - std::array, 1> links = { link }; - - math::Vector initialQ{ pi / 4.0f }; - auto fkPositions = fk1.Compute(links, initialQ); - math::Vector target = fkPositions[1]; - - auto result = ik1.Solve(links, target, initialQ); - - EXPECT_TRUE(result.converged); - EXPECT_LT(result.iterations, 2u); - EXPECT_LT(result.finalError, 1e-4f); -} - -TEST_F(TestInverseKinematics, two_link_planar_reaches_known_position) -{ - float l1 = 1.0f, l2 = 0.8f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ 0.0f, 0.0f }; - math::Vector target{ 0.5f, 1.0f, 0.0f }; - - auto result = ik2.Solve(links, target, initialQ); - - EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), target.at(1, 0), tolerance); - EXPECT_NEAR(positions[2].at(2, 0), target.at(2, 0), tolerance); -} - -TEST_F(TestInverseKinematics, two_link_planar_fully_extended) -{ - float l1 = 1.0f, l2 = 0.5f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.5f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ 0.1f, 0.1f }; - math::Vector target{ l1 + l2 - 0.05f, 0.0f, 0.0f }; - - auto result = ik2.Solve(links, target, initialQ); - - EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), 0.0f, tolerance); -} - -TEST_F(TestInverseKinematics, two_link_planar_folded_elbow_up) -{ - float l1 = 1.0f, l2 = 0.8f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ pi / 4.0f, -pi / 4.0f }; - math::Vector target{ 1.2f, 0.5f, 0.0f }; - - auto result = ik2.Solve(links, target, initialQ); - - EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), target.at(1, 0), tolerance); -} - -TEST_F(TestInverseKinematics, unreachable_target_does_not_converge) -{ - float l1 = 1.0f, l2 = 0.5f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.5f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ 0.0f, 0.0f }; - math::Vector target{ 10.0f, 10.0f, 10.0f }; - - auto result = ik2.Solve(links, target, initialQ); - - EXPECT_FALSE(result.converged); - EXPECT_GT(result.finalError, 1e-4f); - EXPECT_EQ(result.iterations, 100u); -} - -TEST_F(TestInverseKinematics, result_contains_populated_iteration_count_and_error) -{ - float l1 = 1.0f, l2 = 0.8f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ 0.0f, 0.0f }; - math::Vector target{ 0.5f, 1.0f, 0.0f }; - - auto result = ik2.Solve(links, target, initialQ); - - EXPECT_GT(result.iterations, 0u); - EXPECT_LT(result.finalError, 1e-4f); -} - -TEST_F(TestInverseKinematics, custom_damping_factor_still_converges) -{ - float l1 = 1.0f, l2 = 0.8f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - kinematics::InverseKinematicsConfig cfg; - cfg.dampingFactor = 0.5f; - cfg.tolerance = 1e-4f; - cfg.maxIterations = 500; - kinematics::InverseKinematics ikHighDamping{ cfg }; - - math::Vector initialQ{ 0.0f, 0.0f }; - math::Vector target{ 0.5f, 1.0f, 0.0f }; - - auto result = ikHighDamping.Solve(links, target, initialQ); - - EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), target.at(1, 0), tolerance); -} - -TEST_F(TestInverseKinematics, three_link_spatial_reaches_target) -{ - float l1 = 0.5f, l2 = 0.4f, l3 = 0.3f; - - auto link1 = MakeLink(1.0f, l1, - math::Vector{ 0.0f, 0.0f, 1.0f }, - math::Vector{}, - math::Vector{ l1 / 2.0f, 0.0f, 0.0f }); - auto link2 = MakeLink(0.8f, l2, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{ l1, 0.0f, 0.0f }, - math::Vector{ l2 / 2.0f, 0.0f, 0.0f }); - auto link3 = MakeLink(0.6f, l3, - math::Vector{ 0.0f, 1.0f, 0.0f }, - math::Vector{ l2, 0.0f, 0.0f }, - math::Vector{ l3 / 2.0f, 0.0f, 0.0f }); - std::array, 3> links = { link1, link2, link3 }; - - kinematics::InverseKinematicsConfig cfg; - cfg.dampingFactor = 0.05f; - cfg.tolerance = 1e-4f; - cfg.maxIterations = 300; - kinematics::InverseKinematics ik3Spatial{ cfg }; - - math::Vector referenceQ{ pi / 4.0f, pi / 6.0f, -pi / 3.0f }; - auto fkRef = fk3.Compute(links, referenceQ); - math::Vector target = fkRef[3]; - - math::Vector initialQ{ 0.0f, 0.0f, 0.0f }; - auto result = ik3Spatial.Solve(links, target, initialQ); - - EXPECT_TRUE(result.converged); - - auto positions = fk3.Compute(links, result.q); - EXPECT_NEAR(positions[3].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[3].at(1, 0), target.at(1, 0), tolerance); - EXPECT_NEAR(positions[3].at(2, 0), target.at(2, 0), tolerance); -} - -TEST_F(TestInverseKinematics, two_link_target_at_origin_region) -{ - float l1 = 1.0f, l2 = 0.8f; - - auto link1 = MakePlanarLink(1.0f, l1, math::Vector{}); - auto link2 = MakePlanarLink(0.8f, l2, math::Vector{ l1, 0.0f, 0.0f }); - std::array, 2> links = { link1, link2 }; - - math::Vector initialQ{ pi / 3.0f, -2.0f * pi / 3.0f }; - math::Vector target{ 0.4f, 0.0f, 0.0f }; - - auto result = ik2.Solve(links, target, initialQ); - - EXPECT_TRUE(result.converged); - - auto positions = fk2.Compute(links, result.q); - EXPECT_NEAR(positions[2].at(0, 0), target.at(0, 0), tolerance); - EXPECT_NEAR(positions[2].at(1, 0), target.at(1, 0), tolerance); -} diff --git a/roadmap/dynamics/FrictionCompensation/explanation.md b/roadmap/dynamics/FrictionCompensation/explanation.md deleted file mode 100644 index 1a4bd92a..00000000 --- a/roadmap/dynamics/FrictionCompensation/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Friction Compensation — Overview - -## What it is -A feedforward model of joint friction — the sum of **Coulomb** (constant drag), **viscous** -(speed-proportional drag), and **Stribeck** (extra "stiction" that fades as the joint starts -moving) terms — evaluated from joint velocity and added to a torque command to cancel real friction. - -## Why it matters (embedded) -Friction is the dominant un-modeled effect in geared robot joints: it causes stick-slip, tracking -lag, and steady-state error that pure PD/PID struggles to remove. A few multiplies, one `exp`, and -one `tanh` per joint — evaluated in the control loop — recover much of that lost accuracy without a -larger, slower feedback gain that would risk instability. - -## How it works (intuition) -Plot friction torque against velocity and you get a characteristic curve: a tall "breakaway" spike -near zero speed (stiction), dropping into a Coulomb plateau, then rising linearly (viscous) as speed -increases. The model reproduces that curve. Because the ideal curve is discontinuous at zero -velocity (the `sign` flip), a smooth `tanh` boundary layer replaces the hard sign so the feedforward -command does not chatter as the joint reverses direction. - -## Key parameters -- **F_c (Coulomb)** — constant drag magnitude once sliding. -- **F_s (stiction)** — higher breakaway level at near-zero speed (`F_s ≥ F_c`). -- **F_v (viscous)** — drag proportional to velocity. -- **v_s (Stribeck velocity)** — how quickly stiction fades into the Coulomb plateau. -- **ε (smoothing width)** — trades zero-crossing smoothness against compensation sharpness. - -## Reference -B. Armstrong-Hélouvry, P. Dupont, C. Canudas de Wit, "A survey of models, analysis tools and -compensation methods for the control of machines with friction," *Automatica*, 30(7), 1994. - -## See also -PD + gravity compensation (M5) and computed-torque control (M12), which add this term to their -control law; `RecursiveNewtonEuler` for the model-based torque it complements. diff --git a/roadmap/dynamics/FrictionCompensation/implementation.md b/roadmap/dynamics/FrictionCompensation/implementation.md deleted file mode 100644 index 7efe290c..00000000 --- a/roadmap/dynamics/FrictionCompensation/implementation.md +++ /dev/null @@ -1,80 +0,0 @@ -# Friction Compensation (Coulomb + Viscous + Stribeck) — Implementation Pseudocode - -> Roadmap ref: #M4 (Tier 1) · Target: `numerical/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct JointFrictionParameters: # one per joint - T coulomb # F_c — kinetic friction magnitude - T stiction # F_s — static/breakaway level (>= F_c) - T viscous # F_v — velocity-proportional coefficient - T stribeckVelocity # v_s — width of the Stribeck dip (> 0) - T stribeckShape # delta — exponent, typically 1 or 2 - T smoothingVelocity# eps — zero-crossing smoothing width (> 0) - -template # static_assert(std::is_floating_point_v); instantiated for float -class FrictionCompensation: - std::array, NumJoints> params -``` - -## Interface - -``` -FrictionCompensation(const std::array, NumJoints>& params) - -# Feedforward friction torque to add to the control law: -Vector Compute(const Vector& qDot) const # hot path - -# Single-joint helper (also reused by the vector form): -static T JointTorque(const JointFrictionParameters& p, T qDot) -``` - -## Algorithm (pseudocode) - -``` -function JointTorque(p, v): # OPTIMIZE_FOR_SPEED - # Stribeck curve: stiction blends down to Coulomb as |v| grows - fall = exp( -(|v| / p.stribeckVelocity) ^ p.stribeckShape ) - level = p.coulomb + (p.stiction - p.coulomb) * fall - # smooth sign (tanh boundary layer) avoids chattering at v = 0 - dir = tanh(v / p.smoothingVelocity) - return level * dir + p.viscous * v - -function Compute(qDot): # OPTIMIZE_FOR_SPEED - for i in 0 .. NumJoints-1: - tau[i] = JointTorque(params[i], qDot[i]) - return tau -``` - -## Complexity & memory - -- `Compute`: `O(NumJoints)` — one `exp` and one `tanh` per joint, no allocation. -- Memory: `O(NumJoints)` parameter structs, all static/stack-resident. - -## Numerical / embedded notes - -- **Feedforward, not feedback:** add the result to the controller output - (`τ_cmd = τ_control + Compute(q̇)`); it composes with PD+gravity (M5) and computed-torque (M12). -- Feed **desired** velocity for a noise-free command, or **measured** velocity for accuracy — the - smoothing width `eps` trades tracking sharpness against chattering either way. -- The ideal `sign(v)` is replaced by `tanh(v/eps)`: too small an `eps` reintroduces limit cycles - near zero velocity; too large under-compensates stiction on slow moves. -- Over-compensation is worse than under (it can drive the joint), so cap each term and validate - `stiction >= coulomb`, `stribeckVelocity > 0`, `smoothingVelocity > 0` at construction. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/dynamics/FrictionCompensation.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`/`JointTorque`, and - `extern template class FrictionCompensation;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/dynamics/FrictionCompensation.cpp` → - `template class FrictionCompensation;` -- Test: `numerical/dynamics/test/TestFrictionCompensation.cpp` -- Doc: `doc/dynamics/FrictionCompensation.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestFrictionCompensation.cpp` → the `_test` target. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/dynamics/FrictionCompensation/tests.md b/roadmap/dynamics/FrictionCompensation/tests.md deleted file mode 100644 index 958f852e..00000000 --- a/roadmap/dynamics/FrictionCompensation/tests.md +++ /dev/null @@ -1,63 +0,0 @@ -# Friction Compensation — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestFrictionCompensation : public ::testing::Test: - # single-joint params: Fc=0.5, Fs=0.8, Fv=0.1, vs=0.05, delta=2, eps=1e-3 - JointFrictionParameters p = MakeParams() - FrictionCompensation friction{ {p} } -# each case below is a TEST_F(TestFrictionCompensation, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -positive_velocity_positive_torque: - Arrange: qDot = +1.0 - Act: tau = Compute(qDot) - Assert: tau[0] > 0 and tau[0] ≈ Fc + Fv*1.0 (dip negligible at high v) - -sign_flips_with_velocity: - Arrange: compare Compute(+v) and Compute(-v) - Assert: tau(+v) ≈ -tau(-v) (odd symmetry) - -viscous_dominates_at_high_speed: - Arrange: sweep large |qDot| - Assert: tau grows ~linearly with slope Fv - -stiction_exceeds_coulomb_near_zero: - Arrange: small v just above eps - Assert: |level| closer to Fs than Fc (Stribeck peak) - -stribeck_decays_to_coulomb: - Arrange: v >> v_s - Assert: level -> Fc within tol - -smooth_through_zero: - Arrange: v = 0 - Assert: tau[0] == 0 (tanh(0)=0), continuous, no jump - -multi_joint_elementwise: - Arrange: FrictionCompensation, distinct params, qDot vector - Assert: each output equals its per-joint JointTorque - -feedforward_additive_cancels_model: - Arrange: plant friction = model; command = -Compute(qDot) - Assert: net joint friction ≈ 0 (compensation cancels) -``` - -## Reference vectors - -- High speed `v=1`, dip≈0: `tau ≈ Fc·tanh(1/eps) + Fv·1 ≈ Fc + Fv`. -- `v=0`: `tau = 0` exactly (odd, smoothed). -- `v = v_s`: `fall = e^{-1}`, `level = Fc + (Fs−Fc)/e`. - -## Edge cases - -- `qDot = 0` — no torque, no NaN from the `^delta` power. -- Very small `eps` — steep but finite slope; assert no overflow. -- `Fs == Fc` — Stribeck term vanishes, pure Coulomb+viscous. -- Negative-going zero crossing — output stays continuous (no chatter). diff --git a/roadmap/dynamics/GenericJointLink/explanation.md b/roadmap/dynamics/GenericJointLink/explanation.md deleted file mode 100644 index 66a0b82f..00000000 --- a/roadmap/dynamics/GenericJointLink/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# Generic Joint Link — Overview - -## What it is -A single struct that describes **either** a revolute (rotating) **or** a prismatic (sliding) joint, -by carrying a joint *type* alongside a unit *axis* and the usual inertial parameters. It generalizes -the revolute-only link so one chain can mix hinges and slides. - -## Why it matters (embedded) -Real machines are rarely all-revolute: SCARA arms, gantry/Cartesian robots, hydraulic rams, and -3-D printers all contain sliding axes. Encoding the joint type once — as a byte-sized enum plus a -shared code path — lets forward kinematics, the Jacobian, and inverse dynamics handle mixed chains -without duplicating an entire link/algorithm family per joint type. - -## How it works (intuition) -Every 1-DOF joint moves along a **screw axis**. A revolute joint spends its motion in the *angular* -part of that screw (it rotates by `q` about the axis); a prismatic joint spends it in the *linear* -part (it slides by `q` along the axis). The link therefore exposes two things: the relative -transform produced by `q`, and which channel — angular or linear — the axis occupies. Downstream -algorithms read those and stay joint-type-agnostic. - -## Key parameters -- **type** — `Revolute` or `Prismatic`. -- **axis** — unit vector; rotation axis (revolute) or slide direction (prismatic). -- **parentToJoint / jointToCoM** — link geometry, unchanged from the revolute link. -- **mass / inertia** — rigid-body inertial parameters. - -## Reference -J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 -(link description and joint transforms). - -## See also -`RevoluteJointLink` (the specialization it generalizes), `RecursiveNewtonEuler` (consumes the -motion subspace), Denavit-Hartenberg parameters (M7), spatial Jacobian (M8). diff --git a/roadmap/dynamics/GenericJointLink/implementation.md b/roadmap/dynamics/GenericJointLink/implementation.md deleted file mode 100644 index 2dfef393..00000000 --- a/roadmap/dynamics/GenericJointLink/implementation.md +++ /dev/null @@ -1,79 +0,0 @@ -# Generic (Revolute / Prismatic) Joint Link — Implementation Pseudocode - -> Roadmap ref: #M1 (Tier 1) · Target: `numerical/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -enum class JointType : uint8_t { Revolute, Prismatic } - -template # static_assert(std::is_floating_point_v); instantiated for float -struct GenericJointLink: - JointType type # revolute (rotate) or prismatic (slide) - T mass - math::SquareMatrix inertia # inertia tensor at CoM, link frame - math::Vector axis # unit joint axis in link frame - math::Vector parentToJoint # fixed joint origin in parent frame - math::Vector jointToCoM # CoM position in link frame -``` - -## Interface - -``` -# Relative transform produced by the joint variable q: -(Matrix3 R, Vector3 p) JointTransform(T q) const # hot path - -# Motion-subspace (screw) split — exactly one is the axis, the other is zero: -Vector3 AngularAxis() const # axis if Revolute else 0 -Vector3 LinearAxis() const # axis if Prismatic else 0 - -# Adapter so existing revolute-only code keeps compiling: -static GenericJointLink FromRevolute(const RevoluteJointLink& link) -``` - -## Algorithm (pseudocode) - -``` -function JointTransform(q): # OPTIMIZE_FOR_SPEED - if type == Revolute: - R = math::RotationAboutAxis(axis, q) # rotate by angle q about axis - p = parentToJoint # origin fixed - else: # Prismatic - R = Identity3 # no rotation - p = parentToJoint + axis * q # slide distance q along axis - return (R, p) - -function AngularAxis(): return (type == Revolute) ? axis : 0 -function LinearAxis(): return (type == Prismatic) ? axis : 0 -``` - -## Complexity & memory - -- `JointTransform`: `O(1)` — one axis-angle rotation (revolute) or one scaled add (prismatic). -- Memory: one enum byte + the existing inertial fields; no growth over `RevoluteJointLink`. - -## Numerical / embedded notes - -- The `(AngularAxis, LinearAxis)` pair *is* the motion-subspace `S`: FK multiplies `JointTransform` - down the chain; the 6×N Jacobian (M8) fills column `i` from these two vectors; RNEA injects - `axis·q̇` into the angular channel (revolute) or the linear channel (prismatic). -- Keep `axis` **unit-normalized** at construction; a non-unit axis silently rescales both the - rotation angle and the slide distance. -- `RotationAboutAxis` (Rodrigues form) avoids gimbal issues and is branch-light — good for the FK loop. -- A `Prismatic` joint contributes no rotation, so its inertia only shifts via the parallel-axis - term along `axis·q`; revolute links keep the existing propagation unchanged. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/dynamics/GenericJointLink.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `JointTransform`, and - `extern template struct GenericJointLink;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/dynamics/GenericJointLink.cpp` → - `template struct GenericJointLink;` -- Test: `numerical/dynamics/test/TestGenericJointLink.cpp` -- Doc: `doc/dynamics/GenericJointLink.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestGenericJointLink.cpp` → the `_test` target. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/dynamics/GenericJointLink/tests.md b/roadmap/dynamics/GenericJointLink/tests.md deleted file mode 100644 index f2041ecf..00000000 --- a/roadmap/dynamics/GenericJointLink/tests.md +++ /dev/null @@ -1,64 +0,0 @@ -# Generic Joint Link — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestGenericJointLink : public ::testing::Test: - # unit axes for readable expectations - Vector3 z = {0, 0, 1} - Vector3 x = {1, 0, 0} - GenericJointLink MakeRevolute(axis, parentToJoint) - GenericJointLink MakePrismatic(axis, parentToJoint) -# each case below is a TEST_F(TestGenericJointLink, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -revolute_rotates_about_axis: - Arrange: revolute about z, q = pi/2 - Act: (R, p) = JointTransform(q) - Assert: R maps x -> y (±tol), p == parentToJoint - -revolute_zero_angle_is_identity: - Arrange: revolute about z, q = 0 - Assert: R == I, p == parentToJoint - -prismatic_translates_along_axis: - Arrange: prismatic along x, parentToJoint = {0,0,0}, q = 0.3 - Act: (R, p) = JointTransform(q) - Assert: R == I, p == {0.3, 0, 0} - -prismatic_keeps_orientation: - Arrange: prismatic along z, sweep q - Assert: R == I for all q (no rotation) - -revolute_motion_subspace: - Arrange: revolute about axis a - Assert: AngularAxis() == a, LinearAxis() == 0 - -prismatic_motion_subspace: - Arrange: prismatic along axis a - Assert: LinearAxis() == a, AngularAxis() == 0 - -from_revolute_matches_legacy: - Arrange: RevoluteJointLink r; g = FromRevolute(r) - Assert: same mass/inertia/axis; JointTransform is a pure rotation - -offset_joint_origin_applied: - Arrange: prismatic along x, parentToJoint = {0,1,0}, q = 0.5 - Assert: p == {0.5, 1, 0} -``` - -## Reference vectors - -- Revolute z, q = π/2: `R·[1,0,0]ᵀ = [0,1,0]ᵀ`. -- Prismatic x, q = d: `p = parentToJoint + [d,0,0]ᵀ`, `R = I`. - -## Edge cases - -- Negative `q` (reverse rotation / retracting slide) mirrors the positive case. -- Non-unit `axis` — assert construction normalizes (or a guard rejects it). -- Full `2π` revolute wrap returns to identity within tolerance. diff --git a/roadmap/kinematics/AnalyticalIkPieper/explanation.md b/roadmap/kinematics/AnalyticalIkPieper/explanation.md deleted file mode 100644 index 93666b86..00000000 --- a/roadmap/kinematics/AnalyticalIkPieper/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Analytical IK (Pieper) — Overview - -## What it is -A **closed-form** inverse-kinematics solver for the classic six-revolute arm whose last three axes -meet at a point (a spherical wrist). Instead of iterating, it computes every joint angle directly with -trigonometry and returns *all* the arm postures — up to eight — that reach a given pose. - -## Why it matters (embedded) -Closed-form IK is exact, has no convergence loop, and runs in constant time — ideal for a hard -real-time controller that cannot afford an iterative solver's worst-case iteration count. Getting *all* -solutions also lets a planner choose the posture that best avoids joint limits or obstacles, something -a single-answer numerical solver cannot offer. - -## How it works (intuition) -Pieper's insight is that a spherical wrist **decouples** the problem. The wrist centre — the point -where the last three axes cross — depends only on the first three joints, so you first back it out of -the target pose and solve a 3-DOF *position* problem for joints 1-3 using plain planar geometry (an -`atan2` for the base and the law of cosines for the elbow, giving shoulder and elbow-up/down branches). -With the arm placed, the leftover rotation from frame 3 to the tool is handled by wrist joints 4-6, -extracted as a set of Euler angles (two branches for the middle wrist joint). Multiplying the branch -counts gives up to eight complete solutions. - -## Key parameters -- **DH table of the 6R arm** — with the spherical-wrist assumption baked in. -- **target pose** — the tool position and orientation to invert. -- **branch selection** — shoulder, elbow, and wrist flips the caller chooses among. - -## Reference -D. L. Pieper, "The Kinematics of Manipulators Under Computer Control," PhD thesis, Stanford -University, 1968. - -## See also -`DenavitHartenberg` (M7) / `SE3Transform` (M6) for the frames, `Cordic` (item 23) for the trig, -`PoseInverseKinematics` (M13, the iterative fallback for non-spherical wrists). diff --git a/roadmap/kinematics/AnalyticalIkPieper/implementation.md b/roadmap/kinematics/AnalyticalIkPieper/implementation.md deleted file mode 100644 index 54ada2f1..00000000 --- a/roadmap/kinematics/AnalyticalIkPieper/implementation.md +++ /dev/null @@ -1,77 +0,0 @@ -# Analytical IK — Pieper (Wrist-Partitioned 6R) — Implementation Pseudocode - -> Roadmap ref: #M21 (Tier 4) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct IkSolutions: - std::array, 8> q # up to 8 branches - std::size_t count # how many are valid / reachable - -template -class AnalyticalIkPieper: # fixed 6R with a spherical wrist - std::array, 6> dh # last three axes intersect at the wrist -``` - -## Interface - -``` -AnalyticalIkPieper(std::array, 6> dh) -IkSolutions Solve(SE3 target) # all real solutions; hot path -``` - -## Algorithm (pseudocode) - -``` -function Solve(target): # OPTIMIZE_FOR_SPEED (closed form, no iteration) - # 1. wrist centre — subtract the tool offset along the approach axis - p_wc = target.p - d6 * (target.R * ẑ) - - # 2. arm (joints 1-3) from the wrist-centre position — planar geometry - θ1 = atan2(p_wc.y, p_wc.x) # and the θ1 + π branch - (r, s) = planar coords of p_wc in the θ1 plane - c3 = (r² + s² - a2² - a3²) / (2·a2·a3) - if |c3| > 1: this branch is unreachable — skip it - θ3 = ±acos(c3) # elbow-up / elbow-down - θ2 = atan2(s, r) - atan2(a3·sinθ3, a2 + a3·cosθ3) - - # 3. wrist (joints 4-6) from the leftover orientation - R03 = rotation of joints 1-3 - R36 = Transpose(R03) * target.R - (θ4, θ5, θ6) = ZYZ-Euler(R36) # two branches: θ5 = ±acos(R36[2,2]) - - # 4. assemble the (≤ 2×2×2 = 8) combinations into IkSolutions - return solutions -``` - -## Complexity & memory - -- `O(1)` — a fixed number of `atan2` / `acos` calls; **no iteration, no Jacobian, no matrix solve**. -- Enumerates up to `8` postures (2 shoulder × 2 elbow × 2 wrist). -- Memory: the 8-solution array; entirely on the stack. - -## Numerical / embedded notes - -- **Reachability filter:** if the law-of-cosines argument `|c3| > 1` the target is out of reach on that - branch — drop it instead of feeding `acos` an out-of-domain value. -- **Wrist singularity** (`θ5 → 0`, axes 4 and 6 align): `θ4` and `θ6` are individually undefined, only - their sum is fixed — pick a convention (e.g. hold `θ4` at its current value) and flag it. -- Requires a **spherical wrist** (last three axes intersect); the partition into position (1-3) and - orientation (4-6) is what makes the closed form possible — reuse M6/M7 for the transforms. -- Enumerate *all* real solutions and let the caller choose (nearest to current `q`, joint-limit-feasible). -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/AnalyticalIkPieper.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Solve`, and - `extern template class AnalyticalIkPieper;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/AnalyticalIkPieper.cpp` → `template class AnalyticalIkPieper;` -- Test: `numerical/kinematics/test/TestAnalyticalIkPieper.cpp` -- Doc: `doc/kinematics/AnalyticalIkPieper.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestAnalyticalIkPieper.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/AnalyticalIkPieper/tests.md b/roadmap/kinematics/AnalyticalIkPieper/tests.md deleted file mode 100644 index 1caade4f..00000000 --- a/roadmap/kinematics/AnalyticalIkPieper/tests.md +++ /dev/null @@ -1,58 +0,0 @@ -# Analytical IK (Pieper) — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestPieperIk : public ::testing::Test: - std::array, 6> puma{ ... } # PUMA-like 6R, spherical wrist - AnalyticalIkPieper ik{ puma } - DenavitHartenberg fk{ puma } # for round-trips -# each case below is a TEST_F(TestPieperIk, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -every_solution_reproduces_target: - Arrange: q_true; target = fk.Forward(q_true) - Act: sols = ik.Solve(target) - Assert: for each k < count: fk.Forward(sols.q[k]) ≈ target - -finds_the_known_configuration: - Assert: some returned branch ≈ q_true (within angle wrap) - -enumerates_up_to_eight: - Assert: count ≤ 8; a generic reachable pose yields 8 - -elbow_up_and_down_present: - Assert: two solutions share θ1 but differ in θ3 sign - -unreachable_target_returns_zero: - Arrange: target far outside the workspace - Assert: count == 0 - -boundary_pose_is_single_elbow: - Arrange: target at max reach (arm straight) - Assert: elbow-up and elbow-down coincide (c3 = 1) - -wrist_singularity_flagged: - Arrange: pose with θ5 = 0 - Assert: θ4 / θ6 use the documented tie-break; their sum is correct - -no_iteration_side_effects: - Assert: Solve is pure / const; repeated calls identical -``` - -## Reference vectors - -- `target = fk.Forward(q_true)` ⇒ at least one branch matches `q_true`; all `count` branches satisfy FK. -- Fully stretched target ⇒ `c3 = 1`, single elbow solution. - -## Edge cases - -- `|c3| > 1` ⇒ branch pruned (unreachable). -- Wrist singularity `θ5 = 0` ⇒ `θ4 + θ6` fixed, split by convention. -- Shoulder singularity (`p_wc` on axis 1) ⇒ `θ1` free; documented handling. -- Angle wrap ⇒ compare solutions modulo `2π`. diff --git a/roadmap/kinematics/ContinuumKinematics/explanation.md b/roadmap/kinematics/ContinuumKinematics/explanation.md deleted file mode 100644 index d5f684e9..00000000 --- a/roadmap/kinematics/ContinuumKinematics/explanation.md +++ /dev/null @@ -1,36 +0,0 @@ -# Continuum Kinematics — Overview - -## What it is -Kinematics for arms that bend continuously instead of pivoting at discrete joints — tendon-driven, -pneumatic, or "soft" robots shaped like an elephant's trunk. The standard model treats each section as -a **circular arc of constant curvature**, described by three numbers: how sharply it bends, in which -plane, and over what length. - -## Why it matters (embedded) -Continuum robots reach into cluttered, delicate spaces — inside the body for surgery, around obstacles -for inspection — where a rigid arm cannot go. The constant-curvature model reduces an infinite-DOF -flexible body to a handful of arc parameters per section, small and cheap enough to run the forward -kinematics on the embedded controller driving the tendons or air chambers. - -## How it works (intuition) -Each section is assumed to bow into a perfect circular arc. Three parameters pin it down: the curvature -`κ` (how tight the arc is), the plane angle `φ` (which way it bends), and the arc length `s` (how far -along). From these, the section's tip pose is a closed-form point on that circle, with the frame -rotated by the total bend angle `θ = κ·s`. Chaining the section transforms — exactly like multiplying -joint transforms on a rigid arm — gives the whole robot's shape. The one delicate spot is a nearly -straight section: the formulas divide by `κ`, so as the arc flattens you must switch to a straight-line -limit to avoid dividing by zero. This "robot-independent" arc mapping is kept separate from the -"robot-specific" step that converts actual tendon pulls or chamber pressures into `(κ, φ, s)`. - -## Key parameters -- **curvature `κ`** — inverse bend radius of the section. -- **bending-plane angle `φ`** — the direction the section curves (undefined when straight). -- **arc length `s`** — how long the section is. - -## Reference -R. J. Webster III, B. A. Jones, "Design and Kinematic Modeling of Constant Curvature Continuum Robots: -A Review," *Int. J. Robotics Research*, 29(13), 2010. - -## See also -`SE3Transform` (M6, per-section transform), `Quaternion` (item 18, orientation blending), -`PoseInverseKinematics` (M13, the iterative multi-section inverse). diff --git a/roadmap/kinematics/ContinuumKinematics/implementation.md b/roadmap/kinematics/ContinuumKinematics/implementation.md deleted file mode 100644 index c1d2cbf1..00000000 --- a/roadmap/kinematics/ContinuumKinematics/implementation.md +++ /dev/null @@ -1,84 +0,0 @@ -# Continuum Kinematics (Constant Curvature) — Implementation Pseudocode - -> Roadmap ref: #M26 (Tier 4) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct ArcParameters: # configuration space of one section - T kappa # curvature (1 / radius) - T phi # bending-plane angle - T length # arc length s - -template -class ContinuumKinematics: - std::array, NumSections> sections -``` - -## Interface - -``` -ContinuumKinematics(std::array, NumSections> sections) -SE3 SectionTransform(ArcParameters arc) # one arc; hot path -SE3 Forward() # tip pose; hot path -ArcParameters InverseSection(SE3 sectionPose) # single-section geometric IK -``` - -## Algorithm (pseudocode) - -``` -function SectionTransform(arc): # OPTIMIZE_FOR_SPEED (robot-independent map) - θ = arc.kappa * arc.length # total bend angle - if arc.kappa ≈ 0: # straight section — series fallback - return SE3(I, (0, 0, arc.length)) - # arc lies in a plane rotated by φ about z; circle of radius 1/κ - p = (1/κ) * ( (1 - cosθ)·(cosφ, sinφ, 0) + sinθ·ẑ ) - R = Rotz(φ) * Roty(θ) * Rotz(-φ) # frame swept along the arc - return SE3(R, p) - -function Forward(): # OPTIMIZE_FOR_SPEED - T = Identity - for i in 0..NumSections-1: - T = T * SectionTransform(sections[i]) # reuse SE(3) compose (M6) - return T - -function InverseSection(pose): # single section, closed form - # recover (κ, φ, s) from one constant-curvature arc's tip - φ = atan2(pose.p.y, pose.p.x) - θ = 2 * atan2( ‖(pose.p.x, pose.p.y)‖ , pose.p.z ) # from arc geometry - κ = θ / arcLengthFrom(pose.p, θ) - s = θ / κ - return { κ, φ, s } -``` - -## Complexity & memory - -- `SectionTransform`: `O(1)` — a few trig calls and one `SE(3)` build. -- `Forward`: `O(NumSections)` `SE(3)` products; `InverseSection`: `O(1)` closed form. -- Memory: the section array plus one accumulator; no heap. - -## Numerical / embedded notes - -- **Curvature `κ → 0` is the trap:** the pose uses `1/κ`, which blows up for a straight section — switch - to the `θ → 0` series (`p → (0,0,s)`) below a threshold, and note `φ` is undefined when straight. -- Keep the **robot-independent** map `(κ, φ, s) → SE(3)` separate from the **robot-specific** map - (tendon lengths / chamber pressures → `(κ, φ, s)`); only the latter changes between hardware. -- Constant-curvature is a **modeling assumption** (piecewise circular arcs); real gravity/load bending - deviates from it — document it as an approximation, not exact kinematics. -- Multi-section inverse kinematics generally needs iteration (reuse damped least squares, M13-style); - only the single-section case is closed-form here. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/ContinuumKinematics.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `SectionTransform`/`Forward`, and - `extern template class ContinuumKinematics;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/ContinuumKinematics.cpp` → `template class ContinuumKinematics;` -- Test: `numerical/kinematics/test/TestContinuumKinematics.cpp` -- Doc: `doc/kinematics/ContinuumKinematics.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestContinuumKinematics.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ContinuumKinematics/tests.md b/roadmap/kinematics/ContinuumKinematics/tests.md deleted file mode 100644 index c9467731..00000000 --- a/roadmap/kinematics/ContinuumKinematics/tests.md +++ /dev/null @@ -1,60 +0,0 @@ -# Continuum Kinematics — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestContinuum : public ::testing::Test: - # single unit-length section, straight by default - std::array, 1> sec = { { kappa=0, phi=0, length=1 } } - ContinuumKinematics ck{ sec } -# each case below is a TEST_F(TestContinuum, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -straight_section_is_pure_translation: - Act: T = ck.SectionTransform({0, 0, 1}) - Assert: T ≈ SE3(I, (0, 0, 1)) - -quarter_circle_bend: - Arrange: κ = π/2, length = 1 ⇒ θ = π/2, radius = 2/π - Assert: tip position ≈ (2/π)·(1, 0, 1) - -bending_plane_angle_rotates_tip: - Arrange: same κ, s; φ = π/2 - Assert: tip bends in the y-z plane instead of the x-z plane - -forward_composes_sections: - Arrange: two identical bent sections - Assert: Forward = SectionTransform ∘ SectionTransform - -inverse_recovers_arc_parameters: - Arrange: known (κ, φ, s); pose = SectionTransform - Act: arc = ck.InverseSection(pose) - Assert: arc ≈ (κ, φ, s) - -curvature_zero_series_no_blowup: - Arrange: κ = 1e-9 - Assert: SectionTransform finite ≈ straight (no 1/κ overflow) - -arc_length_preserved: - Assert: geodesic length of the section == s regardless of κ - -tip_orientation_matches_bend: - Assert: section R rotates the tangent by θ = κ·s -``` - -## Reference vectors - -- Straight unit section ⇒ tip `(0, 0, 1)`, `R = I`. -- `κ = π/2`, `s = 1`, `φ = 0` ⇒ `θ = π/2`, radius `2/π`, tip `= (2/π)·(1, 0, 1)`. - -## Edge cases - -- `κ → 0` ⇒ series fallback; `φ` undefined but the result is independent of `φ`. -- Full loop `θ = 2π` ⇒ tip returns near the base (closed circle). -- Negative `κ` ⇒ bends the opposite way; sign consistent with `φ`. -- Multi-section inverse ⇒ falls back to iteration (documented, not closed-form). diff --git a/roadmap/kinematics/DenavitHartenberg/explanation.md b/roadmap/kinematics/DenavitHartenberg/explanation.md deleted file mode 100644 index d7f18fb6..00000000 --- a/roadmap/kinematics/DenavitHartenberg/explanation.md +++ /dev/null @@ -1,31 +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 link's four parameters generate one 4×4 homogeneous transform, and -multiplying them down the chain gives the forward kinematics. - -## 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 a handful of trig calls — cheap enough for a servo loop on a microcontroller. - -## How it works (intuition) -The convention forces every joint axis onto its frame's `z`-axis and every common normal onto the -`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 that follows -(`a` slides along `x`, `α` twists about `x`). One of `θ`/`d` is the moving joint variable; the other -three are fixed geometry. Chaining the link transforms walks the frame from the base out to the tool. - -## Key parameters -- **a (link length), α (link twist)** — fixed geometry of the link. -- **d (link offset), θ (joint angle)** — one is the joint variable, chosen by the joint type. -- **convention** — standard (distal) vs modified (proximal) DH place the frame differently. -- **joint type** — revolute (θ varies) or prismatic (d varies). - -## Reference -J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 (the DH convention). - -## See also -`SE3Transform` (M6, the per-link transform), `SpatialJacobian` (M8, differentiates this chain), -`ProductOfExponentials` (M15, the screw-theory alternative that skips DH bookkeeping). diff --git a/roadmap/kinematics/DenavitHartenberg/implementation.md b/roadmap/kinematics/DenavitHartenberg/implementation.md deleted file mode 100644 index 26398bfb..00000000 --- a/roadmap/kinematics/DenavitHartenberg/implementation.md +++ /dev/null @@ -1,88 +0,0 @@ -# Denavit-Hartenberg Parameters — Implementation Pseudocode - -> Roadmap ref: #M7 (Tier 2) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -enum JointType: Revolute, Prismatic - -template # static_assert(std::is_floating_point_v); instantiated for float -struct DhLink: - T a # link length (along xᵢ) - T alpha # link twist (about xᵢ) - T d # link offset (along zᵢ₋₁) - T theta # joint angle (about zᵢ₋₁) - JointType type # which of {theta, d} is the variable - -template -class DenavitHartenberg: - std::array, NumLinks> links # constant frame description - bool modified = false # standard (distal) vs modified (proximal) DH -``` - -## Interface - -``` -DenavitHartenberg(std::array, NumLinks> links, bool modified = false) -SE3 LinkTransform(DhLink link, T q) # one Aᵢ; hot path -SE3 Forward(JointVector q) # ⁰Tₙ; hot path -std::array, N + 1> FrameChain(JointVector q) # every ⁰Tᵢ (for Jacobian) -``` - -## Algorithm (pseudocode) - -``` -function LinkTransform(link, q): # OPTIMIZE_FOR_SPEED - theta = link.type == Revolute ? q : link.theta - d = link.type == Prismatic ? q : link.d - (ct, st) = (cos theta, sin theta) - (ca, sa) = (cos link.alpha, sin link.alpha) # constant per link — precompute - # standard (distal) DH: Rotz(θ)·Transz(d)·Transx(a)·Rotx(α) - R = [[ ct, -st·ca, st·sa ], - [ st, ct·ca, -ct·sa ], - [ 0, sa, ca ]] - p = (link.a·ct, link.a·st, d) - return SE3(R, p) - -function Forward(q): # OPTIMIZE_FOR_SPEED - T = Identity - for i in 0..N-1: - T = T * LinkTransform(links[i], q[i]) # reuse SE3 compose (M6) - return T - -function FrameChain(q): - frames[0] = Identity - for i in 0..N-1: - frames[i+1] = frames[i] * LinkTransform(links[i], q[i]) - return frames -``` - -## Complexity & memory - -- `LinkTransform`: `O(1)` — four trig calls and a fixed 3×3 fill, no loop. -- `Forward` / `FrameChain`: `O(N)` SE(3) products; `FrameChain` stores `N+1` transforms. -- Memory: the constant `NumLinks` link table plus one `SE(3)` accumulator; all on the stack. - -## Numerical / embedded notes - -- Precompute `sin/cos` of each `alpha` once (constant per link) — only `theta`/`d` vary at runtime. -- Support both **standard** and **modified** DH: the two conventions place the frame differently, so - the factor order changes; expose the flag rather than hard-coding one. -- `JointType` selects whether `theta` or `d` is the variable — one struct serves revolute and prismatic - chains (unblocks mixed SCARA/gantry arms), unlike the revolute-only `RevoluteJointLink`. -- Keep each `R` on `SO(3)`: the factored form is exactly orthonormal, so no reorthonormalization needed. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/DenavitHartenberg.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `LinkTransform`/`Forward`, and - `extern template class DenavitHartenberg;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/DenavitHartenberg.cpp` → `template class DenavitHartenberg;` -- Test: `numerical/kinematics/test/TestDenavitHartenberg.cpp` -- Doc: `doc/kinematics/DenavitHartenberg.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestDenavitHartenberg.cpp` → the `_test` target. -- 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 e2e00f10..00000000 --- a/roadmap/kinematics/DenavitHartenberg/tests.md +++ /dev/null @@ -1,61 +0,0 @@ -# Denavit-Hartenberg Parameters — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestDenavitHartenberg : public ::testing::Test: - # 2-link planar arm: a₁ = a₂ = 1, α = d = 0, both revolute - std::array, 2> planar = { {1,0,0,0,Revolute}, {1,0,0,0,Revolute} } - DenavitHartenberg dh{ planar } -# each case below is a TEST_F(TestDenavitHartenberg, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -zero_config_is_stretched_along_x: - Act: T = dh.Forward({0, 0}) - Assert: T.p ≈ (2, 0, 0), T.R ≈ Identity - -single_link_revolute_rotates: - Arrange: 1-link arm a = 1 - Act: T = dh.Forward({π/2}) - Assert: T.p ≈ (0, 1, 0) - -planar_elbow_ninety_deg: - Act: T = dh.Forward({0, π/2}) - Assert: T.p ≈ (1, 1, 0) - -prismatic_link_translates_along_z: - Arrange: single Prismatic link, a = α = θ = 0 - Act: T = dh.Forward({0.5}) - Assert: T.p ≈ (0, 0, 0.5) - -link_twist_alpha_reorients_frame: - Arrange: link a = 0, α = π/2 - Assert: LinkTransform.R maps ẑ → -ŷ (axis reorientation) - -frame_chain_last_matches_forward: - Assert: FrameChain(q).back() ≈ Forward(q) - -link_transform_is_orthonormal: - Assert: RᵀR ≈ Identity for arbitrary q - -modified_vs_standard_differ: - Assert: modified-DH Forward ≠ standard Forward for the same table (documented) -``` - -## Reference vectors - -- 2-link unit planar arm, `q = (0, π/2)` ⇒ tip `(1, 1, 0)`. -- Single revolute `a = 1`, `q = π/2` ⇒ tip `(0, 1, 0)`. -- Prismatic `d = 0.5` ⇒ tip `(0, 0, 0.5)`. - -## Edge cases - -- `alpha = ±π/2` ⇒ frame axis swaps; verify no sign error in `sa`. -- Zero-length links (`a = d = 0`) ⇒ pure rotation joints. -- Mixed revolute/prismatic chain ⇒ correct variable substitution per link. -- Full-turn `theta = 2π` ⇒ same pose as `theta = 0` (wrap-agnostic). diff --git a/roadmap/kinematics/ManipulabilityIndex/explanation.md b/roadmap/kinematics/ManipulabilityIndex/explanation.md deleted file mode 100644 index 60b051b7..00000000 --- a/roadmap/kinematics/ManipulabilityIndex/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Manipulability Index — Overview - -## What it is -A single number that scores how well an arm can move in its current posture. Yoshikawa's measure, -`w = √det(J Jᵀ)`, is the volume of the "velocity ellipsoid" — the set of tool velocities reachable by -unit-norm joint rates. Big `w` means dexterous; `w = 0` means the arm is at a singularity. - -## Why it matters (embedded) -Singularities are where Jacobian-based controllers blow up: joint rates rocket toward infinity for a -finite tool motion. A cheap scalar that flags "you are getting close" lets a real-time controller slow -down, switch strategies, or damp the inverse *before* the actuators saturate. It is also the objective -a redundant arm optimizes in its null space to stay out of trouble. - -## How it works (intuition) -Map the unit sphere of joint velocities through `J` and it becomes an ellipsoid in task space. The -ellipsoid's principal axes are the singular values of `J`; its volume is their product, which equals -`√det(J Jᵀ)`. When the arm nears a singularity one axis collapses — one singular value goes to zero — -so the volume, and hence `w`, drops to zero. The ratio of the longest to shortest axis (the condition -number) tells you how *lopsided* the reachable set is, a finer warning than volume alone. - -## Key parameters -- **the Jacobian `J`** — supplied by `SpatialJacobian` at the current joint configuration. -- **singularity threshold `eps`** — below which `NearSingular` trips. -- **square vs redundant** — picks `|det J|` versus the Gram-determinant form. - -## Reference -T. Yoshikawa, "Manipulability of Robotic Mechanisms," *Int. J. Robotics Research*, 4(2), 1985. - -## See also -`SpatialJacobian` (M8, the input), `SingularValueDecomposition` (item 43, robust axes + conditioning), -`RedundancyResolution` (M14, which maximizes this in the null space). diff --git a/roadmap/kinematics/ManipulabilityIndex/implementation.md b/roadmap/kinematics/ManipulabilityIndex/implementation.md deleted file mode 100644 index c3aad13e..00000000 --- a/roadmap/kinematics/ManipulabilityIndex/implementation.md +++ /dev/null @@ -1,73 +0,0 @@ -# Manipulability Index (Yoshikawa) — Implementation Pseudocode - -> Roadmap ref: #M11 (Tier 2) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class ManipulabilityIndex: - SpatialJacobian jac - # scratch: SquareMatrix JJt (task-space Gram, m = 6) -``` - -## Interface - -``` -ManipulabilityIndex(SpatialJacobian jac) -T Compute(JointVector q) # w = √det(J Jᵀ); hot path -T ConditionNumber(JointVector q) # σ_max / σ_min -std::array EllipsoidAxes(JointVector q) # singular values (semi-axis lengths) -bool NearSingular(JointVector q, T eps) # w < eps -``` - -## Algorithm (pseudocode) - -``` -function Compute(q): # OPTIMIZE_FOR_SPEED - J = jac.Compute(q) # 6×N - JJt = J * Transpose(J) # 6×6, symmetric PSD - return sqrt( determinant(JJt) ) # Yoshikawa measure w - # square non-redundant arm (N = 6): w = |det J| directly — skip the product - -function EllipsoidAxes(q): - J = jac.Compute(q) - σ = SingularValues(J) # reuse SVD (item 43) - return σ # w = ∏ σᵢ ; axes of the velocity ellipsoid - -function ConditionNumber(q): - σ = SingularValues(jac.Compute(q)) - return σ_max / σ_min # → ∞ at a singularity - -function NearSingular(q, eps): - return Compute(q) < eps -``` - -## Complexity & memory - -- `Compute` via `J Jᵀ` + determinant: `O(N·36 + 6³)` — cheap for small `N`. -- `EllipsoidAxes` / `ConditionNumber` via SVD: `O(6²·N)` but more numerically robust. -- Memory: one `6×6` scratch matrix (or the SVD factors); no heap. - -## Numerical / embedded notes - -- `w = √det(J Jᵀ)` is the **volume** of the velocity ellipsoid: large = dexterous, `w → 0` = singular. -- Prefer **SVD** (item 43) over an explicit determinant when you also need conditioning or the - ellipsoid axes — `det(J Jᵀ) = (∏ σᵢ)²`, but the product of singular values avoids cancellation. -- For a **redundant** arm (`N > 6`) the Gram-determinant root is the only valid form — `det J` does - not exist for a non-square `J`. -- Use `NearSingular` as a guard *before* any Jacobian inverse; do not wait for the solve to blow up. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/ManipulabilityIndex.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class ManipulabilityIndex;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/ManipulabilityIndex.cpp` → `template class ManipulabilityIndex;` -- Test: `numerical/kinematics/test/TestManipulabilityIndex.cpp` -- Doc: `doc/kinematics/ManipulabilityIndex.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestManipulabilityIndex.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ManipulabilityIndex/tests.md b/roadmap/kinematics/ManipulabilityIndex/tests.md deleted file mode 100644 index 01fb5aa0..00000000 --- a/roadmap/kinematics/ManipulabilityIndex/tests.md +++ /dev/null @@ -1,56 +0,0 @@ -# Manipulability Index — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestManipulability : public ::testing::Test: - DenavitHartenberg arm{ ... } # 2-link unit planar arm - SpatialJacobian jac{ arm } - ManipulabilityIndex w{ jac } -# each case below is a TEST_F(TestManipulability, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -right_angle_elbow_is_most_dexterous: - Act: value = w.Compute({0, π/2}) - Assert: value is the local maximum over q₂ - -stretched_arm_is_singular: - Act: value = w.Compute({0, 0}) - Assert: value ≈ 0 (elbow straight ⇒ lost a DOF) - -folded_arm_is_singular: - Assert: w.Compute({0, π}) ≈ 0 - -index_is_nonnegative: - Assert: Compute(q) ≥ 0 across a sweep of q - -near_singular_flag_trips: - Arrange: q close to stretched - Assert: NearSingular(q, eps) == true - -condition_number_grows_near_singularity: - Assert: ConditionNumber → large as q → stretched - -ellipsoid_axes_match_singular_values: - Assert: EllipsoidAxes(q) are sorted σ; their product ≈ Compute(q) - -symmetry_about_elbow_sign: - Assert: Compute({0, +β}) ≈ Compute({0, -β}) -``` - -## Reference vectors - -- 2-link unit arm: `w(q₂) = |sin q₂|` (planar case) ⇒ max at `q₂ = π/2`, zero at `q₂ ∈ {0, π}`. -- Condition number `→ ∞` as `q₂ → 0`. - -## Edge cases - -- Square arm (`N = 6`) ⇒ `w = |det J|`, skip the Gram product. -- Redundant arm (`N > 6`) ⇒ `√det(J Jᵀ)` only; `det J` undefined. -- Exactly singular `J` ⇒ `ConditionNumber` division guarded (returns ∞ sentinel). -- Tiny `w` below `float` epsilon ⇒ `NearSingular` still deterministic. diff --git a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md b/roadmap/kinematics/MobileManipulatorKinematics/explanation.md deleted file mode 100644 index e2ccdbb8..00000000 --- a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md +++ /dev/null @@ -1,36 +0,0 @@ -# Mobile-Manipulator Kinematics — Overview - -## What it is -Kinematics for an arm mounted on a driving base — a robot that can both reach and roam. It combines the -arm's Jacobian with the base's motion into one map from "wheel commands plus joint rates" to -end-effector velocity, while respecting that a wheeled base cannot move sideways. - -## Why it matters (embedded) -Mobile manipulators — warehouse robots, service robots, rovers with arms — must coordinate base and arm -as a single system, not two. A unified Jacobian lets one controller command the whole platform, and the -extra freedom of "drive there *and* reach" is what gives these robots their large effective workspace. -Doing it on-board in real time needs a compact, constraint-aware model. - -## How it works (intuition) -The tool's velocity is the sum of two contributions: what the arm does on top of a stationary base, and -what the base does while carrying a frozen arm. The arm part is its ordinary Jacobian. The base part -depends on the lever arm from the base to the tool — driving forward translates the tool, spinning the -base swings it around, and the farther out the tool, the more a base rotation moves it. The catch is the -**nonholonomic** constraint: a differential-drive base has three planar coordinates but only two -controls (drive speed and turn rate), because it physically cannot slip sideways. A small matrix `S(φ)` -encodes that, mapping the two real controls into allowed base motion. Stacking the constrained base -block beside the arm Jacobian gives the combined map; because base-plus-arm usually over-provides DOF, -null-space resolution decides how much each should contribute. - -## Key parameters -- **base pose `(x, y, φ)`** and **drive controls `(v, ω)`** — the constrained base freedom. -- **arm configuration `q`** — the joint angles feeding the arm Jacobian. -- **base-to-arm mount** — the fixed transform placing the arm on the base. - -## Reference -Y. Yamamoto, X. Yun, "Coordinating Locomotion and Manipulation of a Mobile Manipulator," *IEEE Trans. -Automatic Control*, 39(6), 1994. - -## See also -`SpatialJacobian` (M8, the arm block), `RedundancyResolution` (M14, splitting base vs arm), -`SE3Transform` (M6, base pose composition). diff --git a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md b/roadmap/kinematics/MobileManipulatorKinematics/implementation.md deleted file mode 100644 index ad39b29d..00000000 --- a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md +++ /dev/null @@ -1,79 +0,0 @@ -# Mobile-Manipulator Kinematics (Nonholonomic Base) — Implementation Pseudocode - -> Roadmap ref: #M24 (Tier 4) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct BaseState: # differential-drive base pose - T x, y, phi # planar position + heading - -template -class MobileManipulatorKinematics: - SpatialJacobian armJac - SE3 baseToArm # arm mount on the base - T wheelBase # for the drive model -``` - -## Interface - -``` -MobileManipulatorKinematics(armJac, SE3 baseToArm, T wheelBase) -Matrix Compute(BaseState base, JointVector q) # combined J; hot path -SE3 EndEffectorPose(BaseState base, JointVector q) -Matrix BaseConstraint(BaseState base) # S(φ): (v,ω) → q̇_base -``` - -## Algorithm (pseudocode) - -``` -function BaseConstraint(base): # nonholonomic: no side-slip - # Pfaffian constraint [-sinφ, cosφ, 0]·q̇_base = 0 ⇒ parameterize by (v, ω) - return [[ cosφ, 0 ], - [ sinφ, 0 ], - [ 0, 1 ]] # q̇_base = S(φ)·(v, ω) - -function Compute(base, q): # OPTIMIZE_FOR_SPEED - # end-effector velocity = base contribution + arm contribution - J_arm = armJac.Compute(q) # 6×NumArm, in the base frame - r = EndEffectorPose(base, q).p - basePosition # lever arm base → tool - J_base = [[ I₃ , -SkewSymmetric(r) ], # planar base twist → tool twist - [ 0 , ẑ ]] # keep the (v, ω) columns only - J_base_reduced = J_base * lift(S(φ)) # apply nonholonomic S(φ) ⇒ 6×2 - return [ J_base_reduced | J_arm ] # 6 × (2 + NumArm) - -function EndEffectorPose(base, q): - T_base = SE3(Rotz(base.phi), (base.x, base.y, 0)) - return T_base * baseToArm * armForward(q) -``` - -## Complexity & memory - -- `Compute`: `O(NumArm)` — arm Jacobian plus a fixed base block and one `S(φ)` multiply. -- Memory: one `6 × (2 + NumArm)` combined Jacobian; bounded, stack-allocated. - -## Numerical / embedded notes - -- The **nonholonomic constraint** (a differential-drive base cannot slide sideways) drops the base from - 3 planar DOF to 2 controls `(v, ω)`; `S(φ)` enforces it — never command lateral base velocity directly. -- Base and arm together are **redundant** for a 6-DOF task — resolve the split with null-space - projection (M14), e.g. prefer arm motion for fine moves and base motion for gross reach. -- Compute the tool lever arm `r` in the **base frame** so the `[r]×` block and the arm Jacobian share - one convention; reuse `SkewSymmetric` (Geometry3D) and `SpatialJacobian` (M8). -- Watch coupling: base rotation `ω` moves a far-out tool a lot (`|r|` large) — scale the two blocks so - the solver does not over-use the base. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/MobileManipulatorKinematics.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class MobileManipulatorKinematics;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/MobileManipulatorKinematics.cpp` → `template class MobileManipulatorKinematics;` -- Test: `numerical/kinematics/test/TestMobileManipulatorKinematics.cpp` -- Doc: `doc/kinematics/MobileManipulatorKinematics.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestMobileManipulatorKinematics.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/MobileManipulatorKinematics/tests.md b/roadmap/kinematics/MobileManipulatorKinematics/tests.md deleted file mode 100644 index 13743dd1..00000000 --- a/roadmap/kinematics/MobileManipulatorKinematics/tests.md +++ /dev/null @@ -1,58 +0,0 @@ -# Mobile-Manipulator Kinematics — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestMobileManipulator : public ::testing::Test: - DenavitHartenberg arm{ ... } - SpatialJacobian armJac{ arm } - MobileManipulatorKinematics mm{ armJac, mount, wheelBase=0.3f } -# each case below is a TEST_F(TestMobileManipulator, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -combined_jacobian_dimensions: - Act: J = mm.Compute(base0, q0) - Assert: J is 6 × (2 + 3) - -base_pose_composes_with_arm: - Assert: EndEffectorPose = T_base · mount · FK_arm(q) - -nonholonomic_constraint_blocks_sideslip: - Assert: BaseConstraint(base)·(v,ω) has zero lateral component in the world frame - -pure_forward_drive_moves_tool: - Arrange: (v=1, ω=0), q̇ = 0 - Assert: tool linear velocity ≈ the forward heading direction - -base_yaw_moves_far_tool_more: - Arrange: ω = 1, large lever arm r - Assert: tool linear-velocity magnitude scales with |r| - -arm_only_motion_when_base_still: - Arrange: (v, ω) = 0 - Assert: tool velocity = armJac·q̇ - -redundancy_split_base_and_arm: - Arrange: task rate reachable by base or arm - Assert: null-space resolution distributes the motion (M14) - -twist_matches_finite_difference: - Assert: J·[v, ω, q̇] ≈ (pose(t + ε) ⊖ pose(t)) / ε -``` - -## Reference vectors - -- Base heading `φ = 0`, `(v=1, ω=0)` ⇒ tool linear velocity `(1, 0, 0)` plus any arm term. -- Nonholonomic `S(φ)` ⇒ `[-sinφ, cosφ, 0]·q̇_base = 0` for all `(v, ω)`. - -## Edge cases - -- Zero lever arm (tool over the base axis) ⇒ base yaw adds no tool translation. -- Pure spin (`v=0, ω≠0`) ⇒ tool moves only if `r ≠ 0`. -- Heading wrap (`φ = ±π`) ⇒ `S(φ)` still consistent. -- Non-redundant case (task = base + arm DOF) ⇒ unique split, empty null space. diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md b/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md deleted file mode 100644 index d8e0a35a..00000000 --- a/roadmap/kinematics/ParallelManipulatorKinematics/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# Parallel Manipulator Kinematics — Overview - -## What it is -Kinematics for robots whose moving platform is held by several legs in parallel — the pick-and-place -**Delta** (three arms) and the six-legged **Stewart-Gough** hexapod. Given a platform pose it finds the -leg lengths (inverse), and given the leg lengths it finds the platform pose (forward). - -## Why it matters (embedded) -Parallel robots are stiff, fast, and accurate — the workhorses of high-speed packaging and motion -platforms. Their control flips the usual serial-arm effort: computing the leg commands from a desired -pose is a trivial, closed-form, per-leg calculation that fits any real-time loop, exactly what the -servo layer needs on every tick. - -## How it works (intuition) -For the **inverse** problem each leg is independent: transform its platform anchor into the base frame, -subtract its base anchor, and the length is just the distance between those two points — no coupling, no -iteration. The **forward** problem is the hard one, because moving one leg moves the whole platform and -thus every other leg. It has no general closed form, so you solve it with Newton's method: guess a -pose, compute what leg lengths it *would* produce, and correct the pose until those match the measured -lengths. Several platform poses can produce the same leg lengths (assembly modes), so the initial guess -picks which one you land on. The three-legged Delta is special enough to admit a closed-form forward -solve by intersecting spheres. - -## Key parameters -- **base and platform anchor points** — the fixed geometry of the mechanism. -- **leg lengths** — the actuated variables (prismatic legs). -- **forward-solve guess** — selects the assembly mode and warm-starts Newton. - -## Reference -J.-P. Merlet, *Parallel Robots*, 2nd ed. (2006); R. Clavel, "Delta, a Fast Robot with Parallel -Geometry," *Int. Symp. on Industrial Robots*, 1990. - -## See also -`SE3Transform` (M6, platform pose), `LuDecomposition` / `GaussianElimination` (item 28, the Newton -step), `ManipulabilityIndex` (M11, singularity monitoring). diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md b/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md deleted file mode 100644 index 3ef8df42..00000000 --- a/roadmap/kinematics/ParallelManipulatorKinematics/implementation.md +++ /dev/null @@ -1,82 +0,0 @@ -# Parallel Manipulator Kinematics (Delta / Stewart-Gough) — Implementation Pseudocode - -> Roadmap ref: #M23 (Tier 4) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct PlatformGeometry: - std::array, NumLegs> baseAnchors # bᵢ in the fixed frame - std::array, NumLegs> platformAnchors # pᵢ in the moving frame - -template -struct ForwardConfig: - T tolerance - std::size_t maxIterations - -template -class ParallelManipulatorKinematics: - PlatformGeometry geom - ForwardConfig config -``` - -## Interface - -``` -ParallelManipulatorKinematics(geom, ForwardConfig cfg = {}) -std::array Inverse(SE3 platformPose) # leg lengths; closed form, hot path -SE3 Forward(std::array legLengths, - SE3 guess) # Newton iteration -``` - -## Algorithm (pseudocode) - -``` -function Inverse(pose): # OPTIMIZE_FOR_SPEED (closed form) - for i in 0..NumLegs-1: - legVec = pose.R * platformAnchors[i] + pose.p - baseAnchors[i] - length[i] = VectorNorm(legVec) # each leg independent - return length - -function Forward(measured, guess): # iterative — multiple assembly modes - x = guess # pose as (p, small-rotation) - for iter in 0..maxIterations-1: - f = Inverse(x) - measured # residual per leg - if norm(f) < tolerance: return x - J = ∂(legLengths)/∂x # NumLegs × 6 (Stewart), analytic or numeric - Δx = solve(J, -f) # LU (item 28); least squares if NumLegs ≠ 6 - x = x ⊕ Δx # retract onto SE(3) - return x # best effort (may not converge) -``` - -## Complexity & memory - -- `Inverse`: `O(NumLegs)` — one transform + norm per leg, fully parallel and branch-free. -- `Forward`: `O(iters · 6³)` — a `6×6` (or least-squares) solve per Newton step. -- Memory: geometry arrays plus one `NumLegs×6` Jacobian; bounded, stack-allocated. - -## Numerical / embedded notes - -- Parallel robots invert the usual difficulty: **inverse** kinematics is trivial and closed-form, while - **forward** kinematics needs Newton iteration (the opposite of a serial arm). -- Forward FK has **multiple assembly modes** (the platform can sit in several poses for the same leg - lengths) — the `guess` selects the branch; seed it from the previous cycle. -- The **Delta** (3 legs, parallelogram arms) admits a closed-form forward solve via three-sphere - intersection — specialize it rather than iterating. -- Near a platform singularity the leg Jacobian is ill-conditioned — monitor it (M11 idea) and fall back - to a damped step; reuse `GaussianElimination` / LU (item 28) for the linear solve. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/ParallelManipulatorKinematics.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Inverse`, and - `extern template class ParallelManipulatorKinematics;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/ParallelManipulatorKinematics.cpp` → `template class ParallelManipulatorKinematics;` -- Test: `numerical/kinematics/test/TestParallelManipulatorKinematics.cpp` -- Doc: `doc/kinematics/ParallelManipulatorKinematics.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestParallelManipulatorKinematics.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ParallelManipulatorKinematics/tests.md b/roadmap/kinematics/ParallelManipulatorKinematics/tests.md deleted file mode 100644 index 31f617ce..00000000 --- a/roadmap/kinematics/ParallelManipulatorKinematics/tests.md +++ /dev/null @@ -1,59 +0,0 @@ -# Parallel Manipulator Kinematics — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestStewartGough : public ::testing::Test: - PlatformGeometry hexapod{ ... } # symmetric 6-6 platform - ParallelManipulatorKinematics kin{ hexapod, { 1e-6, 50 } } -# each case below is a TEST_F(TestStewartGough, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -inverse_of_home_pose: - Act: L = kin.Inverse(homePose) - Assert: all legs equal the nominal home length - -inverse_is_closed_form_deterministic: - Assert: Inverse(pose) is repeatable, no iteration state - -forward_inverse_round_trip: - Arrange: pose_true; L = kin.Inverse(pose_true) - Act: pose = kin.Forward(L, pose_true + small_perturbation) - Assert: pose ≈ pose_true - -forward_converges_from_nearby_guess: - Assert: Newton reaches tolerance in few iterations (warm start) - -leg_length_monotone_with_height: - Arrange: raise the platform along +z - Assert: each leg length increases - -assembly_mode_selected_by_guess: - Arrange: two guesses bracketing different modes - Assert: Forward returns the pose near each guess - -singular_pose_illconditioned: - Arrange: geometry at a platform singularity - Assert: Forward step damped; no NaN - -delta_closed_form_forward: - Arrange: 3-leg Delta geometry - Assert: closed-form Forward matches the Inverse round-trip -``` - -## Reference vectors - -- Home pose ⇒ all six legs at nominal length `L₀`. -- Pure `+z` translation `Δh` ⇒ symmetric increase in every leg length. - -## Edge cases - -- Platform singularity ⇒ leg Jacobian rank-deficient; damped fallback. -- Wrong assembly-mode guess ⇒ converges to a different valid pose (documented). -- Unreachable leg lengths ⇒ Forward fails gracefully, returns best effort. -- Delta (3 legs) vs Stewart (6 legs) ⇒ different `NumLegs` template instantiations. diff --git a/roadmap/kinematics/PoseInverseKinematics/explanation.md b/roadmap/kinematics/PoseInverseKinematics/explanation.md deleted file mode 100644 index 579faf59..00000000 --- a/roadmap/kinematics/PoseInverseKinematics/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# Pose Inverse Kinematics — Overview - -## What it is -Given a desired **pose** — position *and* orientation — of the tool, find joint angles that reach it. -This extends position-only IK to all six task dimensions by driving a 6-vector error (three for -translation, three for rotation) to zero with a damped-least-squares Newton step. - -## Why it matters (embedded) -Real tasks are pose tasks: point a welding torch, align a gripper, hold a camera level. A numerically -robust solver that survives singularities without commanding infinite joint speeds is what makes an -arm safe to run in a tight real-time loop, warm-started from the previous cycle for a handful of -iterations. - -## How it works (intuition) -Each step linearizes the arm about the current joints with the Jacobian, then asks "what joint change -best cancels the current pose error?" The plain least-squares answer, `J⁺e`, explodes near -singularities; adding a damping term `λ²I` inside the inverse tames it — the arm gives up a little -tracking accuracy in exchange for never blowing up. The subtle part is the **orientation** error: -subtracting Euler angles wraps and stalls, so the error is computed as the *rotation vector* (the log -of the relative rotation, or twice the vector part of the error quaternion), which always points the -short way around. - -## Key parameters -- **damping λ** — larger = more stable near singularities, less accurate away from them. -- **tolerance** — the 6-vector error norm at which the solve stops. -- **max iterations** and **seed `q0`** — warm-starting from the last cycle keeps counts low. - -## Reference -S. R. Buss, "Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped -Least Squares methods," 2004; Y. Nakamura, H. Hanafusa, "Inverse Kinematic Solutions with Singularity -Robustness," *ASME J. Dyn. Sys. Meas. Control*, 1986. - -## See also -`SpatialJacobian` (M8, the linearization), `Quaternion` (item 18, the orientation error), -`GaussianElimination` (the 6×6 solve), `AnalyticalIkPieper` (M21, the closed-form alternative). diff --git a/roadmap/kinematics/PoseInverseKinematics/implementation.md b/roadmap/kinematics/PoseInverseKinematics/implementation.md deleted file mode 100644 index 8afd4e6d..00000000 --- a/roadmap/kinematics/PoseInverseKinematics/implementation.md +++ /dev/null @@ -1,87 +0,0 @@ -# Pose Inverse Kinematics (Damped Least Squares) — Implementation Pseudocode - -> Roadmap ref: #M13 (Tier 3) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct PoseIkConfig: - T damping # λ (Levenberg-Marquardt) - T tolerance # 6-vector error norm to declare success - std::size_t maxIterations - -template -struct PoseIkResult: - Vector q - T finalError - std::size_t iterations - bool converged - -template -class PoseInverseKinematics: - DenavitHartenberg fk - SpatialJacobian jac - PoseIkConfig config -``` - -## Interface - -``` -PoseInverseKinematics(model, PoseIkConfig cfg = {}) -PoseIkResult Solve(SE3 target, JointVector q0) # hot path -Vector PoseError(SE3 target, SE3 current) # [Δp; Δω] -``` - -## Algorithm (pseudocode) - -``` -function PoseError(target, current): - e_p = target.p - current.p # position error - R_err = target.R * Transpose(current.R) # relative rotation - e_ω = AxisAngle(R_err) # log map → rotation vector - # equivalently 2·(vector part of quaternion(R_err)); reuse item 18 - return concat(e_p, e_ω) # 6-vector - -function Solve(target, q0): # OPTIMIZE_FOR_SPEED - q = q0 - for iter in 0..maxIterations-1: - T = fk.Forward(q) - e = PoseError(target, T) # 6-vector - if norm(e) < tolerance: return { q, norm(e), iter, true } - J = jac.Compute(q) # 6×N - # damped least squares: Δq = Jᵀ (J Jᵀ + λ²I)⁻¹ e - A = J * Transpose(J) + λ² * I₆ # 6×6 SPD - y = GaussianElimination(A, e) # solve A y = e (reuse solver) - q = q + Transpose(J) * y - return { q, norm(e), maxIterations, false } -``` - -## Complexity & memory - -- Per iteration: `O(6²N)` to form `J Jᵀ` plus `O(6³)` for the 6×6 solve. -- Total: `O(iters · (6²N + 6³))`; iteration count depends on `λ` and start pose. -- Memory: one `6×N` Jacobian and a `6×6` system; no heap, no dynamic sizing. - -## Numerical / embedded notes - -- **Damping `λ`** trades accuracy for stability: it keeps `(J Jᵀ + λ²I)` invertible *through* - singularities (Nakamura's singularity-robust inverse) at the cost of a small steady-state error. -- Orientation error **must** use the log/quaternion form — naive Euler-angle subtraction wraps and - stalls; reuse `Quaternion` (item 18) and fix the double-cover sign so the arm takes the short way. -- Solve `A y = e` with `GaussianElimination`; never form `(J Jᵀ)⁻¹` explicitly. -- Seed `q0` from the previous control cycle for warm-started, few-iteration convergence. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/PoseInverseKinematics.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Solve`, and - `extern template class PoseInverseKinematics;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/PoseInverseKinematics.cpp` → `template class PoseInverseKinematics;` -- Test: `numerical/kinematics/test/TestPoseInverseKinematics.cpp` -- Doc: `doc/kinematics/PoseInverseKinematics.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestPoseInverseKinematics.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/PoseInverseKinematics/tests.md b/roadmap/kinematics/PoseInverseKinematics/tests.md deleted file mode 100644 index 23a79b10..00000000 --- a/roadmap/kinematics/PoseInverseKinematics/tests.md +++ /dev/null @@ -1,60 +0,0 @@ -# Pose Inverse Kinematics — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestPoseIk : public ::testing::Test: - DenavitHartenberg arm{ ... } # 6R with spherical wrist - SpatialJacobian jac{ arm } - PoseInverseKinematics ik{ arm, { λ=0.05, tol=1e-4, 100 } } -# each case below is a TEST_F(TestPoseIk, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -round_trip_reachable_pose: - Arrange: q_true random; target = FK(q_true) - Act: r = ik.Solve(target, q_true + small_perturbation) - Assert: r.converged; FK(r.q) ≈ target (position + orientation) - -position_and_orientation_both_met: - Assert: PoseError(target, FK(r.q)) < tol in all 6 components - -converges_from_nearby_seed: - Assert: iterations small when q0 near solution (warm start) - -pure_orientation_change: - Arrange: target = same position, rotated tool - Assert: solver reorients without large base-joint motion - -damping_survives_singularity: - Arrange: target forcing a wrist singularity - Assert: Δq stays bounded; no NaN / Inf - -unreachable_target_fails_gracefully: - Arrange: target beyond arm reach - Assert: r.converged == false, finalError > tol, q finite - -orientation_error_takes_short_path: - Arrange: target rotation of +190° about ẑ - Assert: ‖e_ω‖ ≈ 170° (short way), not 190° - -zero_error_is_immediate: - Arrange: target = FK(q0) - Assert: iterations == 0, converged -``` - -## Reference vectors - -- Reachable `target = FK(q_true)` ⇒ recovered `q` with `‖e‖ < 10⁻⁴`. -- Orientation error of `R` vs `R·Rot(ẑ, 190°)` ⇒ `‖e_ω‖ ≈ 170° = 2.967 rad ≤ π`. - -## Edge cases - -- Wrist singularity (aligned axes) ⇒ damping keeps the solve finite. -- Target exactly at reach boundary ⇒ slow but bounded convergence. -- Antipodal orientation (180°) ⇒ axis ambiguous; document the tie-break. -- Quaternion double cover ⇒ `q` and `−q` give the same error direction. diff --git a/roadmap/kinematics/ProductOfExponentials/explanation.md b/roadmap/kinematics/ProductOfExponentials/explanation.md deleted file mode 100644 index f48a673f..00000000 --- a/roadmap/kinematics/ProductOfExponentials/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# Product of Exponentials — Overview - -## What it is -A forward-kinematics formula built from screw theory: `T = e^{[S₁]θ₁}···e^{[Sₙ]θₙ}·M`. Each joint is a -**screw axis** `Sᵢ` fixed in the base frame; turning the joint by `θᵢ` applies the matrix exponential of -that screw, and `M` is the tool's pose when every joint is at zero. - -## Why it matters (embedded) -PoE describes an arm with a single home pose `M` and one screw axis per joint — all in the *base* -frame — instead of a chain of intermediate DH frames. There is nothing to line up between links, so it -is less error-prone to author, and the same screw axes give the Jacobian almost for free. For code that -already has an `SE(3)` type, it is the most direct FK you can write. - -## How it works (intuition) -A screw axis packages "rotate about this line while sliding along it" into one 6-vector. Its matrix -exponential is the finite rigid motion produced by riding that screw for a parameter `θ` — the -rigid-body version of Rodrigues' rotation formula. Forward kinematics is then just: start at the home -pose, and for each joint multiply in the motion its screw produces. Because every axis is written in -the fixed base frame, you never rebuild intermediate frames; you only compose `SE(3)` exponentials. - -## Key parameters -- **screw axes `Sᵢ = (ωᵢ; vᵢ)`** — one per joint, expressed in the base frame. -- **home configuration `M`** — the tool pose at `q = 0`. -- **space vs body form** — whether the screws are read in the fixed or the tool frame. - -## Reference -K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 4 (forward kinematics via the product of -exponentials). - -## See also -`SE3Transform` (M6, the `Exp` and adjoint), `MatrixExponential` (item 29, the `se(3)→SE(3)` map), -`DenavitHartenberg` (M7, the frame-based alternative), `SpatialJacobian` (M8). diff --git a/roadmap/kinematics/ProductOfExponentials/implementation.md b/roadmap/kinematics/ProductOfExponentials/implementation.md deleted file mode 100644 index c754497a..00000000 --- a/roadmap/kinematics/ProductOfExponentials/implementation.md +++ /dev/null @@ -1,69 +0,0 @@ -# Product of Exponentials (Screw-Theory FK) — Implementation Pseudocode - -> Roadmap ref: #M15 (Tier 3) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class ProductOfExponentials: - std::array, NumLinks> screws # space-frame screw axes Sᵢ = (ωᵢ; vᵢ) - SE3 home # M: tool pose at q = 0 - bool bodyForm = false # space vs body PoE -``` - -## Interface - -``` -ProductOfExponentials(std::array, N> screws, SE3 home) -SE3 Compute(JointVector q) # ⁰Tₙ; hot path -Matrix SpaceJacobian(JointVector q) # columns = Adⱼ·Sᵢ -``` - -## Algorithm (pseudocode) - -``` -function Compute(q): # OPTIMIZE_FOR_SPEED (space form) - T = Identity - for i in 0..N-1: - T = T * SE3::Exp(screws[i], q[i]) # e^{[Sᵢ]qᵢ} (reuse M6 / item 29) - return T * home # T = e^{[S₁]q₁}···e^{[Sₙ]qₙ}·M - -function SpaceJacobian(q): - Js[0] = screws[0] - T = Identity - for i in 1..N-1: - T = T * SE3::Exp(screws[i-1], q[i-1]) - Js[i] = Adjoint(T) * screws[i] # transform screw into the current frame - return Js -``` - -## Complexity & memory - -- `Compute`: `O(N)` — one `SE(3)` exponential and one compose per joint. -- `SpaceJacobian`: `O(N)` — one adjoint (6×6) per column, reusing the running product. -- Memory: the `N` screw axes plus `M` and one accumulator; `O(1)` working set, no heap. - -## Numerical / embedded notes - -- PoE needs **no per-link frames** — every screw axis is expressed once in the fixed base frame, so - there is no DH bookkeeping and no accumulated convention error (its main advantage over M7). -- Each `SE3::Exp` uses the closed-form `se(3)` exponential (Rodrigues + left Jacobian); guard the - `θ → 0` case with a series expansion to avoid `sinθ/θ` division (reuse item 29 / M6). -- **Space vs body** form differ only in factor order and where `M` sits — expose the flag; space - Jacobian columns are `Adⱼ·Sᵢ`, body Jacobian columns are `Adⱼ⁻¹·Bᵢ`. -- Normalize each `ωᵢ` to unit length (or zero for a prismatic screw) so `qᵢ` is a true angle/length. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/ProductOfExponentials.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class ProductOfExponentials;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/ProductOfExponentials.cpp` → `template class ProductOfExponentials;` -- Test: `numerical/kinematics/test/TestProductOfExponentials.cpp` -- Doc: `doc/kinematics/ProductOfExponentials.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestProductOfExponentials.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/ProductOfExponentials/tests.md b/roadmap/kinematics/ProductOfExponentials/tests.md deleted file mode 100644 index 4855d813..00000000 --- a/roadmap/kinematics/ProductOfExponentials/tests.md +++ /dev/null @@ -1,57 +0,0 @@ -# Product of Exponentials — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestPoE : public ::testing::Test: - # 2-link unit planar arm as screws about ẑ at x = 0 and x = 1 - std::array, 2> screws = { (ẑ; 0,0,0), (ẑ; 0,-1,0) } - SE3 home = SE3(I, (2,0,0)) - ProductOfExponentials poe{ screws, home } -# each case below is a TEST_F(TestPoE, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -zero_config_returns_home: - Assert: poe.Compute({0, 0}) ≈ home (tip at (2,0,0)) - -single_joint_rotation: - Act: T = poe.Compute({π/2, 0}) - Assert: T.p ≈ (0, 2, 0) - -elbow_rotation_matches_dh: - Act: T = poe.Compute({0, π/2}) - Assert: T.p ≈ (1, 1, 0) - -agrees_with_denavit_hartenberg: - Assert: poe.Compute(q) ≈ dh.Forward(q) over a sweep of q (same arm) - -space_jacobian_first_column_is_screw: - Assert: SpaceJacobian(q) column 0 == screws[0] - -jacobian_matches_finite_difference: - Assert: SpaceJacobian(q)·q̇ ≈ twist from (Compute(q + εq̇) ⊖ Compute(q)) / ε - -prismatic_screw_translates: - Arrange: screw with ω = 0, v = x̂ - Assert: Compute({s}) translates the tool by s·x̂ - -exp_zero_angle_is_identity: - Assert: SE3::Exp(S, 0) ≈ Identity for each screw -``` - -## Reference vectors - -- 2-link unit arm, `q = (0, π/2)` ⇒ tip `(1, 1, 0)` (identical to the DH result). -- `q = (π/2, 0)` ⇒ tip `(0, 2, 0)`. - -## Edge cases - -- `q → 0` per joint ⇒ series fallback in `Exp`, no `sinθ/θ` blow-up. -- Prismatic screw (`ω = 0`) ⇒ pure translation, `Exp` skips the rotation branch. -- Non-unit `ω` fed in ⇒ document the normalization expectation. -- Long chain ⇒ compose order fixed (space form left-to-right) to match `M`. diff --git a/roadmap/kinematics/RedundancyResolution/explanation.md b/roadmap/kinematics/RedundancyResolution/explanation.md deleted file mode 100644 index ffefef67..00000000 --- a/roadmap/kinematics/RedundancyResolution/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Redundancy Resolution — Overview - -## What it is -A way to use a robot that has *more* joints than the task needs. A 7-DOF arm reaching a 6-DOF pose has -one spare degree of freedom; redundancy resolution splits the joint motion into a **primary** part that -achieves the task and a **secondary** part, living in the Jacobian's null space, that pursues an extra -goal without disturbing the tool. - -## Why it matters (embedded) -Extra DOF are what let a robot dodge its own joint limits, avoid obstacles, or stay away from -singularities *while still doing the job*. The formula `q̇ = J⁺ẋ + (I − J⁺J)q̇₀` packages that into one -matrix expression cheap enough to run every control tick — the difference between an arm that jams at a -joint stop and one that gracefully reconfigures around it. - -## How it works (intuition) -The pseudo-inverse `J⁺` gives the smallest joint motion that produces the desired tool velocity — the -primary term. The projector `I − J⁺J` filters *any* vector down to the component the tool cannot -feel: push the joints with it and the end-effector holds still. So you pick a secondary joint velocity -`q̇₀` — typically the downhill direction of some cost like "distance from joint limits" or -"manipulability" — run it through the projector, and add it on. The task is untouched; the spare -freedom does useful work. - -## Key parameters -- **task rate `ẋ`** — the desired 6-vector end-effector velocity (primary objective). -- **secondary rate `q̇₀`** — gradient of the objective the null space should optimize. -- **damping λ** — singularity-robustness of the pseudo-inverse. - -## Reference -A. Liégeois, "Automatic Supervisory Control of the Configuration and Behavior of Multibody -Mechanisms," *IEEE Trans. Systems, Man, and Cybernetics*, 7(12), 1977. - -## See also -`SpatialJacobian` (M8, supplies `J`), `SingularValueDecomposition` / `QrDecomposition` (items 43/27, -robust `J⁺`), `ManipulabilityIndex` (M11, a common secondary objective). diff --git a/roadmap/kinematics/RedundancyResolution/implementation.md b/roadmap/kinematics/RedundancyResolution/implementation.md deleted file mode 100644 index 7abcce25..00000000 --- a/roadmap/kinematics/RedundancyResolution/implementation.md +++ /dev/null @@ -1,74 +0,0 @@ -# Redundancy Resolution (Null-Space Projection) — Implementation Pseudocode - -> Roadmap ref: #M14 (Tier 3) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class RedundancyResolution: # NumLinks > 6 (redundant) - SpatialJacobian jac - T damping # λ for the damped pseudo-inverse - # scratch: JJt (6×6), Jpinv (N×6), P (N×N) -``` - -## Interface - -``` -RedundancyResolution(SpatialJacobian jac, T damping = 0) -Matrix PseudoInverse(JointVector q) # J⁺ -Matrix NullSpaceProjector(JointVector q) # I − J⁺J -JointVector Resolve(JointVector q, Vector xdot, - JointVector qdot0) # hot path -``` - -## Algorithm (pseudocode) - -``` -function PseudoInverse(q): # right inverse, wide J - J = jac.Compute(q) # 6×N - JJt = J * Transpose(J) + λ² I₆ # damped ⇒ singularity-robust - # J⁺ = Jᵀ (JJt)⁻¹ via 6×6 solves (reuse GaussianElimination / QR item 27) - return Transpose(J) * Inverse(JJt) - -function NullSpaceProjector(q): - J = jac.Compute(q) - Jp = PseudoInverse(q) - return I_N - Jp * J # N×N, idempotent - -function Resolve(q, xdot, qdot0): # OPTIMIZE_FOR_SPEED - Jp = PseudoInverse(q) - qdot_task = Jp * xdot # minimum-norm primary solution - P = I_N - Jp * jac.Compute(q) # null-space projector - return qdot_task + P * qdot0 # secondary objective in the null space -``` - -## Complexity & memory - -- `PseudoInverse`: `O(6²N + 6³)` — form `J Jᵀ`, one 6×6 factorization, back-substitute. -- `Resolve`: adds an `O(N²)` projector multiply for the secondary term. -- Memory: `N×6`, `N×N`, and `6×6` scratch; all bounded, stack-allocated. - -## Numerical / embedded notes - -- The **right** pseudo-inverse `Jᵀ(J Jᵀ)⁻¹` is correct for a *wide* (redundant) `J`; the left form - applies to tall `J`. Damping `λ²` keeps `J Jᵀ` invertible near singularities. -- `P = I − J⁺J` is a **projector**: `P² = P`. Anything it multiplies moves the joints *without* - disturbing the tool — so the secondary objective is exactly task-consistent. -- Common secondary rates `q̇₀`: gradient of manipulability (M11), distance-to-joint-limits, or an - obstacle-avoidance potential. Scale `q̇₀` so it never dominates the primary task. -- For best conditioning build `J⁺` from a **QR/SVD** (items 27/43) rather than an explicit inverse. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/RedundancyResolution.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Resolve`, and - `extern template class RedundancyResolution;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/RedundancyResolution.cpp` → `template class RedundancyResolution;` -- Test: `numerical/kinematics/test/TestRedundancyResolution.cpp` -- Doc: `doc/kinematics/RedundancyResolution.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestRedundancyResolution.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/RedundancyResolution/tests.md b/roadmap/kinematics/RedundancyResolution/tests.md deleted file mode 100644 index 26993183..00000000 --- a/roadmap/kinematics/RedundancyResolution/tests.md +++ /dev/null @@ -1,59 +0,0 @@ -# Redundancy Resolution — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestRedundancy : public ::testing::Test: - DenavitHartenberg arm{ ... } # 7-DOF redundant arm - SpatialJacobian jac{ arm } - RedundancyResolution rr{ jac, 0.01f } -# each case below is a TEST_F(TestRedundancy, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -primary_task_is_satisfied: - Arrange: desired ẋ; qdot0 = 0 - Act: q̇ = rr.Resolve(q, ẋ, 0) - Assert: jac.Compute(q) * q̇ ≈ ẋ - -null_space_motion_is_task_invisible: - Arrange: arbitrary qdot0 - Act: q̇ = P * qdot0 (task rate zero) - Assert: jac.Compute(q) * q̇ ≈ 0 - -projector_is_idempotent: - Assert: P * P ≈ P - -minimum_norm_primary_solution: - Assert: ‖J⁺ẋ‖ ≤ ‖any other q̇ solving Jq̇ = ẋ‖ - -secondary_objective_reduces_cost: - Arrange: qdot0 = -∇(joint-limit cost) - Assert: cost decreases along q̇ while task still met - -pseudo_inverse_right_identity: - Assert: J * J⁺ ≈ I₆ (right inverse for wide J) - -damping_bounds_near_singularity: - Arrange: near-singular q - Assert: ‖J⁺‖ stays finite - -zero_inputs_give_zero_motion: - Assert: Resolve(q, 0, 0) ≈ 0 -``` - -## Reference vectors - -- Wide `J` (`6×7`): `J·J⁺ ≈ I₆`, but `J⁺·J ≠ I₇` (rank 6). -- Null-space rate: `J·(P q̇₀) = 0` for any `q̇₀`. - -## Edge cases - -- Non-redundant arm (`N = 6`) ⇒ `P ≈ 0`, no null space. -- Fully singular `J` ⇒ damping prevents blow-up, task partially met. -- Conflicting secondary objective ⇒ primary task still exact, secondary compromised. -- Over-scaled `q̇₀` ⇒ document that the projector still protects the task. diff --git a/roadmap/kinematics/SpatialJacobian/explanation.md b/roadmap/kinematics/SpatialJacobian/explanation.md deleted file mode 100644 index b5bd56af..00000000 --- a/roadmap/kinematics/SpatialJacobian/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Spatial Jacobian — Overview - -## What it is -The Jacobian is the matrix that links how fast the joints move to how fast the end-effector moves. -For an `N`-joint arm it is `6×N`: the top three rows give the tool's linear velocity, the bottom three -its angular velocity, as a linear function of the joint-rate vector `q̇`. - -## Why it matters (embedded) -It is the workhorse of manipulator control. Velocity control, force control, singularity detection, -and every Jacobian-based inverse-kinematics solver need it. Its transpose maps end-effector -forces/torques back to joint torques — so the same matrix does velocity kinematics *and* statics, -exactly the reuse an embedded arm controller wants. - -## How it works (intuition) -Each joint contributes one column. A **revolute** joint spins the tool about its own axis `zᵢ`, so it -adds angular velocity `zᵢ` and linear velocity `zᵢ × r`, where `r` is the lever arm from the joint to -the tool. A **prismatic** joint slides the tool along `zᵢ`, adding pure linear velocity `zᵢ` and no -rotation. Stack those columns and you have the map `twist = J·q̇`. When two columns line up the arm is -**singular** — it has locally lost a direction of motion, and `J` can no longer be inverted safely. - -## Key parameters -- **joint axes `zᵢ`** and **origins `oᵢ`** — read off the forward-kinematics frame chain. -- **joint types** — revolute vs prismatic pick the column formula. -- **twist ordering** — `(v; ω)` (linear first) here; must be consistent everywhere downstream. - -## Reference -K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 5 (velocity kinematics and the Jacobian). - -## See also -`DenavitHartenberg` (M7) / `ProductOfExponentials` (M15) supply the frames; `ManipulabilityIndex` -(M11) scores it; `PoseInverseKinematics` (M13) and `RedundancyResolution` (M14) invert it. diff --git a/roadmap/kinematics/SpatialJacobian/implementation.md b/roadmap/kinematics/SpatialJacobian/implementation.md deleted file mode 100644 index 36516507..00000000 --- a/roadmap/kinematics/SpatialJacobian/implementation.md +++ /dev/null @@ -1,76 +0,0 @@ -# Spatial Jacobian (6×N) — Implementation Pseudocode - -> Roadmap ref: #M8 (Tier 2) · Target: `numerical/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -class SpatialJacobian: - # geometric Jacobian in the base frame: - # Matrix columns = [ linear (3); angular (3) ] - DenavitHartenberg model # or a screw / SE(3) chain -``` - -## Interface - -``` -SpatialJacobian(DenavitHartenberg model) -Matrix Compute(JointVector q) # geometric J; hot path -Vector EndEffectorTwist(JointVector q, JointVector qdot) # J·q̇ -JointVector JointTorques(JointVector q, Vector wrench) # Jᵀ·w (statics) -``` - -## Algorithm (pseudocode) - -``` -function Compute(q): # OPTIMIZE_FOR_SPEED - frames = model.FrameChain(q) # every ⁰Tᵢ (reuse M7/M6) - oₙ = frames[N].p # end-effector origin - for i in 0..N-1: - zᵢ = frames[i].R * ẑ # joint axis in base frame - oᵢ = frames[i].p - if joint i is Revolute: - Jv = CrossProduct(zᵢ, oₙ - oᵢ) # reuse Geometry3D - Jw = zᵢ - else: # Prismatic - Jv = zᵢ - Jw = 0 - column i = concat(Jv, Jw) # 6-vector - return J - -function EndEffectorTwist(q, qdot): - return Compute(q) * qdot # (v; ω) - -function JointTorques(q, w): # OPTIMIZE_FOR_SPEED - return Transpose(Compute(q)) * w # τ = Jᵀ w -``` - -## Complexity & memory - -- `Compute`: `O(N)` — one `FrameChain` pass plus a cross product per column. -- `EndEffectorTwist` / `JointTorques`: `O(6N)` matrix-vector products. -- Memory: one `6×N` matrix plus the `N+1` cached frames; all stack-allocated. - -## Numerical / embedded notes - -- This **promotes** the private 3×N position Jacobian inside `InverseKinematics.hpp` to the full 6×N - form — the single dependency that unblocks pose-IK, manipulability, and redundancy. -- Twist ordering is `(v; ω)` (linear first) here; downstream consumers must match this block layout. -- Near a **singularity** two columns become linearly dependent (rank drops) — detectable via the - manipulability index (M11); do not invert `J` directly, use damped least squares. -- `Jᵀ` gives the statics/force map for free — the transpose needs no recomputation. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/kinematics/SpatialJacobian.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Compute`/`JointTorques`, and - `extern template class SpatialJacobian;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/kinematics/SpatialJacobian.cpp` → `template class SpatialJacobian;` -- Test: `numerical/kinematics/test/TestSpatialJacobian.cpp` -- Doc: `doc/kinematics/SpatialJacobian.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestSpatialJacobian.cpp` → the `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/SpatialJacobian/tests.md b/roadmap/kinematics/SpatialJacobian/tests.md deleted file mode 100644 index 3f25c5c6..00000000 --- a/roadmap/kinematics/SpatialJacobian/tests.md +++ /dev/null @@ -1,60 +0,0 @@ -# Spatial Jacobian — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestSpatialJacobian : public ::testing::Test: - # 2-link unit planar arm, both revolute, in the x-y plane - DenavitHartenberg arm{ ... } - SpatialJacobian jac{ arm } -# each case below is a TEST_F(TestSpatialJacobian, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -planar_arm_column_dimensions: - Act: J = jac.Compute({0, 0}) - Assert: J is 6×2 - -stretched_arm_linear_block: - Arrange: q = (0, 0), tip at (2,0,0) - Act: J = jac.Compute(q) - Assert: Jv column0 ≈ (0, 2, 0), Jv column1 ≈ (0, 1, 0) - -revolute_angular_block_is_axis: - Assert: Jw columns ≈ ẑ for both joints (rotation about z) - -twist_matches_finite_difference: - Arrange: q̇ = (1, 0) - Assert: EndEffectorTwist ≈ (FK(q + εq̇) - FK(q)) / ε - -transpose_maps_wrench_to_torque: - Arrange: unit force f = x̂ at the tip - Assert: JointTorques ≈ hand-computed moment arms - -prismatic_column_is_pure_translation: - Arrange: chain with a prismatic joint - Assert: that column = (axis; 0) - -singular_configuration_drops_rank: - Arrange: fully stretched arm (q = (0,0)) - Assert: columns linearly dependent ⇒ rank < 2 - -zero_config_is_deterministic: - Assert: Compute is repeatable and side-effect free -``` - -## Reference vectors - -- 2-link unit arm at `q = (0,0)`: `Jv = [[0,0],[2,1],[0,0]]`, `Jw = [[0,0],[0,0],[1,1]]`. -- Unit tip force `x̂` on that arm ⇒ joint torques `(0, 0)` (force along the link line). - -## Edge cases - -- Fully stretched / folded arm ⇒ rank-deficient Jacobian (singularity). -- Prismatic-only chain ⇒ zero angular block. -- Single-joint arm ⇒ `6×1` Jacobian, no cross-column coupling. -- Twist ordering `(v; ω)` vs `(ω; v)` ⇒ pin the convention in one assertion. diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md b/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md deleted file mode 100644 index 908d2ea0..00000000 --- a/roadmap/trajectory/CartesianSlerpInterpolation/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# Cartesian Path + Orientation (SLERP) Interpolation — Overview - -## What it is -A task-space motion generator: it moves the end-effector along a **straight line** (or screw) in -Cartesian position while blending orientation with **SLERP** (spherical linear interpolation of -unit quaternions). A single scalar progress variable `s(t) ∈ [0, 1]` drives both channels so they -start and finish together. - -## Why it matters (embedded) -Many manipulator tasks are defined in the workspace, not joint space — draw a straight bead, keep a -tool square to a surface, approach along an axis. Interpolating position and orientation directly in -Cartesian space produces predictable, collision-friendly paths that joint-space interpolation cannot -guarantee. - -## How it works (intuition) -Position is a plain linear blend between the two endpoints. Orientation is trickier: naively -averaging quaternions leaves the sphere and changes speed. SLERP instead walks the **great-circle -arc** on the unit sphere at constant angular rate, giving the shortest, smoothest rotation. Feeding -`s(t)` from a trapezoidal or polynomial time law shapes how fast the pose advances along the path. - -## Key parameters -- **Start / goal pose** — Cartesian position plus a unit quaternion orientation. -- **Duration `tf`** — total move time. -- **Time scaling `s(t)`** — linear, trapezoidal, or polynomial progress law. - -## Reference -K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 9 (trajectory generation); -K. Shoemake, "Animating rotation with quaternion curves," *SIGGRAPH* 1985 (SLERP). - -## See also -`Quaternion` (item #18, provides `Slerp`), `SE3Transform` (item #M6), -`TrapezoidalProfile` / `PolynomialTrajectory` (the `s(t)` drivers). diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md b/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md deleted file mode 100644 index 11f5cd12..00000000 --- a/roadmap/trajectory/CartesianSlerpInterpolation/implementation.md +++ /dev/null @@ -1,85 +0,0 @@ -# Cartesian Path + Orientation (SLERP) Interpolation — Implementation Pseudocode - -> Roadmap ref: #M10 (Tier 2) · Target: `numerical/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct Pose: # reuses existing types - math::Vector position # Cartesian point (m) - math::Quaternion orientation # unit quaternion (item #18) - -template # static_assert(std::is_floating_point_v); instantiated for float -class CartesianSlerpInterpolation: - Pose start, goal - T duration # tf > 0 - T dot, theta, sinTheta # precomputed SLERP geometry - bool useNlerp # near-parallel fallback flag -``` - -## Interface - -``` -CartesianSlerpInterpolation(Pose start, Pose goal, T tf) -Pose Sample(T t) # hot path -T Duration() -void SetTimeScaling(scaling) # linear, or a Trapezoidal/Polynomial s(t) driver -``` - -## Algorithm (pseudocode) - -``` -function plan(): # precompute orientation geometry once - if dot(start.q, goal.q) < 0: # pick the shortest of the two arcs - goal.q = -goal.q - dot = clamp(dot(start.q, goal.q), -1, 1) - theta = acos(dot) - useNlerp = (dot > 0.9995) # arc too small ⇒ linear blend + normalize - sinTheta = sin(theta) - -function Sample(t): # OPTIMIZE_FOR_SPEED - s = timeScaling(clamp(t, 0, duration) / duration) # s ∈ [0, 1] - # position: straight line in Cartesian space - p = start.position + s * (goal.position - start.position) - # orientation: spherical linear interpolation - if useNlerp: - q = normalize( (1 - s)*start.q + s*goal.q ) - else: - w0 = sin((1 - s)*theta) / sinTheta - w1 = sin( s *theta) / sinTheta - q = w0*start.q + w1*goal.q # already unit-length - return Pose{ p, q } -``` - -## Complexity & memory - -- Planning: `O(1)` — one `dot`, one `acos`, one `sin`. -- `Sample`: `O(1)` — a 3-vector lerp plus two `sin` (or an nlerp + one normalize). -- Memory: `O(1)` — two poses and a few cached scalars; stack-resident, no heap. - -## Numerical / embedded notes - -- Reuse `math::Quaternion::Slerp` / `Normalize` (item #18) rather than re-deriving the blend. -- **Shortest-path fix:** negate `goal.q` when `dot < 0`; antipodal quaternions are the same rotation - but the long way round. -- **`nlerp` fallback** when `dot → 1` avoids the `1/sin θ` singularity for near-parallel orientations. -- Decouple position and orientation timing by sharing one scalar `s(t)` from a - `TrapezoidalProfile`/`PolynomialTrajectory`, so both reach the goal simultaneously. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/trajectory/CartesianSlerpInterpolation.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class CartesianSlerpInterpolation;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/trajectory/CartesianSlerpInterpolation.cpp` → - `template class CartesianSlerpInterpolation;` -- Test: `numerical/trajectory/test/TestCartesianSlerpInterpolation.cpp` -- Doc: `doc/trajectory/CartesianSlerpInterpolation.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestCartesianSlerpInterpolation.cpp` → the `_test` target. -- New module: create `numerical/trajectory/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/CMakeLists.txt`, and add a `doc/trajectory/` folder. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/CartesianSlerpInterpolation/tests.md b/roadmap/trajectory/CartesianSlerpInterpolation/tests.md deleted file mode 100644 index cb0fe85d..00000000 --- a/roadmap/trajectory/CartesianSlerpInterpolation/tests.md +++ /dev/null @@ -1,57 +0,0 @@ -# Cartesian Path + Orientation (SLERP) Interpolation — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestCartesianSlerp : public ::testing::Test: - Pose start{ .position = {0,0,0}, .orientation = Identity } - Pose goal { .position = {1,0,0}, .orientation = rot(ẑ, 90°) } - CartesianSlerpInterpolation path{ start, goal, 1.0f } -# each case below is a TEST_F(TestCartesianSlerp, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -endpoints_match_start_and_goal: - Assert: Sample(0) ≈ start and Sample(tf) ≈ goal - -position_is_straight_line: - Arrange: goal.position = (1,0,0) - Assert: Sample(tf/2).position ≈ (0.5, 0, 0) - -orientation_is_unit_at_midpoint: - Assert: |Sample(tf/2).orientation| ≈ 1 (SLERP preserves unit norm) - -slerp_midpoint_is_half_angle: - Arrange: 90° rotation about ẑ - Assert: Sample(tf/2).orientation ≈ rot(ẑ, 45°) - -shortest_path_negates_goal: - Arrange: goal.q with negative dot to start.q (obtuse) - Assert: interpolation takes the < 180° arc (angle decreases monotonically) - -near_parallel_uses_nlerp: - Arrange: goal.q within 0.01° of start.q - Assert: no NaN/inf (no 1/sinθ blow-up), result ≈ start.q - -constant_angular_rate_for_linear_scaling: - Arrange: linear s(t) - Assert: angle(start, Sample(t)) grows linearly in t - -sample_clamps_outside_domain: - Assert: Sample(-1) ≈ start and Sample(tf+1) ≈ goal -``` - -## Reference vectors - -- SLERP of Identity → `rot(ẑ, 90°)` at `s = 0.5` ⇒ `rot(ẑ, 45°)`. -- Straight-line position `p(s) = p0 + s(p1 − p0)`; midpoint of `(0,0,0)→(1,0,0)` is `(0.5,0,0)`. - -## Edge cases - -- Identical start and goal orientation ⇒ constant quaternion, `nlerp` path, no division by zero. -- Antipodal quaternions (`dot ≈ −1`) ⇒ shortest-path negation, well-defined 180° arc. -- Zero-length Cartesian move with pure rotation ⇒ position constant, orientation still slerps. diff --git a/roadmap/trajectory/PolynomialTrajectory/explanation.md b/roadmap/trajectory/PolynomialTrajectory/explanation.md deleted file mode 100644 index 83584be4..00000000 --- a/roadmap/trajectory/PolynomialTrajectory/explanation.md +++ /dev/null @@ -1,30 +0,0 @@ -# Polynomial Point-to-Point Trajectory — Overview - -## What it is -A closed-form time law that drives a joint from a start state to a goal state over a fixed -duration. A **cubic** matches endpoint position and velocity; a **quintic** additionally matches -endpoint acceleration, giving a smoother (bounded-jerk) motion. - -## Why it matters (embedded) -Point-to-point moves are the bread and butter of manipulator motion. The coefficients are solved -**once** from a handful of algebraic expressions, after which each servo tick is just a Horner -polynomial evaluation — no iteration, no lookup table, fully deterministic cycle count. - -## How it works (intuition) -A degree-`n` polynomial has `n+1` free coefficients. A cubic (4 coefficients) exactly satisfies -four boundary conditions — start/end position and start/end velocity. A quintic (6 coefficients) -adds start/end acceleration. Because the boundary equations are linear in the coefficients and the -time basis is fixed, the solution is a plug-in formula rather than a matrix solve. - -## Key parameters -- **Boundary conditions** — start/end position, velocity, and (quintic) acceleration. -- **Duration `tf`** — the move time; sets the peak velocity/acceleration for a given distance. -- **Degree** — cubic (velocity-matched) or quintic (acceleration-matched, jerk-limited endpoints). - -## Reference -M. W. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 5 (polynomial -trajectories). - -## See also -`TrapezoidalProfile` (limit-respecting, not fixed-time), `SCurveProfile` (jerk-bounded), -`CartesianSlerpInterpolation` (task-space paths). diff --git a/roadmap/trajectory/PolynomialTrajectory/implementation.md b/roadmap/trajectory/PolynomialTrajectory/implementation.md deleted file mode 100644 index 593ab853..00000000 --- a/roadmap/trajectory/PolynomialTrajectory/implementation.md +++ /dev/null @@ -1,86 +0,0 @@ -# Polynomial Point-to-Point Trajectory — Implementation Pseudocode - -> Roadmap ref: #M2 (Tier 1) · Target: `numerical/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct BoundaryConditions: # per joint - T q0, qf # start / end position - T v0 = 0, vf = 0 # start / end velocity - T a0 = 0, af = 0 # start / end acceleration (quintic only) - -template # static_assert(std::is_floating_point_v); instantiated for float -struct TrajectoryState: - T position, velocity, acceleration - -template # static_assert(std::is_floating_point_v); instantiated for float -class PolynomialTrajectory: - array coeff # a0..a5 (cubic uses a0..a3) - T duration # tf > 0 - uint8_t degree # 3 = cubic, 5 = quintic -``` - -## Interface - -``` -PolynomialTrajectory(BoundaryConditions bc, T tf, Degree degree) -TrajectoryState Sample(T t) # hot path -T Duration() -void Reset(BoundaryConditions bc, T tf) -``` - -## Algorithm (pseudocode) - -``` -function solveCubic(bc, tf): # closed form, no linear solve - a0 = bc.q0 - a1 = bc.v0 - a2 = ( 3*(bc.qf-bc.q0) - (2*bc.v0 + bc.vf)*tf) / tf^2 - a3 = (-2*(bc.qf-bc.q0) + ( bc.v0 + bc.vf)*tf) / tf^3 - -function solveQuintic(bc, tf): # six matched boundary conditions - a0 = bc.q0; a1 = bc.v0; a2 = bc.a0 / 2 - a3 = ( 20*Δq - (8*bc.vf + 12*bc.v0)*tf - (3*bc.a0 - bc.af)*tf^2) / (2*tf^3) - a4 = (-30*Δq + (14*bc.vf + 16*bc.v0)*tf + (3*bc.a0 - 2*bc.af)*tf^2) / (2*tf^4) - a5 = ( 12*Δq - ( 6*bc.vf + 6*bc.v0)*tf - ( bc.a0 - bc.af)*tf^2) / (2*tf^5) - # Δq = qf - q0 - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, duration) - pos = Horner(coeff, t) # a0 + a1 t + ... + a5 t^5 - vel = Horner(derivative(coeff), t) - acc = Horner(secondDerivative(coeff), t) - return { pos, vel, acc } -``` - -## Complexity & memory - -- Coefficient solve: `O(1)` once at construction (closed-form expressions). -- `Sample`: `O(degree)` — three Horner evaluations, ≤ 15 multiply-adds. -- Memory: `O(1)` — six coefficients plus a duration; all stack-resident, no heap. - -## Numerical / embedded notes - -- Solve coefficients **once** at construction; `Sample` is then pure Horner — deterministic cycles. -- Multi-joint moves: hold one instance per joint, or template the state on `math::Vector`. -- Guard `tf > 0`; the `1/tf^k` terms blow up for a zero-duration segment. -- Quintic gives continuous acceleration (zero jerk endpoints); prefer it when actuators are jerk-sensitive. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/trajectory/PolynomialTrajectory.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class PolynomialTrajectory;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/trajectory/PolynomialTrajectory.cpp` → - `template class PolynomialTrajectory;` -- Test: `numerical/trajectory/test/TestPolynomialTrajectory.cpp` -- Doc: `doc/trajectory/PolynomialTrajectory.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestPolynomialTrajectory.cpp` → the `_test` target. -- New module: create `numerical/trajectory/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/CMakeLists.txt`, and add a `doc/trajectory/` folder. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/trajectory/PolynomialTrajectory/tests.md b/roadmap/trajectory/PolynomialTrajectory/tests.md deleted file mode 100644 index 727fdeb2..00000000 --- a/roadmap/trajectory/PolynomialTrajectory/tests.md +++ /dev/null @@ -1,57 +0,0 @@ -# Polynomial Point-to-Point Trajectory — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestPolynomialTrajectory : public ::testing::Test: - BoundaryConditions restToRest{ .q0 = 0, .qf = 1 } # v0=vf=0 - PolynomialTrajectory cubic{ restToRest, 2.0f, Degree::Cubic } -# each case below is a TEST_F(TestPolynomialTrajectory, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -endpoints_match_boundary_positions: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 1 - -endpoint_velocities_are_matched: - Arrange: rest-to-rest cubic - Assert: Sample(0).velocity ≈ 0 and Sample(tf).velocity ≈ 0 - -cubic_midpoint_is_symmetric: - Arrange: q0=0, qf=1, tf=2 rest-to-rest - Assert: Sample(1.0).position ≈ 0.5 (half distance at half time) - -cubic_peak_velocity_matches_formula: - Arrange: rest-to-rest, distance d, duration tf - Assert: max |velocity| ≈ 1.5 * d / tf (at midpoint) - -quintic_endpoint_accelerations_are_zero: - Arrange: quintic, a0 = af = 0 - Assert: Sample(0).acceleration ≈ 0 and Sample(tf).acceleration ≈ 0 - -nonzero_boundary_velocity_is_honoured: - Arrange: v0 = 0.5, vf = -0.5 - Assert: Sample(0).velocity ≈ 0.5 and Sample(tf).velocity ≈ -0.5 - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) - -reset_recomputes_coefficients: - Arrange: Reset to q0=1, qf=3, tf=1 - Assert: new endpoints reproduced exactly -``` - -## Reference vectors - -- Rest-to-rest cubic, `q0=0, qf=1, tf=2`: `q(t) = 3(t/2)² − 2(t/2)³`; `q(1) = 0.5`, `v(1) = 0.75`. -- Peak velocity of a rest-to-rest cubic `= 1.5·(qf−q0)/tf`. - -## Edge cases - -- Very small `tf` ⇒ large coefficients; assert no overflow / `inf`. -- `q0 == qf` with zero boundary velocities ⇒ constant, zero velocity everywhere. -- Cubic vs quintic on the same move ⇒ quintic has smoother (bounded-jerk) acceleration. diff --git a/roadmap/trajectory/SCurveProfile/explanation.md b/roadmap/trajectory/SCurveProfile/explanation.md deleted file mode 100644 index 9f5042f9..00000000 --- a/roadmap/trajectory/SCurveProfile/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Overview - -## What it is -A **seven-segment, jerk-bounded** motion profile (a "double-S" velocity curve). It extends the -trapezoid by ramping acceleration in and out with a bounded rate of change (jerk), so acceleration -is continuous rather than stepping instantaneously. - -## Why it matters (embedded) -Sudden acceleration jumps excite structural resonances, wear gearing, and spill liquids or wobble -payloads. Limiting **jerk** smooths those transitions, cutting vibration and mechanical stress at -the cost of a slightly longer move. It is the profile of choice for high-precision or -vibration-sensitive machines (pick-and-place, CNC, semiconductor handling). - -## How it works (intuition) -The velocity curve is built from seven phases: jerk up, hold acceleration, jerk down (to reach -cruise velocity), cruise, then the mirror-image deceleration triple. Depending on the distance and -limits, some phases shrink to zero — a short move may never reach `aMax` or `vMax`. All phase -durations follow from closed-form algebra on the velocity/acceleration/jerk ceilings. - -## Key parameters -- **`vMax`** — cruise-velocity ceiling. -- **`aMax`** — acceleration ceiling (flat top of the acceleration curve). -- **`jMax`** — jerk ceiling; the slope of the acceleration ramps and the vibration knob. - -## Reference -L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), -Ch. 3 (double-S / jerk-limited profiles). - -## See also -`TrapezoidalProfile` (jerk-unbounded predecessor), `PolynomialTrajectory` (fixed-time quintic), -`TimeOptimalPathParameterization` (limits along a geometric path). diff --git a/roadmap/trajectory/SCurveProfile/implementation.md b/roadmap/trajectory/SCurveProfile/implementation.md deleted file mode 100644 index f772d567..00000000 --- a/roadmap/trajectory/SCurveProfile/implementation.md +++ /dev/null @@ -1,92 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Implementation Pseudocode - -> Roadmap ref: #M9 (Tier 2) · Target: `numerical/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct MotionLimits: - T vMax, aMax, jMax # velocity, acceleration, jerk ceilings (> 0) - -template # static_assert(std::is_floating_point_v); instantiated for float -struct TrajectoryState: - T position, velocity, acceleration, jerk - -template # static_assert(std::is_floating_point_v); instantiated for float -class SCurveProfile: - T q0, direction - array segT # duration of each of the 7 phases - array segPos, segVel, segAcc # state at each phase boundary - T tf -``` - -## Interface - -``` -SCurveProfile(T q0, T qf, MotionLimits limits) -TrajectoryState Sample(T t) # hot path -T Duration() -bool ReachesMaxAccel() / ReachesMaxVel() -``` - -## Algorithm (pseudocode) - -``` -# Seven phases: [+j][a=const][-j][v=const][-j][a=const][+j] -function plan(q0, qf, lim): - d = |qf - q0|; direction = sign(qf - q0) - # --- acceleration sub-phase durations (rest-to-rest) --- - if (lim.vMax * lim.jMax) >= lim.aMax^2: # aMax is reached - Tj = lim.aMax / lim.jMax - Ta = Tj + lim.vMax / lim.aMax - else: # triangular accel (aMax not reached) - Tj = sqrt(lim.vMax / lim.jMax) - Ta = 2*Tj - # --- constant-velocity phase --- - Tv = d / lim.vMax - Ta # shrink/kill accel phase if Tv < 0 - if Tv < 0: recompute Ta, Tj for a short move (no cruise) - segT = [Tj, Ta-2*Tj, Tj, Tv, Tj, Ta-2*Tj, Tj] - integrate boundary states segPos/segVel/segAcc from constant-jerk kinematics - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, tf) - i, tau = locateSegment(t) # phase index + local time - j = jerkOfPhase(i) # +jMax, 0, or -jMax - acc = segAcc[i] + j*tau - vel = segVel[i] + segAcc[i]*tau + 0.5*j*tau^2 - s = segPos[i] + segVel[i]*tau + 0.5*segAcc[i]*tau^2 + (1/6)*j*tau^3 - return { q0 + direction*s, direction*vel, direction*acc, direction*j } -``` - -## Complexity & memory - -- Planning: `O(1)` — a fixed set of algebraic phase-duration formulas, no iteration. -- `Sample`: `O(1)` — locate one of seven phases, evaluate a cubic-in-time (Horner). -- Memory: `O(1)` — three 8-entry boundary tables plus seven durations; stack only, no heap. - -## Numerical / embedded notes - -- Precompute the seven durations and boundary states **once**; each tick is one cubic evaluation. -- Handle the degenerate cases: short moves where `aMax` and/or `vMax` are never reached collapse - segments to zero length — never emit a negative duration. -- Bounded jerk means acceleration is **continuous** ⇒ far less vibration than a trapezoid; this is - the reason to pay for the extra bookkeeping. -- Profile is symmetric about its midpoint for rest-to-rest moves — reuse the accel table, mirrored. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/trajectory/SCurveProfile.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class SCurveProfile;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/trajectory/SCurveProfile.cpp` → - `template class SCurveProfile;` -- Test: `numerical/trajectory/test/TestSCurveProfile.cpp` -- Doc: `doc/trajectory/SCurveProfile.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestSCurveProfile.cpp` → the `_test` target. -- New module: create `numerical/trajectory/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/CMakeLists.txt`, and add a `doc/trajectory/` folder. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/SCurveProfile/tests.md b/roadmap/trajectory/SCurveProfile/tests.md deleted file mode 100644 index 6cc4e45a..00000000 --- a/roadmap/trajectory/SCurveProfile/tests.md +++ /dev/null @@ -1,54 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestSCurveProfile : public ::testing::Test: - MotionLimits limits{ .vMax = 1.0f, .aMax = 2.0f, .jMax = 10.0f } - SCurveProfile profile{ 0.0f, 5.0f, limits } # full 7-segment move -# each case below is a TEST_F(TestSCurveProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -endpoints_reached_exactly: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 5 - -endpoints_at_rest_and_zero_accel: - Assert: Sample(0){vel,acc} ≈ 0 and Sample(tf){vel,acc} ≈ 0 - -velocity_never_exceeds_vmax: - Assert: max |velocity| ≤ vMax (1.0) across the profile - -acceleration_never_exceeds_amax: - Assert: max |acceleration| ≤ aMax (2.0) across the profile - -jerk_never_exceeds_jmax: - Assert: every sampled |jerk| ∈ {0, jMax}, ≤ jMax (10.0) - -acceleration_is_continuous: - Arrange: dense sampling across phase joins - Assert: no acceleration step between adjacent samples (bounded by jMax·dt) - -short_move_skips_cruise: - Arrange: q0=0, qf=0.05 (very short) - Assert: ReachesMaxVel() == false, endpoints still reached - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) -``` - -## Reference vectors - -- Full profile `vMax=1, aMax=2, jMax=10`: jerk phase `Tj = aMax/jMax = 0.2 s`; - accel phase `Ta = Tj + vMax/aMax = 0.7 s`. -- Rest-to-rest S-curve is symmetric ⇒ velocity peak occurs at `tf/2`. - -## Edge cases - -- `jMax → ∞` ⇒ jerk phases vanish, profile degenerates to a trapezoid (cross-check limit). -- `vMax`/`aMax` unreachable for a short move ⇒ 4- or 2-segment degenerate profile, still valid. -- `d == 0` ⇒ zero-length profile; `Sample(0)` is the start, fully at rest. diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md b/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md deleted file mode 100644 index ac85faa5..00000000 --- a/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# Time-Optimal Path Parameterization (TOPP) — Overview - -## What it is -Given a **fixed geometric path** (the shape the end-effector or joints must follow) and the robot's -**actuator limits**, TOPP computes the fastest possible timing law `s(t)` to traverse that path -without violating any velocity or torque bound. It answers "how fast can I run *this* curve?" — the -geometry is frozen, only the speed along it is optimized. - -## Why it matters (embedded) -Separating *where* to go (path planning) from *how fast* to go (parameterization) is a powerful -decomposition. Once a collision-free path exists, TOPP squeezes maximum throughput out of the -hardware — critical for cycle-time-bound industrial robots — while provably respecting drive -saturation, so the plan never asks for torque the motors cannot deliver. - -## How it works (intuition) -Reparameterize dynamics in terms of the scalar path coordinate `s`: each joint torque becomes affine -in `s̈` and `ṡ²`. This collapses the high-dimensional problem to a 2-D `(s, ṡ)` phase plane. The -**maximum velocity curve** (MVC) marks the fastest `ṡ` still feasible at each `s`. Modern TOPP-RA -then runs a backward "controllable set" pass and a forward "reachable set" pass over a grid, each -step solving a tiny 1-D linear program — numerically robust, linear-time, and free of the fragile -switch-point search in Bobrow's original phase-plane method. - -## Key parameters -- **Path geometry** — `q(s)` and its first/second derivatives along `s ∈ [0, 1]`. -- **Velocity & torque limits** — per-joint bounds, torques via inverse dynamics (RNEA). -- **Grid resolution** — number of `s` samples; trades accuracy against planning cost. - -## Reference -J. Bobrow, S. Dubowsky, J. Gibson, "Time-Optimal Control of Robotic Manipulators Along Specified -Paths," *IJRR* 4(3), 1985; Q.-C. Pham, "A General, Fast, and Robust Implementation of the -Time-Optimal Path Parameterization Algorithm" (TOPP-RA), *IEEE T-RO*, 2014. - -## See also -`CartesianSlerpInterpolation` (supplies the path), `RecursiveNewtonEuler` (torque limits), -`SCurveProfile` (heuristic limit-respecting alternative). diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md deleted file mode 100644 index 8cc98700..00000000 --- a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md +++ /dev/null @@ -1,95 +0,0 @@ -# Time-Optimal Path Parameterization (TOPP) — Implementation Pseudocode - -> Roadmap ref: #M27 (Tier 5) · Target: `numerical/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -interface PathGeometry: # DI — the fixed geometric path q(s), s ∈ [0,1] - math::Vector Position(T s) - math::Vector FirstDerivative(T s) # q'(s) - math::Vector SecondDerivative(T s) # q''(s) - -template # static_assert(std::is_floating_point_v); instantiated for float -interface JointLimits: # DI — inverse dynamics + bounds (reuse RNEA) - void Coefficients(T s, out a, out b, out c) # τ = a(s)·s̈ + b(s)·ṡ² + c(s) - Bounds TorqueBounds(), VelocityBounds() - -template # static_assert(std::is_floating_point_v); instantiated for float -class TimeOptimalPathParameterization: - array sGrid # discretized path parameter - array xMax # MVC: max ṡ² per gridpoint - array xProfile # controllable ṡ² after reachability pass -``` - -## Interface - -``` -TimeOptimalPathParameterization(PathGeometry&, JointLimits&) -bool Parameterize() # build ṡ²(s); false if infeasible -Optional> Sample(T t) # hot path, after parameterization -T TotalTime() -``` - -## Algorithm (pseudocode) - -``` -# Per gridpoint the joint torque law is affine in (u = s̈, x = ṡ²): -# τ_j(s) = a_j(s)·u + b_j(s)·x + c_j(s) ∈ [τ_min, τ_max] - -function maxVelocityCurve(s): # MVC — tightest ṡ² still feasible - x_v = min_j ( velLimit_j / |q'_j(s)| )^2 # velocity bound - x_a = largest x where some u keeps every τ_j in bounds # accel/torque bound - return min(x_v, x_a) - -function Parameterize(): # TOPP-RA reachability analysis - for i in 0..Grid-1: xMax[i] = maxVelocityCurve(sGrid[i]) - # backward pass: controllable set ending at rest (x_N = 0) - for i = Grid-1 .. 0: - [uLo, uHi] = admissibleAccel(sGrid[i], xProfile[i]) # 1-D LP over joints - xProfile[i] = min(xMax[i], propagateBack(xProfile[i+1], uLo, uHi)) - # forward pass: reachable set starting from rest (x_0 = 0), clipped to controllable - for i = 0 .. Grid-1: - xProfile[i] = min(xProfile[i], propagateForward(xProfile[i-1])) - return all(xProfile >= 0) - -function Sample(t): # OPTIMIZE_FOR_SPEED - # integrate dt = ds / sqrt(x(s)) offline into a time->s table, then look up - s = timeToS(t) - sd = sqrt(interp(xProfile, s)) # ṡ - sdd = admissibleAccelMid(s) # s̈ - return jointState(path, s, sd, sdd) -``` - -## Complexity & memory - -- Parameterization: `O(Grid · N)` — each gridpoint solves a tiny 1-D LP over `N` joints. -- Two linear passes (backward controllable + forward reachable), no global optimization loop. -- Memory: `O(Grid)` bounded arrays (`sGrid`, `xMax`, `xProfile`); planning-time, stack/static only. - -## Numerical / embedded notes - -- This is a **planning-time** computation, not an ISR path — run offline, then stream `Sample`. -- Reuse **RNEA** (item exists) to evaluate the `a(s), b(s), c(s)` inverse-dynamics coefficients per - gridpoint; inject it behind `JointLimits` so the planner stays dynamics-agnostic (DIP). -- Grid density trades accuracy for memory/time; `Grid` is a compile-time bound (`std::array`). -- Guard **MVC singularities** (a `q'_j(s)=0` "zero-inertia" direction) — clamp, don't divide by zero. -- TOPP-RA's reachability formulation is numerically robust where Bobrow's switch-point search stalls. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/trajectory/TimeOptimalPathParameterization.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class TimeOptimalPathParameterization;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/trajectory/TimeOptimalPathParameterization.cpp` → - `template class TimeOptimalPathParameterization;` -- Test: `numerical/trajectory/test/TestTimeOptimalPathParameterization.cpp` -- Doc: `doc/trajectory/TimeOptimalPathParameterization.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestTimeOptimalPathParameterization.cpp` → the `_test` target. -- New module: create `numerical/trajectory/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/CMakeLists.txt`, and add a `doc/trajectory/` folder. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/tests.md b/roadmap/trajectory/TimeOptimalPathParameterization/tests.md deleted file mode 100644 index 1c5cf676..00000000 --- a/roadmap/trajectory/TimeOptimalPathParameterization/tests.md +++ /dev/null @@ -1,59 +0,0 @@ -# Time-Optimal Path Parameterization (TOPP) — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestTopp : public ::testing::Test: - StrictMock> path # DI: q(s), q'(s), q''(s) - StrictMock> limits # DI: RNEA coeffs + bounds - TimeOptimalPathParameterization topp{ path, limits } -# each case below is a TEST_F(TestTopp, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -straight_path_respects_velocity_limit: - Arrange: linear q(s), velocity limit vMax - Act: Parameterize(); sample ṡ along path - Assert: every joint velocity |q'(s)·ṡ| ≤ vMax - -profile_respects_torque_bounds: - Arrange: mock RNEA coeffs a,b,c with symmetric torque limits - Assert: reconstructed τ_j(s) ∈ [τ_min, τ_max] at all gridpoints - -starts_and_ends_at_rest: - Assert: ṡ(0) ≈ 0 and ṡ(1) ≈ 0 (x_0 = x_N = 0) - -is_faster_than_uniform_traversal: - Arrange: same path, fixed slow ṡ - Assert: TotalTime() < uniform-traversal time (optimality sanity check) - -infeasible_path_reports_failure: - Arrange: torque bounds too tight to move - Assert: Parameterize() == false - -bang_bang_structure_on_simple_path: - Arrange: single-joint, constant inertia - Assert: accel is at a bound almost everywhere (max-accel then max-decel) - -denser_grid_refines_time: - Arrange: Grid 32 vs 128 - Assert: TotalTime() converges (monotone, bounded change) - -sample_before_parameterize_returns_nullopt: - Assert: Sample(t) == nullopt until Parameterize() succeeds -``` - -## Reference vectors - -- Single joint, unit inertia, `|τ| ≤ 1`, path length `L`, rest-to-rest ⇒ bang-bang time `2√L`. -- Velocity-limited straight segment ⇒ `ṡ_max = vMax / |q'(s)|` on the flat portion. - -## Edge cases - -- Zero-length path (`q(s)` constant) ⇒ `TotalTime() == 0`, trivial success. -- MVC singularity where `q'_j(s) = 0` ⇒ no division blow-up, profile stays finite. -- Velocity-only vs torque-only limiting ⇒ correct binding constraint selected per gridpoint. diff --git a/roadmap/trajectory/TrapezoidalProfile/explanation.md b/roadmap/trajectory/TrapezoidalProfile/explanation.md deleted file mode 100644 index 6e98ef69..00000000 --- a/roadmap/trajectory/TrapezoidalProfile/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Overview - -## What it is -A **linear-segment-with-parabolic-blends** (LSPB) motion profile: accelerate at a constant rate, -cruise at a constant velocity, then decelerate at a constant rate. The velocity-vs-time graph is a -trapezoid (or a triangle for short moves that never reach cruise speed). - -## Why it matters (embedded) -It is the standard "get there as fast as the actuator allows" profile. Instead of fixing the -duration, you fix the **velocity and acceleration limits** and the profile computes the minimum -time that respects them — exactly what a motion controller needs to drive real hardware without -saturating drives. - -## How it works (intuition) -Time splits into three phases with a simple symmetry: the accel and decel ramps are mirror images. -If the move is long enough, velocity saturates at `vMax` and coasts; if it is short, the profile -peaks below `vMax` and immediately decelerates — a triangle. A single distance test -(`d ≥ vMax²/aMax`) decides which case applies. - -## Key parameters -- **`vMax`** — cruise velocity ceiling; caps the flat top of the trapezoid. -- **`aMax`** — ramp acceleration; sets the blend duration `vMax/aMax`. -- **Distance `qf − q0`** — decides trapezoid vs triangle and the total time. - -## Reference -L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), -Ch. 3 (trapezoidal / LSPB profiles). - -## See also -`PolynomialTrajectory` (fixed-time, smooth), `SCurveProfile` (jerk-limited upgrade), -`SaturationRateLimiter` (online slew limiting). diff --git a/roadmap/trajectory/TrapezoidalProfile/implementation.md b/roadmap/trajectory/TrapezoidalProfile/implementation.md deleted file mode 100644 index 52f8fa06..00000000 --- a/roadmap/trajectory/TrapezoidalProfile/implementation.md +++ /dev/null @@ -1,90 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Implementation Pseudocode - -> Roadmap ref: #M3 (Tier 1) · Target: `numerical/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -``` -template # static_assert(std::is_floating_point_v); instantiated for float -struct MotionLimits: - T vMax # peak velocity (> 0) - T aMax # peak accel (> 0) - -template # static_assert(std::is_floating_point_v); instantiated for float -struct TrajectoryState: - T position, velocity, acceleration - -template # static_assert(std::is_floating_point_v); instantiated for float -class TrapezoidalProfile: - T q0, direction # start, sign(qf - q0) - T vPeak, aMax # cruise velocity actually reached - T tAccel, tCruise, tf # phase boundaries (blend, flat, total) -``` - -## Interface - -``` -TrapezoidalProfile(T q0, T qf, MotionLimits limits) -TrajectoryState Sample(T t) # hot path -T Duration() -bool IsTriangular() # true when vMax is never reached -``` - -## Algorithm (pseudocode) - -``` -function plan(q0, qf, limits): - d = |qf - q0|; direction = sign(qf - q0) - dBlend = limits.vMax^2 / limits.aMax # distance used by accel + decel - if d >= dBlend: # trapezoid: cruise phase exists - vPeak = limits.vMax - tAccel = vPeak / limits.aMax - tCruise = (d - dBlend) / vPeak - else: # triangle: peak below vMax - vPeak = sqrt(d * limits.aMax) - tAccel = vPeak / limits.aMax - tCruise = 0 - tf = 2*tAccel + tCruise - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, tf) - if t < tAccel: # parabolic ramp-up - acc = aMax; vel = aMax*t; s = 0.5*aMax*t^2 - else if t < tAccel + tCruise: # linear cruise - acc = 0; vel = vPeak; s = vPeak*(t - 0.5*tAccel) - else: # parabolic ramp-down - td = tf - t - acc = -aMax; vel = aMax*td; s = d - 0.5*aMax*td^2 - return { q0 + direction*s, direction*vel, direction*acc } -``` - -## Complexity & memory - -- Planning: `O(1)` — one `sqrt` and a branch to pick trapezoid vs triangle. -- `Sample`: `O(1)` — one phase branch, a couple of multiply-adds. -- Memory: `O(1)` — six scalars; entirely stack-resident, no heap. - -## Numerical / embedded notes - -- Precompute the three phase boundaries **once**; `Sample` then reduces to one branch per tick. -- Velocity is continuous but acceleration is **discontinuous** at blend joins (bounded jerk spikes) — - upgrade to `SCurveProfile` when that excites structural modes. -- Guard degenerate inputs: `d == 0` ⇒ zero-length profile; `aMax == 0` or `vMax == 0` ⇒ reject. -- Symmetric ramps mean the decel phase mirrors accel — reuse the same `aMax`, flip the sign. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/trajectory/TrapezoidalProfile.hpp` — `#pragma once` → - `#pragma GCC optimize("O3","fast-math")`, `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class TrapezoidalProfile;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/trajectory/TrapezoidalProfile.cpp` → - `template class TrapezoidalProfile;` -- Test: `numerical/trajectory/test/TestTrapezoidalProfile.cpp` -- Doc: `doc/trajectory/TrapezoidalProfile.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `numerical_add_coverage_sources`; - `TestTrapezoidalProfile.cpp` → the `_test` target. -- New module: create `numerical/trajectory/CMakeLists.txt` via `numerical_add_header_library(...)`, - add a `test/` subdir, register it in `numerical/CMakeLists.txt`, and add a `doc/trajectory/` folder. -- Generic pattern: see `roadmap/README.md` → "Deployment shape". diff --git a/roadmap/trajectory/TrapezoidalProfile/tests.md b/roadmap/trajectory/TrapezoidalProfile/tests.md deleted file mode 100644 index b5defe4f..00000000 --- a/roadmap/trajectory/TrapezoidalProfile/tests.md +++ /dev/null @@ -1,56 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -``` -class TestTrapezoidalProfile : public ::testing::Test: - MotionLimits limits{ .vMax = 1.0f, .aMax = 2.0f } - TrapezoidalProfile profile{ 0.0f, 5.0f, limits } # long move ⇒ trapezoid -# each case below is a TEST_F(TestTrapezoidalProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -``` -endpoints_reached_exactly: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 5 - -endpoints_are_at_rest: - Assert: Sample(0).velocity ≈ 0 and Sample(tf).velocity ≈ 0 - -cruise_velocity_is_clamped_to_vmax: - Arrange: long move - Assert: max velocity over the profile ≈ vMax (1.0), never exceeds it - -acceleration_is_within_limit: - Assert: |acceleration| ≤ aMax at all sampled instants - -short_move_is_triangular: - Arrange: q0=0, qf=0.1 (d < vMax²/aMax) - Assert: IsTriangular() == true and peak velocity < vMax - -phase_durations_match_trapezoid_formula: - Arrange: d=5, vMax=1, aMax=2 - Assert: tAccel ≈ 0.5, tCruise ≈ 4.5, Duration ≈ 5.5 - -negative_direction_move: - Arrange: q0=2, qf=0 - Assert: velocity ≤ 0 throughout, endpoints (2 → 0) reached - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) -``` - -## Reference vectors - -- `d=5, vMax=1, aMax=2`: `tAccel = vMax/aMax = 0.5`, `tCruise = (d − vMax²/aMax)/vMax = 4.5`, - `tf = 5.5`. -- Triangular threshold distance `= vMax²/aMax = 0.5`. - -## Edge cases - -- `d` exactly equal to `vMax²/aMax` ⇒ zero cruise time (trapezoid degenerates to triangle). -- `d == 0` ⇒ `tf == 0`, `Sample(0)` is the start at rest. -- `aMax` very large ⇒ ramps vanish, profile approaches a pure velocity step. diff --git a/simulator/CMakeLists.txt b/simulator/CMakeLists.txt index c717691c..24566f25 100644 --- a/simulator/CMakeLists.txt +++ b/simulator/CMakeLists.txt @@ -2,7 +2,6 @@ add_subdirectory(utils) add_subdirectory(widgets) add_subdirectory(analysis) add_subdirectory(controllers) -add_subdirectory(dynamics) add_subdirectory(estimators) add_subdirectory(filters) add_subdirectory(neural_network) diff --git a/simulator/dynamics/CMakeLists.txt b/simulator/dynamics/CMakeLists.txt deleted file mode 100644 index 9156ea79..00000000 --- a/simulator/dynamics/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -add_subdirectory(RobotArm) diff --git a/simulator/dynamics/RobotArm/CMakeLists.txt b/simulator/dynamics/RobotArm/CMakeLists.txt deleted file mode 100644 index 86466b0a..00000000 --- a/simulator/dynamics/RobotArm/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -find_package(Qt6 REQUIRED COMPONENTS Widgets) - -add_subdirectory(application) -add_subdirectory(view) - -add_executable(numerical.simulator.dynamics.robot_arm) -emil_build_for(numerical.simulator.dynamics.robot_arm HOST All PREREQUISITE_BOOL NUMERICAL_TOOLBOX_BUILD_SIMULATOR) - -target_link_libraries(numerical.simulator.dynamics.robot_arm PRIVATE - numerical.simulator.dynamics.robotarm.application - numerical.simulator.dynamics.robotarm.view -) - -target_sources(numerical.simulator.dynamics.robot_arm PRIVATE - Main.cpp -) - -if(MSVC) - target_compile_options(numerical.simulator.dynamics.robot_arm PRIVATE /W4 /wd4244 /wd4100) -else() - target_compile_options(numerical.simulator.dynamics.robot_arm PRIVATE - -Wno-error - ) -endif() diff --git a/simulator/dynamics/RobotArm/Main.cpp b/simulator/dynamics/RobotArm/Main.cpp deleted file mode 100644 index 9d4190ce..00000000 --- a/simulator/dynamics/RobotArm/Main.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include "simulator/dynamics/RobotArm/view/RobotArmMainWindow.hpp" -#include - -int main(int argc, char* argv[]) -{ - QApplication app(argc, argv); - - simulator::dynamics::view::RobotArmMainWindow window; - window.show(); - - return app.exec(); -} diff --git a/simulator/dynamics/RobotArm/application/CMakeLists.txt b/simulator/dynamics/RobotArm/application/CMakeLists.txt deleted file mode 100644 index 2a57e8bf..00000000 --- a/simulator/dynamics/RobotArm/application/CMakeLists.txt +++ /dev/null @@ -1,24 +0,0 @@ -add_library(numerical.simulator.dynamics.robotarm.application STATIC) - -target_include_directories(numerical.simulator.dynamics.robotarm.application PUBLIC - "$" - "$" -) - -target_link_libraries(numerical.simulator.dynamics.robotarm.application PUBLIC - numerical.dynamics - numerical.kinematics -) - -target_sources(numerical.simulator.dynamics.robotarm.application PRIVATE - RobotArmSimulator.cpp - RobotArmSimulator.hpp -) - -if(MSVC) - target_compile_options(numerical.simulator.dynamics.robotarm.application PRIVATE /W4 /wd4244 /wd4100) -else() - target_compile_options(numerical.simulator.dynamics.robotarm.application PRIVATE - -Wno-error - ) -endif() diff --git a/simulator/dynamics/RobotArm/application/RobotArmSimulator.cpp b/simulator/dynamics/RobotArm/application/RobotArmSimulator.cpp deleted file mode 100644 index 1dd9f124..00000000 --- a/simulator/dynamics/RobotArm/application/RobotArmSimulator.cpp +++ /dev/null @@ -1,218 +0,0 @@ -#include "simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp" - -namespace simulator::dynamics -{ - void RobotArmSimulator::Configure(const RobotArmConfig& cfg) - { - config = cfg; - torques.assign(config.dof, 0.0f); - Reset(); - } - - void RobotArmSimulator::SetTorques(const std::vector& t) - { - for (int i = 0; i < config.dof && i < static_cast(t.size()); ++i) - torques[i] = t[i]; - } - - void RobotArmSimulator::SetInitialPositions(const std::vector& positions) - { - for (int i = 0; i < config.dof && i < static_cast(positions.size()); ++i) - state.q[i] = positions[i]; - - state.qDot.assign(config.dof, 0.0f); - UpdateForwardKinematics(); - } - - void RobotArmSimulator::Step() - { - if (config.dof == 2) - StepDof2(); - else - StepDof3(); - - for (int i = 0; i < config.dof; ++i) - state.qDot[i] *= (1.0f - config.damping * config.dt); - - state.time += config.dt; - UpdateForwardKinematics(); - UpdateTrail(); - } - - void RobotArmSimulator::Reset() - { - state.q.assign(config.dof, 0.0f); - state.qDot.assign(config.dof, 0.0f); - state.qDDot.assign(config.dof, 0.0f); - state.inverseDynamicsTorques.assign(config.dof, 0.0f); - state.time = 0.0f; - state.endEffectorTrail.clear(); - UpdateForwardKinematics(); - } - - const RobotArmState& RobotArmSimulator::GetState() const - { - return state; - } - - const RobotArmConfig& RobotArmSimulator::GetConfig() const - { - return config; - } - - void RobotArmSimulator::StepDof2() - { - auto links = BuildLinks2(); - - math::Vector q{ { state.q[0] }, { state.q[1] } }; - math::Vector qDot{ { state.qDot[0] }, { state.qDot[1] } }; - math::Vector tau{ { torques[0] }, { torques[1] } }; - math::Vector g{ { 0.0f }, { 0.0f }, { -gravity } }; - - // ABA: forward dynamics — compute accelerations from applied torques - auto qDDot = aba2.ForwardDynamics(links, q, qDot, tau, g); - - // Semi-implicit Euler integration - for (int i = 0; i < 2; ++i) - { - state.qDDot[i] = qDDot.at(i, 0); - state.qDot[i] += qDDot.at(i, 0) * config.dt; - state.q[i] += state.qDot[i] * config.dt; - } - - // RNEA: inverse dynamics — compute torques required for current motion - math::Vector qNew{ { state.q[0] }, { state.q[1] } }; - math::Vector qDotNew{ { state.qDot[0] }, { state.qDot[1] } }; - auto idTorques = rnea2.InverseDynamics(links, qNew, qDotNew, qDDot, g); - - for (int i = 0; i < 2; ++i) - state.inverseDynamicsTorques[i] = idTorques.at(i, 0); - } - - void RobotArmSimulator::StepDof3() - { - auto links = BuildLinks3(); - - math::Vector q{ { state.q[0] }, { state.q[1] }, { state.q[2] } }; - math::Vector qDot{ { state.qDot[0] }, { state.qDot[1] }, { state.qDot[2] } }; - math::Vector tau{ { torques[0] }, { torques[1] }, { torques[2] } }; - math::Vector g{ { 0.0f }, { 0.0f }, { -gravity } }; - - // ABA: forward dynamics — compute accelerations from applied torques - auto qDDot = aba3.ForwardDynamics(links, q, qDot, tau, g); - - for (int i = 0; i < 3; ++i) - { - state.qDDot[i] = qDDot.at(i, 0); - state.qDot[i] += qDDot.at(i, 0) * config.dt; - state.q[i] += state.qDot[i] * config.dt; - } - - // RNEA: inverse dynamics — compute torques required for current motion - math::Vector qNew{ { state.q[0] }, { state.q[1] }, { state.q[2] } }; - math::Vector qDotNew{ { state.qDot[0] }, { state.qDot[1] }, { state.qDot[2] } }; - auto idTorques = rnea3.InverseDynamics(links, qNew, qDotNew, qDDot, g); - - for (int i = 0; i < 3; ++i) - state.inverseDynamicsTorques[i] = idTorques.at(i, 0); - } - - std::array<::dynamics::RevoluteJointLink, 2> RobotArmSimulator::BuildLinks2() const - { - std::array<::dynamics::RevoluteJointLink, 2> links; - - float L0 = config.linkLengths[0]; - float m0 = config.linkMasses[0]; - float I0 = m0 * L0 * L0 / 12.0f; - - links[0].mass = m0; - links[0].inertia = math::SquareMatrix{ { { I0, 0, 0 }, { 0, 0.001f, 0 }, { 0, 0, I0 } } }; - links[0].jointAxis = math::Vector{ { 0.0f }, { 1.0f }, { 0.0f } }; - links[0].parentToJoint = math::Vector{ { 0.0f }, { 0.0f }, { 0.0f } }; - links[0].jointToCoM = math::Vector{ { L0 / 2.0f }, { 0.0f }, { 0.0f } }; - - float L1 = config.linkLengths[1]; - float m1 = config.linkMasses[1]; - float I1 = m1 * L1 * L1 / 12.0f; - - links[1].mass = m1; - links[1].inertia = math::SquareMatrix{ { { I1, 0, 0 }, { 0, 0.001f, 0 }, { 0, 0, I1 } } }; - links[1].jointAxis = math::Vector{ { 0.0f }, { 1.0f }, { 0.0f } }; - links[1].parentToJoint = math::Vector{ { L0 }, { 0.0f }, { 0.0f } }; - links[1].jointToCoM = math::Vector{ { L1 / 2.0f }, { 0.0f }, { 0.0f } }; - - return links; - } - - std::array<::dynamics::RevoluteJointLink, 3> RobotArmSimulator::BuildLinks3() const - { - std::array<::dynamics::RevoluteJointLink, 3> links; - - float L0 = config.linkLengths[0]; - float m0 = config.linkMasses[0]; - float I0 = m0 * L0 * L0 / 12.0f; - - links[0].mass = m0; - links[0].inertia = math::SquareMatrix{ { { I0, 0, 0 }, { 0, I0, 0 }, { 0, 0, 0.001f } } }; - links[0].jointAxis = math::Vector{ { 0.0f }, { 0.0f }, { 1.0f } }; - links[0].parentToJoint = math::Vector{ { 0.0f }, { 0.0f }, { 0.0f } }; - links[0].jointToCoM = math::Vector{ { 0.0f }, { 0.0f }, { L0 / 2.0f } }; - - float L1 = config.linkLengths[1]; - float m1 = config.linkMasses[1]; - float I1 = m1 * L1 * L1 / 12.0f; - - links[1].mass = m1; - links[1].inertia = math::SquareMatrix{ { { I1, 0, 0 }, { 0, 0.001f, 0 }, { 0, 0, I1 } } }; - links[1].jointAxis = math::Vector{ { 0.0f }, { 1.0f }, { 0.0f } }; - links[1].parentToJoint = math::Vector{ { 0.0f }, { 0.0f }, { L0 } }; - links[1].jointToCoM = math::Vector{ { L1 / 2.0f }, { 0.0f }, { 0.0f } }; - - float L2 = config.linkLengths[2]; - float m2 = config.linkMasses[2]; - float I2 = m2 * L2 * L2 / 12.0f; - - links[2].mass = m2; - links[2].inertia = math::SquareMatrix{ { { I2, 0, 0 }, { 0, 0.001f, 0 }, { 0, 0, I2 } } }; - links[2].jointAxis = math::Vector{ { 0.0f }, { 1.0f }, { 0.0f } }; - links[2].parentToJoint = math::Vector{ { L1 }, { 0.0f }, { 0.0f } }; - links[2].jointToCoM = math::Vector{ { L2 / 2.0f }, { 0.0f }, { 0.0f } }; - - return links; - } - - void RobotArmSimulator::UpdateForwardKinematics() - { - int n = config.dof; - state.jointPositions.resize(n + 1); - - if (n == 2) - { - auto links = BuildLinks2(); - math::Vector q{ { state.q[0] }, { state.q[1] } }; - auto positions = fk2.Compute(links, q); - - for (int i = 0; i <= n; ++i) - state.jointPositions[i] = { positions[i].at(0, 0), positions[i].at(1, 0), positions[i].at(2, 0) }; - } - else - { - auto links = BuildLinks3(); - math::Vector q{ { state.q[0] }, { state.q[1] }, { state.q[2] } }; - auto positions = fk3.Compute(links, q); - - for (int i = 0; i <= n; ++i) - state.jointPositions[i] = { positions[i].at(0, 0), positions[i].at(1, 0), positions[i].at(2, 0) }; - } - } - - void RobotArmSimulator::UpdateTrail() - { - if (!state.jointPositions.empty()) - { - state.endEffectorTrail.push_back(state.jointPositions.back()); - if (state.endEffectorTrail.size() > maxTrailSize) - state.endEffectorTrail.erase(state.endEffectorTrail.begin()); - } - } -} diff --git a/simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp b/simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp deleted file mode 100644 index 43687182..00000000 --- a/simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include "numerical/dynamics/ArticulatedBodyAlgorithm.hpp" -#include "numerical/dynamics/RecursiveNewtonEuler.hpp" -#include "numerical/dynamics/RevoluteJointLink.hpp" -#include "numerical/kinematics/ForwardKinematics.hpp" -#include -#include -#include - -namespace simulator::dynamics -{ - struct RobotArmConfig - { - int dof = 2; - std::vector linkLengths = { 0.5f, 0.4f, 0.3f }; - std::vector linkMasses = { 1.0f, 0.8f, 0.6f }; - float damping = 0.05f; - float dt = 1.0f / 120.0f; - }; - - struct RobotArmState - { - std::vector> jointPositions; - std::vector q; - std::vector qDot; - std::vector qDDot; - std::vector inverseDynamicsTorques; - std::vector> endEffectorTrail; - float time = 0.0f; - }; - - class RobotArmSimulator - { - public: - void Configure(const RobotArmConfig& config); - void SetTorques(const std::vector& torques); - void SetInitialPositions(const std::vector& positions); - void Step(); - void Reset(); - - const RobotArmState& GetState() const; - const RobotArmConfig& GetConfig() const; - - private: - void StepDof2(); - void StepDof3(); - void UpdateForwardKinematics(); - void UpdateTrail(); - - std::array<::dynamics::RevoluteJointLink, 2> BuildLinks2() const; - std::array<::dynamics::RevoluteJointLink, 3> BuildLinks3() const; - - static constexpr float gravity = 9.81f; - static constexpr std::size_t maxTrailSize = 500; - - ::dynamics::ArticulatedBodyAlgorithm aba2; - ::dynamics::ArticulatedBodyAlgorithm aba3; - ::dynamics::RecursiveNewtonEuler rnea2; - ::dynamics::RecursiveNewtonEuler rnea3; - ::kinematics::ForwardKinematics fk2; - ::kinematics::ForwardKinematics fk3; - - RobotArmConfig config; - RobotArmState state; - std::vector torques; - }; -} diff --git a/simulator/dynamics/RobotArm/view/CMakeLists.txt b/simulator/dynamics/RobotArm/view/CMakeLists.txt deleted file mode 100644 index 89630e88..00000000 --- a/simulator/dynamics/RobotArm/view/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -add_library(numerical.simulator.dynamics.robotarm.view STATIC) - -target_include_directories(numerical.simulator.dynamics.robotarm.view PUBLIC - "$" - "$" -) - -target_link_libraries(numerical.simulator.dynamics.robotarm.view PUBLIC - numerical.simulator.dynamics.robotarm.application - Qt6::Widgets -) - -set_target_properties(numerical.simulator.dynamics.robotarm.view PROPERTIES - AUTOMOC ON -) - -target_sources(numerical.simulator.dynamics.robotarm.view PRIVATE - RobotArm3DWidget.cpp - RobotArm3DWidget.hpp - RobotArmConfigurationPanel.cpp - RobotArmConfigurationPanel.hpp - RobotArmMainWindow.cpp - RobotArmMainWindow.hpp -) - -if(MSVC) - target_compile_options(numerical.simulator.dynamics.robotarm.view PRIVATE /W4 /wd4244 /wd4100) -else() - target_compile_options(numerical.simulator.dynamics.robotarm.view PRIVATE - -Wno-error - ) -endif() diff --git a/simulator/dynamics/RobotArm/view/RobotArm3DWidget.cpp b/simulator/dynamics/RobotArm/view/RobotArm3DWidget.cpp deleted file mode 100644 index 4c906dfe..00000000 --- a/simulator/dynamics/RobotArm/view/RobotArm3DWidget.cpp +++ /dev/null @@ -1,372 +0,0 @@ -#include "simulator/dynamics/RobotArm/view/RobotArm3DWidget.hpp" -#include -#include -#include - -namespace simulator::dynamics::view -{ - RobotArm3DWidget::RobotArm3DWidget(QWidget* parent) - : QWidget(parent) - { - setMinimumSize(400, 400); - setMouseTracking(false); - } - - void RobotArm3DWidget::SetState(const RobotArmState& state, int dof) - { - currentState = state; - currentDof = dof; - update(); - } - - QPointF RobotArm3DWidget::Project(float wx, float wy, float wz) const - { - float cx = camera.distance * std::cos(camera.elevation) * std::cos(camera.azimuth) + camera.lookAtX; - float cy = camera.distance * std::cos(camera.elevation) * std::sin(camera.azimuth) + camera.lookAtY; - float cz = camera.distance * std::sin(camera.elevation) + camera.lookAtZ; - - float fx = camera.lookAtX - cx; - float fy = camera.lookAtY - cy; - float fz = camera.lookAtZ - cz; - float fLen = std::sqrt(fx * fx + fy * fy + fz * fz); - if (fLen < 1e-6f) - fLen = 1e-6f; - fx /= fLen; - fy /= fLen; - fz /= fLen; - - float rx = fy * 1.0f - fz * 0.0f; - float ry = fz * 0.0f - fx * 1.0f; - float rz = fx * 0.0f - fy * 0.0f; - float rLen = std::sqrt(rx * rx + ry * ry + rz * rz); - if (rLen < 1e-6f) - { - rx = 1.0f; - ry = 0.0f; - rz = 0.0f; - } - else - { - rx /= rLen; - ry /= rLen; - rz /= rLen; - } - - float ux = ry * fz - rz * fy; - float uy = rz * fx - rx * fz; - float uz = rx * fy - ry * fx; - - float dx = wx - cx; - float dy = wy - cy; - float dz = wz - cz; - float vx = rx * dx + ry * dy + rz * dz; - float vy = ux * dx + uy * dy + uz * dz; - float vz = fx * dx + fy * dy + fz * dz; - - float fov = 60.0f * 3.14159265f / 180.0f; - float tanHalfFov = std::tan(fov * 0.5f); - float aspect = static_cast(width()) / std::max(1, height()); - if (vz < 0.01f) - vz = 0.01f; - - float px = vx / (vz * tanHalfFov * aspect); - float py = vy / (vz * tanHalfFov); - - float sx = (px + 1.0f) * 0.5f * width(); - float sy = (1.0f - py) * 0.5f * height(); - - return QPointF(sx, sy); - } - - float RobotArm3DWidget::ProjectDepth(float wx, float wy, float wz) const - { - float cx = camera.distance * std::cos(camera.elevation) * std::cos(camera.azimuth) + camera.lookAtX; - float cy = camera.distance * std::cos(camera.elevation) * std::sin(camera.azimuth) + camera.lookAtY; - float cz = camera.distance * std::sin(camera.elevation) + camera.lookAtZ; - - float fx = camera.lookAtX - cx; - float fy = camera.lookAtY - cy; - float fz = camera.lookAtZ - cz; - float fLen = std::sqrt(fx * fx + fy * fy + fz * fz); - if (fLen < 1e-6f) - fLen = 1e-6f; - fx /= fLen; - fy /= fLen; - fz /= fLen; - - float dx = wx - cx; - float dy = wy - cy; - float dz = wz - cz; - return fx * dx + fy * dy + fz * dz; - } - - void RobotArm3DWidget::paintEvent(QPaintEvent*) - { - QPainter painter(this); - painter.setRenderHint(QPainter::Antialiasing, true); - - QLinearGradient bg(0, 0, 0, height()); - bg.setColorAt(0.0, QColor(30, 30, 45)); - bg.setColorAt(1.0, QColor(15, 15, 25)); - painter.fillRect(rect(), bg); - - DrawGrid(painter); - DrawAxes(painter); - DrawShadow(painter); - DrawTrail(painter); - DrawRobot(painter); - DrawInfoOverlay(painter); - } - - void RobotArm3DWidget::DrawGrid(QPainter& painter) - { - QPen pen(QColor(60, 60, 80), 1); - painter.setPen(pen); - - float gridSize = 2.0f; - float step = 0.25f; - - for (float x = -gridSize; x <= gridSize; x += step) - { - auto p1 = Project(x, -gridSize, 0); - auto p2 = Project(x, gridSize, 0); - painter.drawLine(p1, p2); - } - - for (float y = -gridSize; y <= gridSize; y += step) - { - auto p1 = Project(-gridSize, y, 0); - auto p2 = Project(gridSize, y, 0); - painter.drawLine(p1, p2); - } - } - - void RobotArm3DWidget::DrawAxes(QPainter& painter) - { - float len = 0.4f; - auto origin = Project(0, 0, 0); - - painter.setPen(QPen(QColor(220, 60, 60), 2)); - painter.drawLine(origin, Project(len, 0, 0)); - painter.setPen(QPen(QColor(60, 220, 60), 2)); - painter.drawLine(origin, Project(0, len, 0)); - painter.setPen(QPen(QColor(60, 100, 255), 2)); - painter.drawLine(origin, Project(0, 0, len)); - - QFont font("Monospace", 9); - painter.setFont(font); - painter.setPen(QColor(220, 60, 60)); - painter.drawText(Project(len + 0.05f, 0, 0).toPoint(), "X"); - painter.setPen(QColor(60, 220, 60)); - painter.drawText(Project(0, len + 0.05f, 0).toPoint(), "Y"); - painter.setPen(QColor(60, 100, 255)); - painter.drawText(Project(0, 0, len + 0.05f).toPoint(), "Z"); - } - - void RobotArm3DWidget::DrawShadow(QPainter& painter) - { - if (currentState.jointPositions.size() < 2) - return; - - QPen shadowPen(QColor(80, 80, 80, 80), 3); - painter.setPen(shadowPen); - - for (std::size_t i = 0; i + 1 < currentState.jointPositions.size(); ++i) - { - auto& p1 = currentState.jointPositions[i]; - auto& p2 = currentState.jointPositions[i + 1]; - auto sp1 = Project(p1[0], p1[1], 0); - auto sp2 = Project(p2[0], p2[1], 0); - painter.drawLine(sp1, sp2); - } - } - - void RobotArm3DWidget::DrawTrail(QPainter& painter) - { - if (currentState.endEffectorTrail.size() < 2) - return; - - auto trailSize = currentState.endEffectorTrail.size(); - for (std::size_t i = 1; i < trailSize; ++i) - { - float alpha = static_cast(i) / static_cast(trailSize); - int a = static_cast(alpha * 180); - QPen trailPen(QColor(255, 100, 100, a), 1); - painter.setPen(trailPen); - - auto& p1 = currentState.endEffectorTrail[i - 1]; - auto& p2 = currentState.endEffectorTrail[i]; - painter.drawLine(Project(p1[0], p1[1], p1[2]), Project(p2[0], p2[1], p2[2])); - } - } - - void RobotArm3DWidget::DrawRobot(QPainter& painter) - { - if (currentState.jointPositions.size() < 2) - return; - - // Draw links - for (std::size_t i = 0; i + 1 < currentState.jointPositions.size(); ++i) - { - auto& p1 = currentState.jointPositions[i]; - auto& p2 = currentState.jointPositions[i + 1]; - QColor color = QColor::fromRgba(linkColors[i % linkColors.size()]); - - QPen linkPen(color, 6, Qt::SolidLine, Qt::RoundCap); - painter.setPen(linkPen); - painter.drawLine( - Project(p1[0], p1[1], p1[2]), - Project(p2[0], p2[1], p2[2])); - - // Brighter outline - QPen outlinePen(color.lighter(140), 8, Qt::SolidLine, Qt::RoundCap); - outlinePen.setColor(QColor(color.red(), color.green(), color.blue(), 80)); - painter.setPen(outlinePen); - painter.drawLine( - Project(p1[0], p1[1], p1[2]), - Project(p2[0], p2[1], p2[2])); - } - - // Draw joints - for (std::size_t i = 0; i < currentState.jointPositions.size(); ++i) - { - auto& p = currentState.jointPositions[i]; - auto sp = Project(p[0], p[1], p[2]); - - bool isBase = (i == 0); - bool isEndEffector = (i == currentState.jointPositions.size() - 1); - - if (isBase) - { - painter.setPen(QPen(QColor(200, 200, 200), 2)); - painter.setBrush(QColor(80, 80, 80)); - painter.drawEllipse(sp, 10, 10); - - // Base platform - auto bl = Project(-0.1f, -0.1f, 0); - auto br = Project(0.1f, -0.1f, 0); - auto tr = Project(0.1f, 0.1f, 0); - auto tl = Project(-0.1f, 0.1f, 0); - QPainterPath basePath; - basePath.moveTo(bl); - basePath.lineTo(br); - basePath.lineTo(tr); - basePath.lineTo(tl); - basePath.closeSubpath(); - painter.setPen(QPen(QColor(100, 100, 120), 1)); - painter.setBrush(QColor(60, 60, 80, 150)); - painter.drawPath(basePath); - } - else if (isEndEffector) - { - painter.setPen(QPen(QColor(255, 255, 255), 2)); - painter.setBrush(QColor(244, 67, 54)); - painter.drawEllipse(sp, 8, 8); - - // Cross-hair at end-effector - painter.setPen(QPen(QColor(255, 255, 255, 150), 1)); - painter.drawLine(QPointF(sp.x() - 14, sp.y()), QPointF(sp.x() - 10, sp.y())); - painter.drawLine(QPointF(sp.x() + 10, sp.y()), QPointF(sp.x() + 14, sp.y())); - painter.drawLine(QPointF(sp.x(), sp.y() - 14), QPointF(sp.x(), sp.y() - 10)); - painter.drawLine(QPointF(sp.x(), sp.y() + 10), QPointF(sp.x(), sp.y() + 14)); - } - else - { - painter.setPen(QPen(QColor(220, 220, 220), 2)); - painter.setBrush(QColor(50, 50, 60)); - painter.drawEllipse(sp, 7, 7); - } - } - } - - void RobotArm3DWidget::DrawInfoOverlay(QPainter& painter) - { - QFont font("Monospace", 10); - painter.setFont(font); - painter.setPen(QColor(200, 200, 220)); - - int y = 20; - int x = 10; - int lineHeight = 18; - - painter.drawText(x, y, QString("t = %1 s").arg(static_cast(currentState.time), 0, 'f', 2)); - y += lineHeight; - - for (int i = 0; i < currentDof && i < static_cast(currentState.q.size()); ++i) - { - float deg = currentState.q[i] * 180.0f / 3.14159265f; - painter.drawText(x, y, - QString("q%1 = %2 deg | dq = %3 rad/s") - .arg(i + 1) - .arg(static_cast(deg), 7, 'f', 1) - .arg(static_cast(currentState.qDot[i]), 7, 'f', 2)); - y += lineHeight; - } - - if (!currentState.inverseDynamicsTorques.empty()) - { - y += 4; - painter.setPen(QColor(180, 180, 120)); - painter.drawText(x, y, "RNEA Inv. Dynamics:"); - y += lineHeight; - for (int i = 0; i < currentDof && i < static_cast(currentState.inverseDynamicsTorques.size()); ++i) - { - painter.drawText(x, y, - QString(" τ%1 = %2 N·m") - .arg(i + 1) - .arg(static_cast(currentState.inverseDynamicsTorques[i]), 8, 'f', 3)); - y += lineHeight; - } - } - - painter.setPen(QColor(200, 200, 220)); - - if (!currentState.jointPositions.empty()) - { - auto& ee = currentState.jointPositions.back(); - painter.drawText(x, y, - QString("EE: (%1, %2, %3)") - .arg(static_cast(ee[0]), 0, 'f', 3) - .arg(static_cast(ee[1]), 0, 'f', 3) - .arg(static_cast(ee[2]), 0, 'f', 3)); - } - - // Camera controls hint - painter.setPen(QColor(120, 120, 140)); - painter.setFont(QFont("Monospace", 8)); - painter.drawText(10, height() - 10, "LMB: Rotate | Scroll: Zoom"); - } - - void RobotArm3DWidget::mousePressEvent(QMouseEvent* event) - { - if (event->button() == Qt::LeftButton) - lastMousePos = event->pos(); - } - - void RobotArm3DWidget::mouseMoveEvent(QMouseEvent* event) - { - if (event->buttons() & Qt::LeftButton) - { - int dx = event->pos().x() - lastMousePos.x(); - int dy = event->pos().y() - lastMousePos.y(); - - camera.azimuth -= dx * 0.008f; - camera.elevation += dy * 0.008f; - - float maxElev = 1.5f; - float minElev = -0.2f; - camera.elevation = std::clamp(camera.elevation, minElev, maxElev); - - lastMousePos = event->pos(); - update(); - } - } - - void RobotArm3DWidget::wheelEvent(QWheelEvent* event) - { - float delta = event->angleDelta().y() / 120.0f; - camera.distance -= delta * 0.3f; - camera.distance = std::clamp(camera.distance, 1.0f, 15.0f); - update(); - } -} diff --git a/simulator/dynamics/RobotArm/view/RobotArm3DWidget.hpp b/simulator/dynamics/RobotArm/view/RobotArm3DWidget.hpp deleted file mode 100644 index ac31a9ce..00000000 --- a/simulator/dynamics/RobotArm/view/RobotArm3DWidget.hpp +++ /dev/null @@ -1,56 +0,0 @@ -#pragma once - -#include "simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp" -#include -#include -#include -#include -#include - -namespace simulator::dynamics::view -{ - class RobotArm3DWidget - : public QWidget - { - Q_OBJECT - - public: - explicit RobotArm3DWidget(QWidget* parent = nullptr); - - void SetState(const RobotArmState& state, int dof); - - protected: - void paintEvent(QPaintEvent* event) override; - void mousePressEvent(QMouseEvent* event) override; - void mouseMoveEvent(QMouseEvent* event) override; - void wheelEvent(QWheelEvent* event) override; - - private: - struct Camera - { - float azimuth = 0.8f; - float elevation = 0.45f; - float distance = 3.5f; - float lookAtX = 0.0f; - float lookAtY = 0.0f; - float lookAtZ = 0.3f; - }; - - QPointF Project(float wx, float wy, float wz) const; - float ProjectDepth(float wx, float wy, float wz) const; - void DrawGrid(QPainter& painter); - void DrawAxes(QPainter& painter); - void DrawRobot(QPainter& painter); - void DrawShadow(QPainter& painter); - void DrawTrail(QPainter& painter); - void DrawInfoOverlay(QPainter& painter); - void DrawLine3D(QPainter& painter, float x1, float y1, float z1, float x2, float y2, float z2); - - Camera camera; - RobotArmState currentState; - int currentDof = 2; - QPoint lastMousePos; - - static constexpr std::array linkColors = { 0xFF2196F3, 0xFF4CAF50, 0xFFFF9800 }; - }; -} diff --git a/simulator/dynamics/RobotArm/view/RobotArmConfigurationPanel.cpp b/simulator/dynamics/RobotArm/view/RobotArmConfigurationPanel.cpp deleted file mode 100644 index 35d3403e..00000000 --- a/simulator/dynamics/RobotArm/view/RobotArmConfigurationPanel.cpp +++ /dev/null @@ -1,232 +0,0 @@ -#include "simulator/dynamics/RobotArm/view/RobotArmConfigurationPanel.hpp" -#include -#include -#include -#include - -namespace simulator::dynamics::view -{ - RobotArmConfigurationPanel::RobotArmConfigurationPanel(QWidget* parent) - : QWidget(parent) - { - SetupUi(); - } - - void RobotArmConfigurationPanel::SetupUi() - { - auto* mainLayout = new QVBoxLayout(this); - mainLayout->setContentsMargins(8, 8, 8, 8); - mainLayout->setSpacing(6); - - // DOF selection - auto* dofGroup = new QGroupBox("Robot Configuration", this); - auto* dofLayout = new QHBoxLayout(dofGroup); - dofLayout->addWidget(new QLabel("DOF:", this)); - dofCombo = new QComboBox(this); - dofCombo->addItem("2 DOF (Planar)", 2); - dofCombo->addItem("3 DOF (Spatial)", 3); - dofLayout->addWidget(dofCombo); - mainLayout->addWidget(dofGroup); - - // Link parameters - std::array defaultLengths = { 0.5f, 0.4f, 0.3f }; - std::array defaultMasses = { 1.0f, 0.8f, 0.6f }; - - for (int i = 0; i < maxDof; ++i) - { - auto* group = new QGroupBox(QString("Link %1").arg(i + 1), this); - auto* layout = new QVBoxLayout(group); - - auto* lenLayout = new QHBoxLayout(); - lenLayout->addWidget(new QLabel("Length (m):", this)); - auto* lenSpin = new QDoubleSpinBox(this); - lenSpin->setRange(0.05, 2.0); - lenSpin->setSingleStep(0.05); - lenSpin->setValue(static_cast(defaultLengths[i])); - lenSpin->setDecimals(2); - lenLayout->addWidget(lenSpin); - layout->addLayout(lenLayout); - - auto* massLayout = new QHBoxLayout(); - massLayout->addWidget(new QLabel("Mass (kg):", this)); - auto* massSpin = new QDoubleSpinBox(this); - massSpin->setRange(0.1, 20.0); - massSpin->setSingleStep(0.1); - massSpin->setValue(static_cast(defaultMasses[i])); - massSpin->setDecimals(2); - massLayout->addWidget(massSpin); - layout->addLayout(massLayout); - - linkGroups.push_back(group); - lengthSpins.push_back(lenSpin); - massSpins.push_back(massSpin); - mainLayout->addWidget(group); - } - - // Torque sliders - torqueGroup = new QGroupBox("Joint Torques (N·m)", this); - auto* torqueLayout = new QVBoxLayout(torqueGroup); - - for (int i = 0; i < maxDof; ++i) - { - auto* rowWidget = new QWidget(torqueGroup); - auto* rowLayout = new QHBoxLayout(rowWidget); - rowLayout->setContentsMargins(0, 0, 0, 0); - rowLayout->addWidget(new QLabel(QString("τ%1:").arg(i + 1), rowWidget)); - - auto* slider = new QSlider(Qt::Horizontal, rowWidget); - slider->setRange(-200, 200); - slider->setValue(0); - slider->setTickPosition(QSlider::TicksBelow); - slider->setTickInterval(50); - rowLayout->addWidget(slider); - - auto* label = new QLabel("0.0", rowWidget); - label->setFixedWidth(55); - label->setAlignment(Qt::AlignRight | Qt::AlignVCenter); - rowLayout->addWidget(label); - - torqueSliders.push_back(slider); - torqueLabels.push_back(label); - torqueRows.push_back(rowWidget); - torqueLayout->addWidget(rowWidget); - - connect(slider, &QSlider::valueChanged, this, &RobotArmConfigurationPanel::UpdateTorqueLabels); - } - mainLayout->addWidget(torqueGroup); - - // Initial position sliders - positionGroup = new QGroupBox("Initial Position (deg)", this); - auto* posLayout = new QVBoxLayout(positionGroup); - - for (int i = 0; i < maxDof; ++i) - { - auto* rowWidget = new QWidget(positionGroup); - auto* rowLayout = new QHBoxLayout(rowWidget); - rowLayout->setContentsMargins(0, 0, 0, 0); - rowLayout->addWidget(new QLabel(QString("q%1:").arg(i + 1), rowWidget)); - - auto* slider = new QSlider(Qt::Horizontal, rowWidget); - slider->setRange(-180, 180); - slider->setValue(0); - slider->setTickPosition(QSlider::TicksBelow); - slider->setTickInterval(45); - rowLayout->addWidget(slider); - - auto* label = new QLabel("0°", rowWidget); - label->setFixedWidth(45); - label->setAlignment(Qt::AlignRight | Qt::AlignVCenter); - rowLayout->addWidget(label); - - positionSliders.push_back(slider); - positionLabels.push_back(label); - positionRows.push_back(rowWidget); - posLayout->addWidget(rowWidget); - - connect(slider, &QSlider::valueChanged, this, &RobotArmConfigurationPanel::UpdatePositionLabels); - } - mainLayout->addWidget(positionGroup); - - // Damping - auto* dampGroup = new QGroupBox("Simulation", this); - auto* dampLayout = new QHBoxLayout(dampGroup); - dampLayout->addWidget(new QLabel("Damping:", this)); - dampingSpin = new QDoubleSpinBox(this); - dampingSpin->setRange(0.0, 5.0); - dampingSpin->setSingleStep(0.01); - dampingSpin->setValue(0.05); - dampingSpin->setDecimals(3); - dampLayout->addWidget(dampingSpin); - mainLayout->addWidget(dampGroup); - - // Control buttons - auto* controlLayout = new QHBoxLayout(); - startButton = new QPushButton("▶ Start", this); - stopButton = new QPushButton("⬜ Stop", this); - resetButton = new QPushButton("↺ Reset", this); - - startButton->setStyleSheet("QPushButton { background-color: #4CAF50; color: white; padding: 6px; }"); - stopButton->setStyleSheet("QPushButton { background-color: #f44336; color: white; padding: 6px; }"); - resetButton->setStyleSheet("QPushButton { background-color: #2196F3; color: white; padding: 6px; }"); - - controlLayout->addWidget(startButton); - controlLayout->addWidget(stopButton); - controlLayout->addWidget(resetButton); - mainLayout->addLayout(controlLayout); - - mainLayout->addStretch(); - - // Connections - connect(dofCombo, QOverload::of(&QComboBox::currentIndexChanged), this, &RobotArmConfigurationPanel::OnDofChanged); - connect(startButton, &QPushButton::clicked, this, &RobotArmConfigurationPanel::StartRequested); - connect(stopButton, &QPushButton::clicked, this, &RobotArmConfigurationPanel::StopRequested); - connect(resetButton, &QPushButton::clicked, this, &RobotArmConfigurationPanel::ResetRequested); - - OnDofChanged(0); - } - - void RobotArmConfigurationPanel::OnDofChanged(int index) - { - int dof = dofCombo->itemData(index).toInt(); - for (int i = 0; i < maxDof; ++i) - { - bool visible = (i < dof); - linkGroups[i]->setVisible(visible); - torqueRows[i]->setVisible(visible); - positionRows[i]->setVisible(visible); - } - - emit ConfigurationChanged(); - } - - void RobotArmConfigurationPanel::UpdateTorqueLabels() - { - for (int i = 0; i < maxDof; ++i) - { - float value = torqueSliders[i]->value() / 10.0f; - torqueLabels[i]->setText(QString::number(static_cast(value), 'f', 1)); - } - } - - void RobotArmConfigurationPanel::UpdatePositionLabels() - { - for (int i = 0; i < maxDof; ++i) - positionLabels[i]->setText(QString("%1°").arg(positionSliders[i]->value())); - } - - RobotArmConfig RobotArmConfigurationPanel::GetConfiguration() const - { - RobotArmConfig cfg; - cfg.dof = dofCombo->currentData().toInt(); - cfg.damping = static_cast(dampingSpin->value()); - - cfg.linkLengths.resize(cfg.dof); - cfg.linkMasses.resize(cfg.dof); - - for (int i = 0; i < cfg.dof; ++i) - { - cfg.linkLengths[i] = static_cast(lengthSpins[i]->value()); - cfg.linkMasses[i] = static_cast(massSpins[i]->value()); - } - - return cfg; - } - - std::vector RobotArmConfigurationPanel::GetTorques() const - { - int dof = dofCombo->currentData().toInt(); - std::vector t(dof); - for (int i = 0; i < dof; ++i) - t[i] = torqueSliders[i]->value() / 10.0f; - return t; - } - - std::vector RobotArmConfigurationPanel::GetInitialPositions() const - { - int dof = dofCombo->currentData().toInt(); - std::vector pos(dof); - for (int i = 0; i < dof; ++i) - pos[i] = positionSliders[i]->value() * 3.14159265f / 180.0f; - return pos; - } -} diff --git a/simulator/dynamics/RobotArm/view/RobotArmConfigurationPanel.hpp b/simulator/dynamics/RobotArm/view/RobotArmConfigurationPanel.hpp deleted file mode 100644 index 8de15d35..00000000 --- a/simulator/dynamics/RobotArm/view/RobotArmConfigurationPanel.hpp +++ /dev/null @@ -1,63 +0,0 @@ -#pragma once - -#include "simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp" -#include -#include -#include -#include -#include -#include -#include -#include - -namespace simulator::dynamics::view -{ - class RobotArmConfigurationPanel - : public QWidget - { - Q_OBJECT - - public: - explicit RobotArmConfigurationPanel(QWidget* parent = nullptr); - - RobotArmConfig GetConfiguration() const; - std::vector GetTorques() const; - std::vector GetInitialPositions() const; - - signals: - void ConfigurationChanged(); - void StartRequested(); - void StopRequested(); - void ResetRequested(); - - private: - void SetupUi(); - void OnDofChanged(int index); - void UpdateTorqueLabels(); - void UpdatePositionLabels(); - - QComboBox* dofCombo; - - static constexpr int maxDof = 3; - - std::vector linkGroups; - std::vector lengthSpins; - std::vector massSpins; - - std::vector torqueSliders; - std::vector torqueLabels; - std::vector torqueRows; - QGroupBox* torqueGroup; - - std::vector positionSliders; - std::vector positionLabels; - std::vector positionRows; - QGroupBox* positionGroup; - - QDoubleSpinBox* dampingSpin; - - QPushButton* startButton; - QPushButton* stopButton; - QPushButton* resetButton; - }; -} diff --git a/simulator/dynamics/RobotArm/view/RobotArmMainWindow.cpp b/simulator/dynamics/RobotArm/view/RobotArmMainWindow.cpp deleted file mode 100644 index 876f1c93..00000000 --- a/simulator/dynamics/RobotArm/view/RobotArmMainWindow.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include "simulator/dynamics/RobotArm/view/RobotArmMainWindow.hpp" -#include -#include - -namespace simulator::dynamics::view -{ - RobotArmMainWindow::RobotArmMainWindow(QWidget* parent) - : QMainWindow(parent) - { - setWindowTitle("Robot Arm Dynamics Simulator"); - resize(1400, 900); - - auto* splitter = new QSplitter(Qt::Horizontal, this); - - configPanel = new RobotArmConfigurationPanel(splitter); - configPanel->setMaximumWidth(350); - configPanel->setMinimumWidth(280); - - view3D = new RobotArm3DWidget(splitter); - - splitter->addWidget(configPanel); - splitter->addWidget(view3D); - splitter->setStretchFactor(0, 0); - splitter->setStretchFactor(1, 1); - - setCentralWidget(splitter); - - simulationTimer = new QTimer(this); - connect(simulationTimer, &QTimer::timeout, this, &RobotArmMainWindow::OnSimulationStep); - - connect(configPanel, &RobotArmConfigurationPanel::StartRequested, this, &RobotArmMainWindow::OnStartRequested); - connect(configPanel, &RobotArmConfigurationPanel::StopRequested, this, &RobotArmMainWindow::OnStopRequested); - connect(configPanel, &RobotArmConfigurationPanel::ResetRequested, this, &RobotArmMainWindow::OnResetRequested); - connect(configPanel, &RobotArmConfigurationPanel::ConfigurationChanged, this, &RobotArmMainWindow::ApplyConfiguration); - - ApplyConfiguration(); - statusBar()->showMessage("Configure robot parameters and press Start"); - } - - void RobotArmMainWindow::ApplyConfiguration() - { - auto config = configPanel->GetConfiguration(); - simulator.Configure(config); - simulator.SetInitialPositions(configPanel->GetInitialPositions()); - simulationTimer->setInterval(static_cast(std::round(1000.0f * config.dt))); - view3D->SetState(simulator.GetState(), config.dof); - } - - void RobotArmMainWindow::OnStartRequested() - { - if (!running) - { - ApplyConfiguration(); - simulator.SetInitialPositions(configPanel->GetInitialPositions()); - running = true; - simulationTimer->start(); - statusBar()->showMessage("Simulation running..."); - } - } - - void RobotArmMainWindow::OnStopRequested() - { - running = false; - simulationTimer->stop(); - statusBar()->showMessage("Simulation stopped"); - } - - void RobotArmMainWindow::OnResetRequested() - { - running = false; - simulationTimer->stop(); - ApplyConfiguration(); - statusBar()->showMessage("Simulation reset"); - } - - void RobotArmMainWindow::OnSimulationStep() - { - simulator.SetTorques(configPanel->GetTorques()); - simulator.Step(); - - auto config = simulator.GetConfig(); - view3D->SetState(simulator.GetState(), config.dof); - - auto& state = simulator.GetState(); - statusBar()->showMessage( - QString("t=%1s | Running") - .arg(static_cast(state.time), 0, 'f', 2)); - } -} diff --git a/simulator/dynamics/RobotArm/view/RobotArmMainWindow.hpp b/simulator/dynamics/RobotArm/view/RobotArmMainWindow.hpp deleted file mode 100644 index d8c72382..00000000 --- a/simulator/dynamics/RobotArm/view/RobotArmMainWindow.hpp +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include "simulator/dynamics/RobotArm/application/RobotArmSimulator.hpp" -#include "simulator/dynamics/RobotArm/view/RobotArm3DWidget.hpp" -#include "simulator/dynamics/RobotArm/view/RobotArmConfigurationPanel.hpp" -#include -#include -#include - -namespace simulator::dynamics::view -{ - class RobotArmMainWindow - : public QMainWindow - { - Q_OBJECT - - public: - explicit RobotArmMainWindow(QWidget* parent = nullptr); - - private: - void OnStartRequested(); - void OnStopRequested(); - void OnResetRequested(); - void OnSimulationStep(); - void ApplyConfiguration(); - - RobotArmSimulator simulator; - RobotArmConfigurationPanel* configPanel; - RobotArm3DWidget* view3D; - QTimer* simulationTimer; - bool running = false; - }; -}