From c6d6530b40bb7a874840e73a22a37fa9c15d9c65 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 09:43:51 +0000 Subject: [PATCH 1/7] feat(trajectory): add CubicSplineTrajectory (M33) Joint-space C2 trajectory through timed via points on upstream math::CubicSpline (NT61), rest-to-rest by default. Creates the trajectory module and the shared JointTrajectoryState type. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 1 + ROADMAP.md | 5 - doc/trajectory/CubicSplineTrajectory.md | 90 ++++++++++++++++ doc/trajectory/README.md | 9 ++ roadmap/README.md | 2 +- .../CubicSplineTrajectory/explanation.md | 35 ------ .../CubicSplineTrajectory/implementation.md | 88 --------------- .../trajectory/CubicSplineTrajectory/tests.md | 53 ---------- .../implementation.md | 3 +- robotics/CMakeLists.txt | 1 + robotics/trajectory/CMakeLists.txt | 22 ++++ robotics/trajectory/CubicSplineTrajectory.cpp | 6 ++ robotics/trajectory/CubicSplineTrajectory.hpp | 63 +++++++++++ robotics/trajectory/TrajectoryTypes.hpp | 18 ++++ robotics/trajectory/test/CMakeLists.txt | 13 +++ .../test/TestCubicSplineTrajectory.cpp | 100 ++++++++++++++++++ 16 files changed, 325 insertions(+), 184 deletions(-) create mode 100644 doc/trajectory/CubicSplineTrajectory.md create mode 100644 doc/trajectory/README.md delete mode 100644 roadmap/trajectory/CubicSplineTrajectory/explanation.md delete mode 100644 roadmap/trajectory/CubicSplineTrajectory/implementation.md delete mode 100644 roadmap/trajectory/CubicSplineTrajectory/tests.md create mode 100644 robotics/trajectory/CMakeLists.txt create mode 100644 robotics/trajectory/CubicSplineTrajectory.cpp create mode 100644 robotics/trajectory/CubicSplineTrajectory.hpp create mode 100644 robotics/trajectory/TrajectoryTypes.hpp create mode 100644 robotics/trajectory/test/CMakeLists.txt create mode 100644 robotics/trajectory/test/TestCubicSplineTrajectory.cpp diff --git a/README.md b/README.md index 3912d28..4f6f667 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. |----------------------------------------|-----------------------------------------------------------------------------------------------| | [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA) | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 61cf34e..f033531 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -118,7 +118,6 @@ existing `dynamics/` convention. | M1 | Generic joint link (prismatic, limits, armature) | `dynamics` + `kinematics` | — | ★☆☆☆☆ | | M5 | PD + gravity compensation control | `controllers/manipulator` | M29 | ★☆☆☆☆ | | M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | -| M33 | Joint-space cubic spline through via points | `trajectory` (new) | NT61 | ★☆☆☆☆ | | M28 | Composite Rigid Body Algorithm (mass matrix) | `dynamics` | — | ★★☆☆☆ | | M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | | M30 | Chain pose kinematics (full-pose FK, frame chain) | `kinematics` | NT53 | ★★☆☆☆ | @@ -161,10 +160,6 @@ retired here. **M32. Inverse dynamics with an external tool wrench.** RNEA overload taking the environment's wrench on the tool; satisfies `τ(w) − τ(0) = −Jᵀw`. - *Algorithm / paper:* Luh, Walker, Paul (1980). -**M33. Joint-space cubic spline through via points.** Timed via points to a C² joint trajectory, rest-to-rest by default; defines the shared `JointTrajectoryState`. -- *Algorithm / paper:* Biagiotti & Melchiorri (2008), Ch. 4. -- *Reuses / builds on:* upstream `math::CubicSpline` (NT61), which carries the tridiagonal solve. - ### Tier 2 — Easy ★★☆☆☆ **M28. Composite Rigid Body Algorithm.** `O(n²)` joint-space mass matrix, sharing the spatial algebra of ABA. diff --git a/doc/trajectory/CubicSplineTrajectory.md b/doc/trajectory/CubicSplineTrajectory.md new file mode 100644 index 0000000..7ceb995 --- /dev/null +++ b/doc/trajectory/CubicSplineTrajectory.md @@ -0,0 +1,90 @@ +# Cubic Spline Trajectory + +## Overview & Motivation + +Teach-in programs, planners and resampled optimal profiles hand a controller a list of **via points**: joint vectors $q_k$ that should be reached at times $t_k$. Stopping at every via point wastes time and excites the structure; fitting one high-order polynomial through all of them rings between the knots. A **cubic spline** is the standard middle ground. Between consecutive knots each joint follows a cubic polynomial, and the pieces join with continuous position, velocity **and** acceleration ($C^2$). + +Taking time as the abscissa makes the spline a trajectory directly. Its slope is the joint velocity and its curvature is the joint acceleration. Planning is a single linear-time pass, and every servo tick costs one interval search plus a cubic evaluation per joint. Both are bounded and deterministic, which suits embedded controllers. + +## Mathematical Theory + +### Piecewise Cubic + +For $K$ knots $t_0 < t_1 < \dots < t_{K-1}$ with values $q_k \in \mathbb{R}^n$, segment $k$ ($t \in [t_k, t_{k+1}]$, $h_k = t_{k+1} - t_k$, $\tau = t - t_k$) is written in terms of the knot accelerations $M_k = \ddot{q}(t_k)$: + +$$q(t) = q_k + \beta_k \tau + \frac{M_k}{2}\tau^2 + \frac{M_{k+1} - M_k}{6 h_k}\tau^3, \qquad \beta_k = \frac{q_{k+1} - q_k}{h_k} - \frac{h_k}{6}\left(2M_k + M_{k+1}\right)$$ + +This form interpolates the knots and has continuous acceleration by construction. Matching velocities at the interior knots gives one equation per knot. + +### Tridiagonal System + +$$h_{k-1} M_{k-1} + 2(h_{k-1} + h_k) M_k + h_k M_{k+1} = 6\left(\frac{q_{k+1} - q_k}{h_k} - \frac{q_k - q_{k-1}}{h_{k-1}}\right), \quad 1 \le k \le K-2$$ + +Two end conditions close the system: + +- **Clamped**: prescribed end velocities $v_0, v_f$, giving $2h_0 M_0 + h_0 M_1 = 6\left(\frac{q_1 - q_0}{h_0} - v_0\right)$ and the mirror equation at the end. With $v_0 = v_f = 0$ the move is **rest-to-rest**, the default for a trajectory. +- **Natural**: $M_0 = M_{K-1} = 0$. End accelerations vanish, and the end velocities follow from the data. + +The matrix is strictly diagonally dominant, so the Thomas algorithm solves it stably in $O(K)$ without pivoting. The joints share the same matrix and differ only in the right-hand side, so the matrix is factored once and back-substituted per joint. + +### Trajectory State + +At time $t$ the trajectory reports the triple $(q, \dot{q}, \ddot{q})$, all from the same segment: + +$$\dot{q}(t) = \beta_k + M_k \tau + \frac{M_{k+1} - M_k}{2h_k}\tau^2, \qquad \ddot{q}(t) = M_k + \frac{M_{k+1} - M_k}{h_k}\tau$$ + +Requests outside $[t_0, t_{K-1}]$ are clamped to the end states. + +## Complexity Analysis + +| Phase | Time | Space | Notes | +|-------------|-------------------|----------------|----------------------------------------------------------| +| Planning | $O(nK)$ | $O(nK)$ | One tridiagonal factorization, $n$ back-substitutions | +| Sampling | $O(\log K + n)$ | $O(1)$ | Binary search for the segment, then a cubic per joint | +| Storage | — | $K(2 + 2n)$ | Knot times, spans, values and knot accelerations | + +Storage is sized by the compile-time capacity, so the memory footprint is fixed no matter how many via points a plan uses. + +## Step-by-Step Walkthrough + +One joint, knots $t = (0, 1, 2.5, 3, 4.5)$ s, positions $q = (0, 1, -0.5, 0.3, 2)$ rad, clamped zero end velocities. + +1. Spans $h = (1, 1.5, 0.5, 1.5)$; secant slopes $(1, -1, 1.6, 1.133)$. +2. Right-hand side: $6(1 - 0) = 6$, $6(-1 - 1) = -12$, $6(1.6 + 1) = 15.6$, $6(1.133 - 1.6) = -2.8$, $6(0 - 1.133) = -6.8$. +3. Solving the $5 \times 5$ tridiagonal system gives $M = (5.665, -5.330, 5.991, -0.737, -1.898)$ rad/s². +4. At $t = 0$: $\tau = 0$, so $q = 0$, $\dot{q} = \beta_0 = 1 - \tfrac{1}{6}(2 \cdot 5.665 - 5.330) = 0$, which is rest as requested. The start acceleration is $5.665$ rad/s², not zero. +5. For two knots with $v_0 = 0.5$, $v_f = -0.5$ over 2 s, the system reduces to $M = (1, -2)$, the same as the cubic point-to-point polynomial with those boundary velocities. + +## Pitfalls & Edge Cases + +- **Acceleration steps at the ends**: clamped zero velocities start and stop at rest, but the end accelerations are generally non-zero, so the acceleration jumps at start and stop. Natural ends remove the jump but leave the end velocities free. Pick per application, or blend into the spline with a higher-order segment. +- **Overshoot between knots**: the spline minimizes curvature, not excursion. Joint and velocity limits can be violated between knots, so check sampled states or stretch all knot times by $\lambda \ge 1$ (velocities scale by $1/\lambda$, accelerations by $1/\lambda^2$). +- **Knot timing**: badly chosen times (a long hop scheduled in a short interval) produce large accelerations. A good default spaces knot times in proportion to the largest joint displacement per segment, or to the chord length. +- **Nearly coincident knots**: spans close to zero make the system ill-conditioned and the slopes huge. Times must be strictly increasing with a margin above floating-point resolution, and plans that violate this are rejected. +- **Capacity**: plans with fewer than two knots or more than the compile-time capacity are rejected rather than truncated. + +## Variants & Generalizations + +- **Periodic splines**: wrap-around end conditions for cyclic motions (gait cycles, conveyor tracking). +- **Quintic splines**: $C^4$ continuity and prescribed end accelerations, removing the acceleration steps at the ends. +- **Smoothing splines**: trade exact interpolation for smoothness when via points are noisy (teach-in by demonstration). +- **B-splines**: local control and convex-hull bounds, useful when limits must be guaranteed between knots. +- **Time-optimal knot timing**: choose the $t_k$ to minimize duration under velocity and acceleration limits, an outer optimization around the same interpolant. + +## Applications + +- Joint-space execution of teach-in programs and planner waypoints without stopping at every point. +- Resampling time-optimal or optimization-based trajectories into a compact, smooth reference. +- Smooth references for computed-torque and PD controllers, which need continuous acceleration feed-forward. + +## Connections to Other Algorithms + +- The interpolant, the tridiagonal (Thomas) solve and the cubic and quintic point-to-point profiles come from the shared primitives of [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). With two knots the spline reduces to the cubic point-to-point profile. +- [Inverse Kinematics](../kinematics/InverseKinematics.md): converts Cartesian via points into the joint-space knots this trajectory interpolates. +- [Recursive Newton-Euler](../dynamics/RecursiveNewtonEuler.md): turns the sampled $(q, \dot{q}, \ddot{q})$ into feed-forward joint torques. + +## References & Further Reading + +- Biagiotti, L. and Melchiorri, C. (2008). *Trajectory Planning for Automatic Machines and Robots*. Springer. Chapter 4. +- de Boor, C. (2001). *A Practical Guide to Splines*. Revised ed. Springer. +- Siciliano, B. et al. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 4. diff --git a/doc/trajectory/README.md b/doc/trajectory/README.md new file mode 100644 index 0000000..986d4f4 --- /dev/null +++ b/doc/trajectory/README.md @@ -0,0 +1,9 @@ +# Trajectory Generation + +Time-parameterized joint and Cartesian references for manipulators. + +## Algorithms + +| Algorithm | Description | +|-----------------------------------------------------|-----------------------------------------------------------------------------------------------------------------| +| [Cubic Spline Trajectory](CubicSplineTrajectory.md) | $C^2$ joint trajectory through timed via points, rest-to-rest by default — $O(nK)$ plan, $O(\log K + n)$ sample | diff --git a/roadmap/README.md b/roadmap/README.md index 7753a2c..43289b9 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -95,7 +95,7 @@ prerequisites. `GenericJointLink` (M1) · `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CompositeRigidBodyAlgorithm` (M28) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) · `ForwardDynamicsIntegrator` (M34) ### `trajectory` -`CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) · `CubicSplineTrajectory` (M33) +`CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) ### `mobile` `PlanarOdometry` (F1) · `DifferentialDrive` (W1) · `CarLikeKinematics` (W2) · `OmnidirectionalKinematics` (W3) · `SkidSteerKinematics` (W4) · `PurePursuit` (W5) · `StanleyController` (W6) · `KanayamaController` (W7) diff --git a/roadmap/trajectory/CubicSplineTrajectory/explanation.md b/roadmap/trajectory/CubicSplineTrajectory/explanation.md deleted file mode 100644 index 1235e1c..0000000 --- a/roadmap/trajectory/CubicSplineTrajectory/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# Multi-Joint Cubic Spline Trajectory — Overview - -## What it is -A joint-space trajectory through a list of **via points** reached at given times. Between consecutive -knots each joint follows a cubic polynomial; position, velocity **and** acceleration are continuous -everywhere (C²). The ends are either **clamped** (prescribed velocities, zero by default ⇒ rest-to-rest) -or **natural** (zero end acceleration). - -## Why it matters (embedded) -Teach-in programs, planner outputs and resampled time-optimal profiles all arrive as a list of -waypoints. A single cubic spline turns them into one smooth reference without stopping at every point, -and without the ringing of high-order polynomials. Planning is one linear-time pass on fixed bounded -storage; each servo tick is a binary search plus a cubic evaluation — cheap and deterministic. - -## How it works (intuition) -The generic part — fitting a smooth vector-valued curve through knots by solving one tridiagonal system -for the knot curvatures — is the shared cubic-spline interpolant of the numerical toolbox. This item uses -time as the abscissa and the joint vector as the value, so the curve's slope and curvature are the joint -velocity and acceleration, and it defaults the end slopes to zero so a move starts and ends at rest. - -## Key parameters -- **Knot times `t_k`** — strictly increasing; they set the speed between via points (stretching all of - them by `λ` divides velocities by `λ` and accelerations by `λ²`). -- **Knot positions `q_k`** — one joint vector per via point. -- **End condition** — clamped (start/end velocity, default zero) or natural (zero end acceleration). -- **`MaxPoints`** — compile-time capacity of the bounded storage. - -## Reference -L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), Ch. 4 -(cubic splines: continuity conditions, clamped/natural ends, tridiagonal solution). - -## See also -Upstream `CubicSpline` (NT61, the interpolant) and `PolynomialProfile` (NT55, the two-knot special case); -`TimeOptimalPathParameterization` (M27, its output can be resampled into a spline); upstream -`TrapezoidalProfile` / `SCurveProfile` (NT56/NT60, single-segment, limit-driven moves). diff --git a/roadmap/trajectory/CubicSplineTrajectory/implementation.md b/roadmap/trajectory/CubicSplineTrajectory/implementation.md deleted file mode 100644 index 2e7ee96..0000000 --- a/roadmap/trajectory/CubicSplineTrajectory/implementation.md +++ /dev/null @@ -1,88 +0,0 @@ -# Multi-Joint Cubic Spline Trajectory — Implementation Pseudocode - -> Roadmap ref: #M33 (Tier 1) · Target: `robotics/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -Joint-space trajectory through timed via points. The interpolation itself — the C² vector-valued spline, -its tridiagonal solve and the clamped/natural ends — is upstream `math::CubicSpline` (NT61); this item -adds the trajectory semantics: knot times as the abscissa, rest-to-rest by default, joint-space state. - -## Data structures - -Shared joint-space trajectory types — **canonical definition**, created once in -`robotics/trajectory/TrajectoryTypes.hpp` by the first of M33/M27 deployed; every other trajectory spec -includes it and never redefines these names (scalar `TrajectoryState`/`MotionLimits` are upstream, NT55): - -```cpp -template # static_assert(std::is_floating_point_v); multi-joint samplers (M27, M33) -struct JointTrajectoryState: - math::Vector position, velocity, acceleration -``` - -This item: - -```cpp -template # static_assert(std::is_floating_point_v); -class CubicSplineTrajectory: # static_assert(MaxPoints >= 2) - using JointVector = math::Vector - math::CubicSpline spline # upstream NT61; abscissa = time -``` - -`SplineBoundary` / `SplineEnd` (upstream) are used as is: `Clamped` with zero slopes — the -default — is a rest-to-rest move. - -## Interface - -```cpp -bool Plan(const std::array& times, const std::array& positions, - std::size_t count, const math::SplineBoundary& boundary = {}) - # false if count ∉ [2, MaxPoints] or times not strictly increasing -JointTrajectoryState Sample(T t) const # hot path; absolute time, precondition: Plan() == true -T StartTime() const; T Duration() const # t_0, t_{K−1} − t_0 -``` - -## Algorithm (pseudocode) - -```text -function Plan(times, positions, count, boundary): - return spline.Fit(times, positions, count, boundary) # end slopes = end joint velocities - -function Sample(t): # OPTIMIZE_FOR_SPEED - p = spline.Evaluate(t) # clamps t to [t_0, t_{K−1}] - return { p.value, p.firstDerivative, p.secondDerivative } # position, velocity, acceleration - -StartTime() = spline.First(); Duration() = spline.Last() − spline.First() -``` - -## Complexity & memory - -- As upstream: `Plan` `O(Dof·K)`, `Sample` `O(log K + Dof)`; storage `MaxPoints·(2 + 2·Dof)` scalars. No heap. - -## Numerical / embedded notes - -- Plan once (planning time), then `Sample` is deterministic: one binary search plus fixed arithmetic. -- Clamped zero velocities (default) give a rest-to-rest move, but end accelerations are generally non-zero - (an acceleration step at start/stop); Natural ends zero the accelerations but leave the end velocities - free — pick per application. -- The spline does not enforce joint limits and may overshoot between knots; verify with `Sample`, or - stretch all knot times by `λ ≥ 1` (velocities `/λ`, accelerations `/λ²`). -- Good default knot times: proportional to the largest joint displacement per segment (or chord length). -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/trajectory/CubicSplineTrajectory.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class CubicSplineTrajectory;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Uses `robotics/trajectory/TrajectoryTypes.hpp` (create it from the block above if this is the first of - M33/M27) and `numerical/math/CubicSpline.hpp` (NT61). -- Coverage: `robotics/trajectory/CubicSplineTrajectory.cpp` → `template class CubicSplineTrajectory;` -- Test: `robotics/trajectory/test/TestCubicSplineTrajectory.cpp` -- Doc: `doc/trajectory/CubicSplineTrajectory.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. -- CMake: `.hpp` → `target_sources(robotics.trajectory …)`; - `robotics_add_coverage_sources(robotics.trajectory CubicSplineTrajectory.cpp)`; - `TestCubicSplineTrajectory.cpp` → `robotics.trajectory_test`. -- New module (first trajectory item only): `robotics/trajectory/CMakeLists.txt` with - `robotics_add_header_library(robotics.trajectory)`, `TrajectoryTypes.hpp` in `target_sources`, a - `test/` subdir, `add_subdirectory(trajectory)` in `robotics/CMakeLists.txt`, and a `doc/trajectory/` folder. -- Depends on: NT61 (`math::CubicSpline`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/CubicSplineTrajectory/tests.md b/roadmap/trajectory/CubicSplineTrajectory/tests.md deleted file mode 100644 index d98663d..0000000 --- a/roadmap/trajectory/CubicSplineTrajectory/tests.md +++ /dev/null @@ -1,53 +0,0 @@ -# Multi-Joint Cubic Spline Trajectory — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -The interpolation math (knot values, continuity, clamped/natural ends, reference second derivatives) is -tested upstream with `math::CubicSpline` (NT61). These cases cover only the trajectory mapping. - -## Fixture - -```cpp -class TestCubicSplineTrajectory : public ::testing::Test: - using Spline = CubicSplineTrajectory - std::array times{ 0, 1, 2.5, 3, 4.5 } # K = 5 used - std::array, 8> positions{ (0, 0.5, 0), (1, 0.2, 0), (−0.5, 0.2, 0), - (0.3, −0.4, 0), (2, 0, 0) } - Spline spline - void SetUp(): ASSERT_TRUE(spline.Plan(times, positions, 5)) # default: clamped, zero end velocities -# each case below is a TEST_F(TestCubicSplineTrajectory, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -default_plan_is_rest_to_rest: - Assert: Sample(0).position ≈ q_0, Sample(4.5).position ≈ q_4; Sample(0).velocity ≈ 0, Sample(4.5).velocity ≈ 0 - -state_maps_value_and_derivatives_to_position_velocity_acceleration: - Assert: joint 0 knot accelerations Sample(t_k).acceleration_0 ≈ (5.665169, −5.330337, 5.991011, - −0.737079, −1.898127) (the upstream second derivatives); Sample(0.5).velocity is the spline slope - -start_time_and_duration_follow_the_knots: - Arrange: times shifted by +2 s - Assert: StartTime() ≈ 2, Duration() ≈ 4.5, Sample(2).position ≈ q_0 - -two_knots_reduce_to_the_cubic_polynomial_profile: - Arrange: times {0, 2}, q {0, 1}, Clamped startVelocity 0.5, endVelocity −0.5; - motion_profiles::PolynomialProfile{ { .q0 = 0, .qf = 1, .v0 = 0.5, .vf = −0.5 }, 2, Degree::Cubic } - Assert: position, velocity, acceleration equal at t ∈ {0.3, 1.0, 1.7}; knot accelerations (1, −2) - -plan_rejects_invalid_knots: - Assert: Plan(count = 1) == false; Plan(count = 9) == false; times {0, 1, 1, 2} == false -``` - -## Reference vectors - -- Fixture joint 0, clamped zero ⇒ knot accelerations `(5.665169, −5.330337, 5.991011, −0.737079, −1.898127)` - (python3; same values as the upstream spline test). -- Two knots, `v0 = 0.5, vf = −0.5, tf = 2` ⇒ `M = (2a2, 2a2 + 6a3·tf) = (1, −2)`. - -## Edge cases - -- `K = MaxPoints` (8) ⇒ accepted; `Sample` at `t = t_{K−1}` in range. -- `t` outside `[t_0, t_{K−1}]` ⇒ clamped by the spline (end state). diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md index 572a580..c91eac3 100644 --- a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md +++ b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md @@ -7,8 +7,7 @@ is streamed with `Sample`. ## Data structures -`JointTrajectoryState` comes from `robotics/trajectory/TrajectoryTypes.hpp` (canonical -definition: CubicSplineTrajectory spec → Data structures). +`JointTrajectoryState` comes from the shipped `robotics/trajectory/TrajectoryTypes.hpp`. ```cpp template using JointVector = math::Vector diff --git a/robotics/CMakeLists.txt b/robotics/CMakeLists.txt index bbfd9df..f66850e 100644 --- a/robotics/CMakeLists.txt +++ b/robotics/CMakeLists.txt @@ -1,2 +1,3 @@ add_subdirectory(dynamics) add_subdirectory(kinematics) +add_subdirectory(trajectory) diff --git a/robotics/trajectory/CMakeLists.txt b/robotics/trajectory/CMakeLists.txt new file mode 100644 index 0000000..a025661 --- /dev/null +++ b/robotics/trajectory/CMakeLists.txt @@ -0,0 +1,22 @@ +robotics_add_header_library(robotics.trajectory) + +target_include_directories(robotics.trajectory ${ROBOTICS_VISIBILITY} + "$" + "$" +) + +target_link_libraries(robotics.trajectory ${ROBOTICS_VISIBILITY} + infra.util + numerical.math +) + +target_sources(robotics.trajectory PRIVATE + CubicSplineTrajectory.hpp + TrajectoryTypes.hpp +) + +robotics_add_coverage_sources(robotics.trajectory + CubicSplineTrajectory.cpp +) + +add_subdirectory(test) diff --git a/robotics/trajectory/CubicSplineTrajectory.cpp b/robotics/trajectory/CubicSplineTrajectory.cpp new file mode 100644 index 0000000..6c939ff --- /dev/null +++ b/robotics/trajectory/CubicSplineTrajectory.cpp @@ -0,0 +1,6 @@ +#include "robotics/trajectory/CubicSplineTrajectory.hpp" + +namespace trajectory +{ + template class CubicSplineTrajectory; +} diff --git a/robotics/trajectory/CubicSplineTrajectory.hpp b/robotics/trajectory/CubicSplineTrajectory.hpp new file mode 100644 index 0000000..1e88670 --- /dev/null +++ b/robotics/trajectory/CubicSplineTrajectory.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/CubicSpline.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/trajectory/TrajectoryTypes.hpp" +#include +#include +#include + +namespace trajectory +{ + template + class CubicSplineTrajectory + { + static_assert(std::is_floating_point_v, "CubicSplineTrajectory supports floating-point types"); + static_assert(MaxPoints >= 2, "CubicSplineTrajectory requires MaxPoints >= 2"); + + public: + using JointVector = math::Vector; + using Times = std::array; + using Positions = std::array; + using Boundary = math::SplineBoundary; + using State = JointTrajectoryState; + + bool Plan(const Times& times, const Positions& positions, std::size_t count, const Boundary& boundary = {}); + OPTIMIZE_FOR_SPEED State Sample(T t) const; + T StartTime() const; + T Duration() const; + + private: + math::CubicSpline spline; + }; + + template + bool CubicSplineTrajectory::Plan(const Times& times, const Positions& positions, std::size_t count, const Boundary& boundary) + { + return spline.Fit(times, positions, count, boundary); + } + + template + OPTIMIZE_FOR_SPEED typename CubicSplineTrajectory::State CubicSplineTrajectory::Sample(T t) const + { + const auto point{ spline.Evaluate(t) }; + return State{ point.value, point.firstDerivative, point.secondDerivative }; + } + + template + T CubicSplineTrajectory::StartTime() const + { + return spline.First(); + } + + template + T CubicSplineTrajectory::Duration() const + { + return spline.Last() - spline.First(); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class CubicSplineTrajectory; +#endif +} diff --git a/robotics/trajectory/TrajectoryTypes.hpp b/robotics/trajectory/TrajectoryTypes.hpp new file mode 100644 index 0000000..faa83fc --- /dev/null +++ b/robotics/trajectory/TrajectoryTypes.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include +#include + +namespace trajectory +{ + template + struct JointTrajectoryState + { + static_assert(std::is_floating_point_v, "JointTrajectoryState supports floating-point types"); + + math::Vector position{}; + math::Vector velocity{}; + math::Vector acceleration{}; + }; +} diff --git a/robotics/trajectory/test/CMakeLists.txt b/robotics/trajectory/test/CMakeLists.txt new file mode 100644 index 0000000..83c2395 --- /dev/null +++ b/robotics/trajectory/test/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(robotics.trajectory_test) +emil_build_for(robotics.trajectory_test BOOL ROBOTICS_TOOLBOX_BUILD_TESTS) +emil_add_test(robotics.trajectory_test) + +target_link_libraries(robotics.trajectory_test PUBLIC + gmock_main + numerical.motion_profiles + robotics.trajectory +) + +target_sources(robotics.trajectory_test PRIVATE + TestCubicSplineTrajectory.cpp +) diff --git a/robotics/trajectory/test/TestCubicSplineTrajectory.cpp b/robotics/trajectory/test/TestCubicSplineTrajectory.cpp new file mode 100644 index 0000000..9f9f2e7 --- /dev/null +++ b/robotics/trajectory/test/TestCubicSplineTrajectory.cpp @@ -0,0 +1,100 @@ +#include "numerical/math/CubicSpline.hpp" +#include "numerical/math/Tolerance.hpp" +#include "numerical/motion_profiles/PolynomialProfile.hpp" +#include "robotics/trajectory/CubicSplineTrajectory.hpp" +#include + +namespace +{ + using Vector3 = math::Vector; + using Spline = trajectory::CubicSplineTrajectory; + + class TestCubicSplineTrajectory + : public ::testing::Test + { + protected: + void SetUp() override + { + ASSERT_TRUE(spline.Plan(times, positions, 5)); + } + + void ExpectNear(const Vector3& actual, const Vector3& expected) const + { + for (std::size_t j = 0; j < 3; ++j) + EXPECT_NEAR(actual[j], expected[j], math::Tolerance()); + } + + Spline::Times times{ 0.0f, 1.0f, 2.5f, 3.0f, 4.5f }; + Spline::Positions positions{ Vector3{ 0.0f, 0.5f, 0.0f }, Vector3{ 1.0f, 0.2f, 0.0f }, Vector3{ -0.5f, 0.2f, 0.0f }, + Vector3{ 0.3f, -0.4f, 0.0f }, Vector3{ 2.0f, 0.0f, 0.0f } }; + Spline spline; + }; +} + +TEST_F(TestCubicSplineTrajectory, default_plan_is_rest_to_rest) +{ + const auto start{ spline.Sample(0.0f) }; + const auto end{ spline.Sample(4.5f) }; + + ExpectNear(start.position, positions[0]); + ExpectNear(end.position, positions[4]); + ExpectNear(start.velocity, Vector3{}); + ExpectNear(end.velocity, Vector3{}); +} + +TEST_F(TestCubicSplineTrajectory, state_maps_value_and_derivatives_to_position_velocity_acceleration) +{ + const std::array knotAccelerations{ 5.665169f, -5.330337f, 5.991011f, -0.737079f, -1.898127f }; + math::CubicSpline reference; + ASSERT_TRUE(reference.Fit(times, positions, 5)); + + for (std::size_t k = 0; k < knotAccelerations.size(); ++k) + EXPECT_NEAR(spline.Sample(times[k]).acceleration[0], knotAccelerations[k], math::Tolerance()); + + const auto state{ spline.Sample(0.5f) }; + const auto point{ reference.Evaluate(0.5f) }; + ExpectNear(state.position, point.value); + ExpectNear(state.velocity, point.firstDerivative); + ExpectNear(state.acceleration, point.secondDerivative); +} + +TEST_F(TestCubicSplineTrajectory, start_time_and_duration_follow_the_knots) +{ + Spline::Times shifted{ 2.0f, 3.0f, 4.5f, 5.0f, 6.5f }; + + ASSERT_TRUE(spline.Plan(shifted, positions, 5)); + + EXPECT_NEAR(spline.StartTime(), 2.0f, math::Tolerance()); + EXPECT_NEAR(spline.Duration(), 4.5f, math::Tolerance()); + ExpectNear(spline.Sample(2.0f).position, positions[0]); +} + +TEST_F(TestCubicSplineTrajectory, two_knots_reduce_to_the_cubic_polynomial_profile) +{ + Spline::Times twoTimes{ 0.0f, 2.0f }; + Spline::Positions twoPositions{ Vector3{}, Vector3{ 1.0f, 0.0f, 0.0f } }; + Spline::Boundary boundary{ math::SplineEnd::Clamped, Vector3{ 0.5f, 0.0f, 0.0f }, Vector3{ -0.5f, 0.0f, 0.0f } }; + motion_profiles::PolynomialProfile profile{ { 0.0f, 1.0f, 0.5f, -0.5f }, 2.0f, motion_profiles::Degree::Cubic }; + + ASSERT_TRUE(spline.Plan(twoTimes, twoPositions, 2, boundary)); + + for (const float t : { 0.3f, 1.0f, 1.7f }) + { + const auto state{ spline.Sample(t) }; + const auto expected{ profile.Sample(t) }; + EXPECT_NEAR(state.position[0], expected.position, math::Tolerance()); + EXPECT_NEAR(state.velocity[0], expected.velocity, math::Tolerance()); + EXPECT_NEAR(state.acceleration[0], expected.acceleration, math::Tolerance()); + } + EXPECT_NEAR(spline.Sample(0.0f).acceleration[0], 1.0f, math::Tolerance()); + EXPECT_NEAR(spline.Sample(2.0f).acceleration[0], -2.0f, math::Tolerance()); +} + +TEST_F(TestCubicSplineTrajectory, plan_rejects_invalid_knots) +{ + Spline::Times repeated{ 0.0f, 1.0f, 1.0f, 2.0f }; + + EXPECT_FALSE(spline.Plan(times, positions, 1)); + EXPECT_FALSE(spline.Plan(times, positions, 9)); + EXPECT_FALSE(spline.Plan(repeated, positions, 4)); +} From e385f732899b8f27bd39a79a483347918c78980e Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 09:47:25 +0000 Subject: [PATCH 2/7] feat(dynamics): add CompositeRigidBodyAlgorithm (M28) O(n^2) joint-space mass matrix from composite inertias, exactly symmetric. ABA's spatial-inertia and wrench transforms move into the shared SpatialInertia.hpp so ABA and CRBA use one implementation. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 10 +- ROADMAP.md | 7 +- doc/dynamics/CompositeRigidBodyAlgorithm.md | 113 ++++++++++++++ doc/dynamics/README.md | 13 +- roadmap/README.md | 2 +- .../explanation.md | 31 ---- .../implementation.md | 72 --------- .../CompositeRigidBodyAlgorithm/tests.md | 41 ----- .../dynamics/ArticulatedBodyAlgorithm.hpp | 82 +--------- robotics/dynamics/CMakeLists.txt | 3 + .../dynamics/CompositeRigidBodyAlgorithm.cpp | 8 + .../dynamics/CompositeRigidBodyAlgorithm.hpp | 90 +++++++++++ robotics/dynamics/SpatialInertia.hpp | 77 ++++++++++ robotics/dynamics/test/CMakeLists.txt | 1 + .../test/TestCompositeRigidBodyAlgorithm.cpp | 143 ++++++++++++++++++ 15 files changed, 455 insertions(+), 238 deletions(-) create mode 100644 doc/dynamics/CompositeRigidBodyAlgorithm.md delete mode 100644 roadmap/dynamics/CompositeRigidBodyAlgorithm/explanation.md delete mode 100644 roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md delete mode 100644 roadmap/dynamics/CompositeRigidBodyAlgorithm/tests.md create mode 100644 robotics/dynamics/CompositeRigidBodyAlgorithm.cpp create mode 100644 robotics/dynamics/CompositeRigidBodyAlgorithm.hpp create mode 100644 robotics/dynamics/SpatialInertia.hpp create mode 100644 robotics/dynamics/test/TestCompositeRigidBodyAlgorithm.cpp diff --git a/README.md b/README.md index 4f6f667..4321bc2 100644 --- a/README.md +++ b/README.md @@ -35,11 +35,11 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|----------------------------------------|-----------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA) | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory | +| Category | Description | +|----------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | +| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Composite Rigid Body Algorithm (CRBA) | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index f033531..c6fb0ba 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -91,7 +91,8 @@ docs; the current pin (`63c9b91`) includes them. The library already has the core of manipulator **modelling**: forward/inverse dynamics ([RNEA](robotics/dynamics/RecursiveNewtonEuler.hpp), [ABA](robotics/dynamics/ArticulatedBodyAlgorithm.hpp), -[Euler-Lagrange](robotics/dynamics/EulerLagrangeSolver.hpp)), position forward kinematics with base and +[Euler-Lagrange](robotics/dynamics/EulerLagrangeSolver.hpp)), the joint-space mass matrix +([CRBA](robotics/dynamics/CompositeRigidBodyAlgorithm.hpp)), position forward kinematics with base and tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). What is missing is the **full-pose kinematics, model plumbing, planning and control layer** that turns those models into a usable manipulator stack. Two current limitations gate many items below: @@ -118,7 +119,6 @@ existing `dynamics/` convention. | M1 | Generic joint link (prismatic, limits, armature) | `dynamics` + `kinematics` | — | ★☆☆☆☆ | | M5 | PD + gravity compensation control | `controllers/manipulator` | M29 | ★☆☆☆☆ | | M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | -| M28 | Composite Rigid Body Algorithm (mass matrix) | `dynamics` | — | ★★☆☆☆ | | M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | | M30 | Chain pose kinematics (full-pose FK, frame chain) | `kinematics` | NT53 | ★★☆☆☆ | | M7 | Denavit-Hartenberg parameters (standard + modified) | `kinematics` | NT53, M30 | ★★☆☆☆ | @@ -162,9 +162,6 @@ retired here. ### Tier 2 — Easy ★★☆☆☆ -**M28. Composite Rigid Body Algorithm.** `O(n²)` joint-space mass matrix, sharing the spatial algebra of ABA. -- *Algorithm / paper:* Walker & Orin (1982); Featherstone (2008), Ch. 6. - **M29. Chain dynamics model.** Adapter implementing `EulerLagrangeDynamics` (M via CRBA, `Cq̇` and `g` via RNEA) and a one-call `InverseDynamicsModel` for the controllers. - *Reuses / builds on:* RNEA, M28; optional friction model from NT57. diff --git a/doc/dynamics/CompositeRigidBodyAlgorithm.md b/doc/dynamics/CompositeRigidBodyAlgorithm.md new file mode 100644 index 0000000..a407380 --- /dev/null +++ b/doc/dynamics/CompositeRigidBodyAlgorithm.md @@ -0,0 +1,113 @@ +# Composite Rigid Body Algorithm (CRBA) + +## Overview & Motivation + +The Composite Rigid Body Algorithm builds the **joint-space mass matrix** $M(q)$ of a serial chain, the symmetric positive-definite matrix in + +$$M(q)\,\ddot{q} + C(q, \dot{q})\,\dot{q} + g(q) = \tau$$ + +that says how joint accelerations turn into joint torques. The [Recursive Newton-Euler Algorithm](RecursiveNewtonEuler.md) and the [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) never form $M$, but several controllers and estimators need the matrix itself: + +- operational-space control reflects it into task space; +- momentum-based collision observers integrate $M(q)\dot{q}$; +- forward-dynamics schemes factor it. + +$M$ can also be built column by column with inverse dynamics, at $n$ calls of $O(n)$ each. That is also $O(n^2)$, but with a much larger constant. CRBA builds it directly, in $O(n^2)$ with a small constant (Walker and Orin, 1982). + +## Mathematical Theory + +### Spatial Inertia + +Each link $i$ has a mass $m_i$, centre of mass $c_i$ and rotational inertia $\bar{I}_i$ about the centre of mass, all in its own frame. About the joint origin, acting on the spatial velocity $(\omega; v)$, its rigid-body inertia is the $6 \times 6$ block matrix + +$$I_i = \begin{bmatrix} \bar{I}_i + m_i [c_i]_\times^T [c_i]_\times & m_i [c_i]_\times \\ m_i [c_i]_\times^T & m_i \mathbf{1}_3 \end{bmatrix}$$ + +so that $I_i (\omega; v) = (n; f)$ is the momentum (angular about the origin, then linear). The upper-left block is the parallel-axis theorem. + +### Composite Inertias + +The **composite body** $i$ is link $i$ together with every link outboard of it, frozen in its current configuration. Its inertia is accumulated from tip to base: + +$$I^c_{n-1} = I_{n-1}, \qquad I^c_{i-1} = I_{i-1} + X^*_i\, I^c_i\, X_i$$ + +where $X^*_i I X_i$ re-expresses an inertia in the parent frame. With $R_i = R(\hat{z}_i, q_i)$ and the joint offset $r_i$, the blocks transform as + +$$\begin{aligned} +A' &= R A R^T, \quad B' = R B R^T, \quad L' = R L R^T \\ +I_{\text{parent}} &= \begin{bmatrix} A' - B'[r]_\times + [r]_\times B'^T - [r]_\times L' [r]_\times & B' + [r]_\times L' \\ \cdot & L' \end{bmatrix} +\end{aligned}$$ + +### Mass-Matrix Columns + +A unit acceleration of joint $i$ with all other joints locked accelerates only composite body $i$, rigidly, about axis $\hat{z}_i$. From rest, the wrench it needs is + +$$(n_i; f_i) = I^c_i\,(\hat{z}_i; 0)$$ + +The diagonal entry is the projection onto the joint's own axis, $M_{ii} = \hat{z}_i^T n_i$. The same wrench, carried towards the base through each joint, + +$$f \leftarrow R_j f, \qquad n \leftarrow R_j n + r_j \times f$$ + +projected on each ancestor axis gives $M_{ji} = M_{ij} = \hat{z}_j^T n$ for $j < i$. Each column costs one projection per ancestor, which gives the triangular operation count. + +### Properties + +- $M$ is symmetric, and positive definite whenever every composite body has non-zero inertia about its joint axis. +- $M$ does not depend on the first joint angle when the base is fixed and gravity is ignored: rotating the whole chain about $\hat{z}_0$ is a symmetry of the kinetic energy. +- $\tfrac12 \dot{q}^T M \dot{q}$ equals the sum of the links' kinetic energies $\tfrac12(m_i \|v_{c,i}\|^2 + \omega_i^T \bar{I}_i \omega_i)$. + +## Complexity Analysis + +| Phase | Time | Space | Notes | +|--------------------|-----------|----------|-------------------------------------------------------------------| +| Rotations | $O(n)$ | $O(n)$ | One axis–angle rotation per joint | +| Composite inertias | $O(n)$ | $O(n)$ | One inertia transform (a few $3 \times 3$ products) per joint | +| Columns | $O(n^2)$ | $O(n^2)$ | $n(n-1)/2$ wrench transforms, $O(1)$ each, plus the $n \times n$ result | + +For short chains ($n \le 6$) the constant matters more than the order: CRBA's per-entry work is one $3 \times 3$ rotation and a cross product, far below a full inverse-dynamics sweep per column. + +## Step-by-Step Walkthrough + +Two uniform rods rotating about parallel $\hat{z}$ axes: lengths $l_1 = l_2 = 1$ m, masses $m_1 = m_2 = 1$ kg, $q_2 = -0.5$ rad. + +1. Each rod has $\bar{I} = \mathrm{diag}(0, \tfrac{1}{12}, \tfrac{1}{12})$ and $c = (\tfrac12, 0, 0)$. Its inertia about the joint has $z$-entry $\tfrac{1}{12} + \tfrac14 = \tfrac13$. +2. Composite body 2 is rod 2 alone: $M_{22} = \tfrac13$. +3. Composite body 1 adds rod 2, expressed in frame 1 and moved by $r_2 = (1, 0, 0)$. Its $z$-inertia about joint 1 is $\tfrac13 + \tfrac13 + 1 + \cos(0.5)$, so $M_{11} = \tfrac53 + \cos 0.5 = 2.544$. +4. Column 2 carried to joint 1: the wrench $n = \tfrac13\hat{z}$, $f = \tfrac12(-\sin q_2, \cos q_2, 0)$ in frame 1 gains the moment $r_2 \times f$, whose $z$-part is $\tfrac12\cos q_2$. So $M_{12} = \tfrac13 + \tfrac12\cos 0.5 = 0.772$. + +These are the textbook closed-form entries of the planar two-link arm. + +## Pitfalls & Edge Cases + +- **Inertia frame**: the link tensor must be about the centre of mass, in the link frame. Supplying it about the joint origin double-counts the parallel-axis term. +- **Exact symmetry**: computing both triangles independently leaves round-off asymmetry that can upset a Cholesky factorization. Write each off-diagonal value once into both positions. +- **Singular mass matrix**: a composite body with zero inertia about its axis (a point mass on the axis, or a massless distal chain) makes $M$ singular. Forward dynamics through $M^{-1}$ then fails, so add reflected rotor inertia (armature) on the diagonal for real actuators. +- **Conditioning**: light distal links next to heavy proximal ones give condition numbers of $10^3$–$10^5$. Solve $M\ddot{q} = b$ with a Cholesky factorization, never an explicit inverse. +- **Non-unit axes**: the rotation formula assumes unit joint axes, and the projections scale with the axis norm. + +## Variants & Generalizations + +- **Armature and rotor inertia**: add $k_r^2 I_{\text{rotor}}$ to each diagonal entry. +- **Prismatic joints**: the unit motion is $(0; \hat{z}_i)$, so the column wrench becomes $I^c_i (0; \hat{z}_i)$ and the projection uses the force part. +- **Floating base**: the base becomes a 6-DOF joint and $M$ gains a $6 \times 6$ block and coupling rows (Featherstone, Ch. 9). +- **Branched trees**: the ancestor loop follows the parent array instead of $j - 1$. Sparsity can then be exploited in the factorization. +- **Column-by-column RNEA**: $M e_j = \mathrm{ID}(q, 0, e_j)$ with gravity off. Simpler to derive and useful as a test oracle, but $n$ times the cost of one inverse-dynamics pass. + +## Applications + +- Operational-space control and task-space inertia shaping, $\Lambda = (J M^{-1} J^T)^{-1}$. +- Generalized-momentum observers for collision detection, which need $M(q)\dot{q}$. +- Forward dynamics by factorization for simulation and model-in-the-loop tests of short chains. +- Inertia-aware trajectory scaling and kinetic-energy limits for collaborative robots. + +## Connections to Other Algorithms + +- [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md): shares the spatial inertia and its parent-frame transform. ABA "downdates" the inertia at each joint, while CRBA keeps the full composite. +- [Recursive Newton-Euler](RecursiveNewtonEuler.md): with zero velocity and gravity, inverse dynamics of a unit acceleration is one column of $M$, the standard cross-check. +- [Euler-Lagrange](EulerLagrange.md): $M(q)$ is the inertia term of the Lagrangian equations of motion. +- The Cholesky factorization used to solve with $M$ comes from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Walker, M.W. and Orin, D.E. (1982). "Efficient Dynamic Computer Simulation of Robotic Mechanisms." *ASME J. Dynamic Systems, Measurement, and Control*, 104(3), 205–211. +- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. Chapter 6. +- Siciliano, B. et al. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 7. diff --git a/doc/dynamics/README.md b/doc/dynamics/README.md index b30e802..d0139cd 100644 --- a/doc/dynamics/README.md +++ b/doc/dynamics/README.md @@ -4,9 +4,10 @@ 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 | +| Algorithm | Description | +|------------------------------------------------------------------|-----------------------------------------------------------------------------------------------| +| [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) | $O(n)$ forward dynamics for serial chains — computes joint accelerations from applied torques | +| [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | +| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | +| [Newton-Euler](NewtonEuler.md) | Single rigid-body forward and inverse dynamics using Newton's and Euler's equations | +| [Recursive Newton-Euler](RecursiveNewtonEuler.md) | $O(n)$ inverse dynamics for serial kinematic chains via recursive velocity/force propagation | diff --git a/roadmap/README.md b/roadmap/README.md index 43289b9..4664593 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -92,7 +92,7 @@ prerequisites. `DenavitHartenberg` (M7) · `GeometricJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) · `ChainPoseKinematics` (M30) ### `dynamics` -`GenericJointLink` (M1) · `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `CompositeRigidBodyAlgorithm` (M28) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) · `ForwardDynamicsIntegrator` (M34) +`GenericJointLink` (M1) · `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) · `ForwardDynamicsIntegrator` (M34) ### `trajectory` `CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) diff --git a/roadmap/dynamics/CompositeRigidBodyAlgorithm/explanation.md b/roadmap/dynamics/CompositeRigidBodyAlgorithm/explanation.md deleted file mode 100644 index 61cd65b..0000000 --- a/roadmap/dynamics/CompositeRigidBodyAlgorithm/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# Composite Rigid Body Algorithm — Overview - -## What it is -An `O(n²)` algorithm that builds the joint-space mass (inertia) matrix `M(q)` of a serial arm — the -matrix in `M(q)q̈ + C(q,q̇)q̇ + g(q) = τ` that says how joint accelerations turn into joint torques. - -## Why it matters (embedded) -Several controllers need `M(q)` itself rather than just torques: operational-space control reflects -it into task space, momentum observers integrate `M(q)q̇`, and some forward-dynamics schemes factor -it. Building it column by column with inverse dynamics costs `O(n²)` too but with a larger constant; -CRBA does it directly and cheaply. - -## How it works (intuition) -Treat everything outboard of a joint as one rigid "composite" body. Accelerating that joint by one -unit with all other joints held still requires a wrench equal to the composite inertia times the -joint's motion. Projecting that wrench onto the joint's own axis gives the diagonal entry; carrying -it inward and projecting on each ancestor axis gives the off-diagonal entries of the same column. - -## Key parameters -- **Link inertial parameters** — mass, center of mass, inertia tensor. -- **Joint axes and offsets** — the chain geometry. - -## Reference -R. Featherstone, *Rigid Body Dynamics Algorithms* (2008), Ch. 6; M. W. Walker, D. E. Orin, -"Efficient Dynamic Computer Simulation of Robotic Mechanisms," *ASME J. Dyn. Sys. Meas. Control*, -104(3), 1982. - -## See also -`ArticulatedBodyAlgorithm` (shares the spatial algebra), `RecursiveNewtonEuler` (column-by-column -alternative and cross-check), `ChainDynamicsModel` (M29), `OperationalSpaceControl` (M18), -`MomentumObserver` (M16). diff --git a/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md b/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md deleted file mode 100644 index 1e154ab..0000000 --- a/roadmap/dynamics/CompositeRigidBodyAlgorithm/implementation.md +++ /dev/null @@ -1,72 +0,0 @@ -# Composite Rigid Body Algorithm (CRBA) — Implementation Pseudocode - -> Roadmap ref: #M28 (Tier 2) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -Computes the joint-space mass matrix `M(q)` of a serial chain described by the shipped link model. -`M(q)` is needed by operational-space control (M18), the momentum observer (M16), the chain dynamics -model (M29) and any forward-dynamics path that solves `M q̈ = τ − h`. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class CompositeRigidBodyAlgorithm: # stateless - using MassMatrix = math::SquareMatrix - # reuses the spatial-inertia block form {rot, cross, lin} of ArticulatedBodyAlgorithm: - # I = [[rot, cross], [crossᵀ, lin]] about the joint origin, link frame, acting on (ω; v) -``` - -## Interface - -```text -MassMatrix Compute(const LinkArray& links, const JointVector& q) const # hot path -``` - -## Algorithm (pseudocode) - -```text -function Compute(links, q): # OPTIMIZE_FOR_SPEED - for i: R[i] = RotationAboutAxis(links[i].jointAxis, q[i]) - for i: Ic[i] = RigidBodyInertia(links[i]) # rot = I_c + m·Skew(c)ᵀSkew(c), cross = m·Skew(c), lin = m·I - # 1. composite inertias, tip → base (same transform as ABA's TransformInertiaToParent) - for i = N−1 .. 1: - Ic[i−1] += TransformInertiaToParent(Ic[i], R[i], links[i].parentToJoint) - # 2. one column per joint: the composite body's wrench for a unit joint acceleration, - # carried towards the base and projected on every ancestor axis - for i = N−1 .. 0: - z = links[i].jointAxis - n = Ic[i].rot · z; f = Ic[i].crossᵀ · z # wrench (n; f) about joint i, frame i - M[i][i] = z · n - j = i - while j > 0: - f = R[j] · f; n = R[j] · n + CrossProduct(links[j].parentToJoint, f) - j = j − 1 - M[i][j] = M[j][i] = links[j].jointAxis · n - return M -``` - -## Complexity & memory - -- `O(N²)`: `O(N)` composite-inertia pass plus `N(N−1)/2` wrench transforms (each `O(1)`). -- Memory: `N` spatial inertias (three 3×3 blocks each) + the `N×N` result; stack only; no recursion. - -## Numerical / embedded notes - -- Factor the spatial helpers now private to `ArticulatedBodyAlgorithm` (rigid-body inertia, inertia - and wrench transforms) into a shared internal header so ABA and CRBA cannot drift apart. -- `M` is symmetric positive definite for valid links; fill both triangles from one computation so the - result is exactly symmetric in `float`. -- Joint armature / reflected rotor inertia (M1 fields) is added to the diagonal: `M[i][i] += armature[i]`. -- To solve `M q̈ = b`, prefer a Cholesky factorization (numerical-toolbox `CholeskyDecomposition`). -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/dynamics/CompositeRigidBodyAlgorithm.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class CompositeRigidBodyAlgorithm;` / `` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/dynamics/CompositeRigidBodyAlgorithm.cpp` → the same instantiations. -- Test: `robotics/dynamics/test/TestCompositeRigidBodyAlgorithm.cpp` -- Doc: `doc/dynamics/CompositeRigidBodyAlgorithm.md` (per `doc/TEMPLATE.md`) -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/CompositeRigidBodyAlgorithm/tests.md b/roadmap/dynamics/CompositeRigidBodyAlgorithm/tests.md deleted file mode 100644 index 2c7bd5b..0000000 --- a/roadmap/dynamics/CompositeRigidBodyAlgorithm/tests.md +++ /dev/null @@ -1,41 +0,0 @@ -# Composite Rigid Body Algorithm — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestCompositeRigidBodyAlgorithm : public ::testing::Test: - CompositeRigidBodyAlgorithm crba2 - CompositeRigidBodyAlgorithm crba3 - # uniform rods: inertia diag(0, ml²/12, ml²/12), CoM at (l/2, 0, 0) -# each case below is a TEST_F(TestCompositeRigidBodyAlgorithm, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -single_rod_about_end_is_ml2_over_3: - Assert: M = m·l²/3 for a z-axis rod -two_link_planar_matches_closed_form: - Arrange: z-axis rods m1, l1, m2, l2; q2 = −0.5 - Assert: M11 = m1l1²/3 + m2(l1² + l2²/3 + l1l2 cos q2), M12 = m2(l2²/3 + l1l2 cos q2 / 2), M22 = m2l2²/3 -matrix_is_independent_of_first_joint_angle: - Assert: M(q1 = 0.3, q2) ≈ M(q1 = −1.2, q2) for the planar arm -columns_match_recursive_newton_euler: - Arrange: 3-link chain with skewed axes, full inertia tensors, off-axis CoM - Assert: M·e_j ≈ RNEA(q, 0, e_j, gravity = 0) for every unit vector e_j -matrix_is_symmetric_positive_definite: - Assert: M == Mᵀ and all leading principal minors > 0 on the 3-link chain -kinetic_energy_matches_link_sum: - Assert: ½ q̇ᵀ M q̇ ≈ Σ ½(m‖v_c‖² + ωᵀ I_c ω) using link velocities from forward propagation -``` - -## Reference vectors - -- Unit rods, `q2 = −0.5`: `M11 = 5/3 + cos(0.5)`, `M12 = 1/3 + cos(0.5)/2`, `M22 = 1/3`. - -## Edge cases - -- One link ⇒ `1×1` matrix, no ancestor loop. -- Point-mass link (zero `I_c`) ⇒ still SPD while the mass is off the joint axis. diff --git a/robotics/dynamics/ArticulatedBodyAlgorithm.hpp b/robotics/dynamics/ArticulatedBodyAlgorithm.hpp index 01ebda4..8b834f6 100644 --- a/robotics/dynamics/ArticulatedBodyAlgorithm.hpp +++ b/robotics/dynamics/ArticulatedBodyAlgorithm.hpp @@ -5,6 +5,7 @@ #include "numerical/math/Geometry3D.hpp" #include "numerical/math/Matrix.hpp" #include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/dynamics/SpatialInertia.hpp" #include #include @@ -39,18 +40,8 @@ namespace dynamics Vector3 cJLin; }; - struct SpatialInertia - { - Matrix3 rot; - Matrix3 cross; - Matrix3 lin; - }; - - struct BiasWrench - { - Vector3 force; - Vector3 torque; - }; + using SpatialInertia = dynamics::SpatialInertia; + using BiasWrench = SpatialWrench; struct JointProjection { @@ -67,9 +58,6 @@ namespace dynamics LinkKinematics& current, const LinkKinematics& parent, const RevoluteJointLink& link, T qDot_i); - static SpatialInertia ComputeRigidBodyInertia( - const RevoluteJointLink& link); - static BiasWrench ComputeBiasWrench( const RevoluteJointLink& link, const LinkKinematics& kin); @@ -85,14 +73,6 @@ namespace dynamics 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, @@ -155,21 +135,6 @@ namespace dynamics 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( @@ -234,41 +199,6 @@ namespace dynamics }; } - 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, @@ -293,9 +223,7 @@ namespace dynamics 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; + Ia[parentIndex] += IaParent; pA[parentIndex].force = pA[parentIndex].force + pAParent.force; pA[parentIndex].torque = pA[parentIndex].torque + pAParent.torque; } @@ -310,7 +238,7 @@ namespace dynamics { for (std::size_t i = 0; i < NumLinks; ++i) { - Ia[i] = ComputeRigidBodyInertia(links[i]); + Ia[i] = RigidBodyInertia(links[i]); pA[i] = ComputeBiasWrench(links[i], kin[i]); } diff --git a/robotics/dynamics/CMakeLists.txt b/robotics/dynamics/CMakeLists.txt index 25591d5..d2c5df8 100644 --- a/robotics/dynamics/CMakeLists.txt +++ b/robotics/dynamics/CMakeLists.txt @@ -13,16 +13,19 @@ target_link_libraries(robotics.dynamics ${ROBOTICS_VISIBILITY} target_sources(robotics.dynamics PRIVATE ArticulatedBodyAlgorithm.hpp + CompositeRigidBodyAlgorithm.hpp EulerLagrangeDynamics.hpp EulerLagrangeSolver.hpp NewtonEulerBody.hpp NewtonEulerSolver.hpp RecursiveNewtonEuler.hpp RevoluteJointLink.hpp + SpatialInertia.hpp ) robotics_add_coverage_sources(robotics.dynamics ArticulatedBodyAlgorithm.cpp + CompositeRigidBodyAlgorithm.cpp EulerLagrangeSolver.cpp NewtonEulerSolver.cpp RecursiveNewtonEuler.cpp diff --git a/robotics/dynamics/CompositeRigidBodyAlgorithm.cpp b/robotics/dynamics/CompositeRigidBodyAlgorithm.cpp new file mode 100644 index 0000000..1ec3243 --- /dev/null +++ b/robotics/dynamics/CompositeRigidBodyAlgorithm.cpp @@ -0,0 +1,8 @@ +#include "robotics/dynamics/CompositeRigidBodyAlgorithm.hpp" + +namespace dynamics +{ + template class CompositeRigidBodyAlgorithm; + template class CompositeRigidBodyAlgorithm; + template class CompositeRigidBodyAlgorithm; +} diff --git a/robotics/dynamics/CompositeRigidBodyAlgorithm.hpp b/robotics/dynamics/CompositeRigidBodyAlgorithm.hpp new file mode 100644 index 0000000..9b1dd58 --- /dev/null +++ b/robotics/dynamics/CompositeRigidBodyAlgorithm.hpp @@ -0,0 +1,90 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/RevoluteJointLink.hpp" +#include "robotics/dynamics/SpatialInertia.hpp" +#include +#include +#include +#include + +namespace dynamics +{ + template + class CompositeRigidBodyAlgorithm + { + static_assert(std::is_floating_point_v, "CompositeRigidBodyAlgorithm supports floating-point types"); + static_assert(NumLinks > 0, "Number of links must be positive"); + + public: + using Matrix3 = math::SquareMatrix; + using JointVector = math::Vector; + using MassMatrix = math::SquareMatrix; + using LinkArray = std::array, NumLinks>; + + OPTIMIZE_FOR_SPEED MassMatrix Compute(const LinkArray& links, const JointVector& q) const; + + private: + using Rotations = std::array; + using CompositeInertias = std::array, NumLinks>; + + static CompositeInertias ComputeCompositeInertias(const LinkArray& links, const Rotations& R); + static void FillColumn(MassMatrix& M, std::size_t column, const LinkArray& links, const Rotations& R, const SpatialInertia& composite); + }; + + template + OPTIMIZE_FOR_SPEED typename CompositeRigidBodyAlgorithm::MassMatrix CompositeRigidBodyAlgorithm::Compute(const LinkArray& links, const JointVector& q) const + { + Rotations R{}; + for (std::size_t i = 0; i < NumLinks; ++i) + { + assert(HasUnitJointAxis(links[i])); + R[i] = math::RotationAboutAxis(links[i].jointAxis, q[i]); + } + + const auto composite{ ComputeCompositeInertias(links, R) }; + + MassMatrix M{}; + for (std::size_t i = 0; i < NumLinks; ++i) + FillColumn(M, i, links, R, composite[i]); + + return M; + } + + template + typename CompositeRigidBodyAlgorithm::CompositeInertias CompositeRigidBodyAlgorithm::ComputeCompositeInertias(const LinkArray& links, const Rotations& R) + { + CompositeInertias composite{}; + for (std::size_t i = 0; i < NumLinks; ++i) + composite[i] = RigidBodyInertia(links[i]); + + for (std::size_t i = NumLinks - 1; i > 0; --i) + composite[i - 1] += TransformInertiaToParent(composite[i], R[i], links[i].parentToJoint); + + return composite; + } + + template + void CompositeRigidBodyAlgorithm::FillColumn(MassMatrix& M, std::size_t column, const LinkArray& links, const Rotations& R, const SpatialInertia& composite) + { + const auto& axis{ links[column].jointAxis }; + SpatialWrench wrench{ composite.cross.Transpose() * axis, composite.rot * axis }; + M.at(column, column) = math::DotProduct(axis, wrench.torque); + + for (std::size_t j = column; j > 0; --j) + { + wrench = TransformWrenchToParent(wrench, R[j], links[j].parentToJoint); + const T entry{ math::DotProduct(links[j - 1].jointAxis, wrench.torque) }; + M.at(column, j - 1) = entry; + M.at(j - 1, column) = entry; + } + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class CompositeRigidBodyAlgorithm; + extern template class CompositeRigidBodyAlgorithm; + extern template class CompositeRigidBodyAlgorithm; +#endif +} diff --git a/robotics/dynamics/SpatialInertia.hpp b/robotics/dynamics/SpatialInertia.hpp new file mode 100644 index 0000000..bb9e9fb --- /dev/null +++ b/robotics/dynamics/SpatialInertia.hpp @@ -0,0 +1,77 @@ +#pragma once + +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/RevoluteJointLink.hpp" +#include + +namespace dynamics +{ + template + struct SpatialInertia + { + static_assert(std::is_floating_point_v, "SpatialInertia supports floating-point types"); + + math::SquareMatrix rot; + math::SquareMatrix cross; + math::SquareMatrix lin; + + SpatialInertia& operator+=(const SpatialInertia& other) + { + rot = rot + other.rot; + cross = cross + other.cross; + lin = lin + other.lin; + return *this; + } + }; + + template + struct SpatialWrench + { + static_assert(std::is_floating_point_v, "SpatialWrench supports floating-point types"); + + math::Vector force; + math::Vector torque; + }; + + template + SpatialInertia RigidBodyInertia(const RevoluteJointLink& link) + { + const auto rSkew{ math::SkewSymmetric(link.jointToCoM) }; + + return SpatialInertia{ + link.inertia + rSkew.Transpose() * rSkew * link.mass, + rSkew * link.mass, + math::ScaledIdentity(link.mass) + }; + } + + template + SpatialInertia TransformInertiaToParent(const SpatialInertia& inertia, + const math::SquareMatrix& R, const math::Vector& parentToJoint) + { + const auto Rt{ R.Transpose() }; + const auto rotP{ R * inertia.rot * Rt }; + const auto crossP{ R * inertia.cross * Rt }; + const auto linP{ R * inertia.lin * Rt }; + const auto rSkew{ math::SkewSymmetric(parentToJoint) }; + + return SpatialInertia{ + rotP - crossP * rSkew + rSkew * crossP.Transpose() - rSkew * linP * rSkew, + crossP + rSkew * linP, + linP + }; + } + + template + SpatialWrench TransformWrenchToParent(const SpatialWrench& wrench, + const math::SquareMatrix& R, const math::Vector& parentToJoint) + { + const auto forceP{ R * wrench.force }; + + return SpatialWrench{ + forceP, + R * wrench.torque + math::CrossProduct(parentToJoint, forceP) + }; + } +} diff --git a/robotics/dynamics/test/CMakeLists.txt b/robotics/dynamics/test/CMakeLists.txt index 9dd0425..4ce1ebd 100644 --- a/robotics/dynamics/test/CMakeLists.txt +++ b/robotics/dynamics/test/CMakeLists.txt @@ -9,6 +9,7 @@ target_link_libraries(robotics.dynamics_test PUBLIC target_sources(robotics.dynamics_test PRIVATE TestArticulatedBodyAlgorithm.cpp + TestCompositeRigidBodyAlgorithm.cpp TestEulerLagrangeSolver.cpp TestNewtonEulerSolver.cpp TestRecursiveNewtonEuler.cpp diff --git a/robotics/dynamics/test/TestCompositeRigidBodyAlgorithm.cpp b/robotics/dynamics/test/TestCompositeRigidBodyAlgorithm.cpp new file mode 100644 index 0000000..4a82de8 --- /dev/null +++ b/robotics/dynamics/test/TestCompositeRigidBodyAlgorithm.cpp @@ -0,0 +1,143 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Tolerance.hpp" +#include "robotics/dynamics/CompositeRigidBodyAlgorithm.hpp" +#include "robotics/dynamics/RecursiveNewtonEuler.hpp" +#include +#include + +namespace +{ + using Vector3 = math::Vector; + using Matrix3 = math::SquareMatrix; + using Link = dynamics::RevoluteJointLink; + + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeRod(float mass, float length, const Vector3& parentToJoint) + { + const float inertia{ mass * length * length / 12.0f }; + return Link{ mass, Matrix3{ { 0.0f, 0.0f, 0.0f }, { 0.0f, inertia, 0.0f }, { 0.0f, 0.0f, inertia } }, zAxis, parentToJoint, + Vector3{ length / 2.0f, 0.0f, 0.0f } }; + } + + Vector3 Normalized(float x, float y, float z) + { + const float norm{ std::sqrt(x * x + y * y + z * z) }; + return Vector3{ x / norm, y / norm, z / norm }; + } + + float Determinant2(const math::SquareMatrix& M) + { + return M.at(0, 0) * M.at(1, 1) - M.at(0, 1) * M.at(1, 0); + } + + float Determinant3(const math::SquareMatrix& M) + { + return M.at(0, 0) * (M.at(1, 1) * M.at(2, 2) - M.at(1, 2) * M.at(2, 1)) - M.at(0, 1) * (M.at(1, 0) * M.at(2, 2) - M.at(1, 2) * M.at(2, 0)) + M.at(0, 2) * (M.at(1, 0) * M.at(2, 1) - M.at(1, 1) * M.at(2, 0)); + } + + class TestCompositeRigidBodyAlgorithm + : public ::testing::Test + { + protected: + float KineticEnergyOfLinks(const math::Vector& q, const math::Vector& qDot) const + { + Vector3 omega{}; + Vector3 velocity{}; + float energy{ 0.0f }; + for (std::size_t i = 0; i < 3; ++i) + { + const Matrix3 Rt{ math::RotationAboutAxis(chain[i].jointAxis, q[i]).Transpose() }; + velocity = Rt * (velocity + math::CrossProduct(omega, chain[i].parentToJoint)); + omega = Rt * omega + chain[i].jointAxis * qDot[i]; + const Vector3 velocityCoM{ velocity + math::CrossProduct(omega, chain[i].jointToCoM) }; + energy += 0.5f * chain[i].mass * math::DotProduct(velocityCoM, velocityCoM) + 0.5f * math::DotProduct(omega, chain[i].inertia * omega); + } + return energy; + } + + dynamics::CompositeRigidBodyAlgorithm crba2; + dynamics::CompositeRigidBodyAlgorithm crba3; + std::array chain{ + Link{ 1.2f, Matrix3{ { 0.020f, 0.001f, -0.002f }, { 0.001f, 0.030f, 0.003f }, { -0.002f, 0.003f, 0.025f } }, zAxis, Vector3{ 0.0f, 0.0f, 0.1f }, Vector3{ 0.05f, 0.02f, 0.1f } }, + Link{ 0.9f, Matrix3{ { 0.015f, -0.002f, 0.001f }, { -0.002f, 0.020f, 0.0f }, { 0.001f, 0.0f, 0.012f } }, Normalized(0.0f, 1.0f, 1.0f), Vector3{ 0.1f, 0.0f, 0.2f }, Vector3{ 0.15f, -0.03f, 0.02f } }, + Link{ 0.6f, Matrix3{ { 0.008f, 0.0f, 0.001f }, { 0.0f, 0.010f, -0.001f }, { 0.001f, -0.001f, 0.006f } }, Normalized(1.0f, 0.0f, 1.0f), Vector3{ 0.3f, 0.05f, 0.0f }, Vector3{ 0.1f, 0.04f, -0.05f } } + }; + math::Vector q{ 0.4f, -0.7f, 1.1f }; + }; +} + +TEST_F(TestCompositeRigidBodyAlgorithm, single_rod_about_end_is_ml2_over_3) +{ + const float mass{ 2.0f }; + const float length{ 1.5f }; + dynamics::CompositeRigidBodyAlgorithm crba1; + + const auto M{ crba1.Compute({ MakeRod(mass, length, Vector3{}) }, math::Vector{ 0.7f }) }; + + EXPECT_NEAR(M.at(0, 0), mass * length * length / 3.0f, math::Tolerance()); +} + +TEST_F(TestCompositeRigidBodyAlgorithm, two_link_planar_matches_closed_form) +{ + const float m1{ 2.0f }; + const float l1{ 1.2f }; + const float m2{ 0.8f }; + const float l2{ 0.7f }; + const float q2{ -0.5f }; + + const auto M{ crba2.Compute({ MakeRod(m1, l1, Vector3{}), MakeRod(m2, l2, Vector3{ l1, 0.0f, 0.0f }) }, math::Vector{ 0.3f, q2 }) }; + + EXPECT_NEAR(M.at(0, 0), m1 * l1 * l1 / 3.0f + m2 * (l1 * l1 + l2 * l2 / 3.0f + l1 * l2 * std::cos(q2)), math::Tolerance()); + EXPECT_NEAR(M.at(0, 1), m2 * (l2 * l2 / 3.0f + l1 * l2 * std::cos(q2) / 2.0f), math::Tolerance()); + EXPECT_NEAR(M.at(1, 1), m2 * l2 * l2 / 3.0f, math::Tolerance()); +} + +TEST_F(TestCompositeRigidBodyAlgorithm, matrix_is_independent_of_first_joint_angle) +{ + const std::array links{ MakeRod(1.0f, 1.0f, Vector3{}), MakeRod(1.0f, 1.0f, Vector3{ 1.0f, 0.0f, 0.0f }) }; + + const auto first{ crba2.Compute(links, math::Vector{ 0.3f, -0.5f }) }; + const auto second{ crba2.Compute(links, math::Vector{ -1.2f, -0.5f }) }; + + for (std::size_t r = 0; r < 2; ++r) + for (std::size_t c = 0; c < 2; ++c) + EXPECT_NEAR(first.at(r, c), second.at(r, c), math::Tolerance()); +} + +TEST_F(TestCompositeRigidBodyAlgorithm, columns_match_recursive_newton_euler) +{ + dynamics::RecursiveNewtonEuler rnea; + + const auto M{ crba3.Compute(chain, q) }; + + for (std::size_t j = 0; j < 3; ++j) + { + math::Vector unit{}; + unit[j] = 1.0f; + const auto column{ rnea.InverseDynamics(chain, q, math::Vector{}, unit, Vector3{}) }; + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(M.at(i, j), column[i], math::Tolerance()); + } +} + +TEST_F(TestCompositeRigidBodyAlgorithm, matrix_is_symmetric_positive_definite) +{ + const auto M{ crba3.Compute(chain, q) }; + + for (std::size_t r = 0; r < 3; ++r) + for (std::size_t c = 0; c < 3; ++c) + EXPECT_EQ(M.at(r, c), M.at(c, r)); + EXPECT_GT(M.at(0, 0), 0.0f); + EXPECT_GT(Determinant2(M), 0.0f); + EXPECT_GT(Determinant3(M), 0.0f); +} + +TEST_F(TestCompositeRigidBodyAlgorithm, kinetic_energy_matches_link_sum) +{ + const math::Vector qDot{ 0.8f, -1.3f, 2.1f }; + + const auto M{ crba3.Compute(chain, q) }; + + EXPECT_NEAR(0.5f * math::DotProduct(qDot, M * qDot), KineticEnergyOfLinks(q, qDot), math::Tolerance()); +} From 0a106a9c066ee9de7b2a6e57d5517983025d5227 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 09:50:58 +0000 Subject: [PATCH 3/7] feat(planning): add PrimitiveDistance (P1) Closed-form closest points and signed distances between points, segments, spheres, capsules and oriented boxes. Creates the planning module; P2, P4 and P5 build on these queries. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 1 + ROADMAP.md | 4 - doc/planning/PrimitiveDistance.md | 105 +++++++ doc/planning/README.md | 9 + roadmap/README.md | 2 +- .../planning/PrimitiveDistance/explanation.md | 30 -- .../PrimitiveDistance/implementation.md | 114 -------- roadmap/planning/PrimitiveDistance/tests.md | 49 ---- robotics/CMakeLists.txt | 1 + robotics/planning/CMakeLists.txt | 21 ++ robotics/planning/PrimitiveDistance.cpp | 6 + robotics/planning/PrimitiveDistance.hpp | 259 ++++++++++++++++++ robotics/planning/test/CMakeLists.txt | 12 + .../planning/test/TestPrimitiveDistance.cpp | 118 ++++++++ 14 files changed, 533 insertions(+), 198 deletions(-) create mode 100644 doc/planning/PrimitiveDistance.md create mode 100644 doc/planning/README.md delete mode 100644 roadmap/planning/PrimitiveDistance/explanation.md delete mode 100644 roadmap/planning/PrimitiveDistance/implementation.md delete mode 100644 roadmap/planning/PrimitiveDistance/tests.md create mode 100644 robotics/planning/CMakeLists.txt create mode 100644 robotics/planning/PrimitiveDistance.cpp create mode 100644 robotics/planning/PrimitiveDistance.hpp create mode 100644 robotics/planning/test/CMakeLists.txt create mode 100644 robotics/planning/test/TestPrimitiveDistance.cpp diff --git a/README.md b/README.md index 4321bc2..17daeb2 100644 --- a/README.md +++ b/README.md @@ -39,6 +39,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. |----------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------| | [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Composite Rigid Body Algorithm (CRBA) | +| [Planning](doc/planning/README.md) | Primitive Distance Queries | | [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory | Each category page lists its algorithms with a brief description and links to the detailed diff --git a/ROADMAP.md b/ROADMAP.md index c6fb0ba..13663c9 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -454,16 +454,12 @@ Cross-family building blocks that several of the families below depend on. | # | Component | Target module | Depends on | Difficulty | |----|-------------------------------------------------------------------|---------------|------------|------------| -| P1 | Primitive distance queries (point, segment, sphere, capsule, box) | `planning` | — | ★★☆☆☆ | | P2 | Artificial potential fields | `planning` | P1 | ★★☆☆☆ | | P3 | A* / Dijkstra on bounded occupancy grids | `planning` | — | ★★★☆☆ | | P4 | Self-collision checking for a link chain (capsule model) | `planning` | P1, M30 | ★★★☆☆ | | P5 | GJK + EPA convex distance and penetration | `planning` | — | ★★★★☆ | | P6 | RRT-Connect in joint space with a fixed node pool | `planning` | P4 | ★★★★☆ | -**P1. Primitive distances.** Closed-form closest points and signed distances (negative = penetration) between points, segments, spheres, capsules and oriented boxes, as core geometry plus radius. -- *Algorithm / paper:* C. Ericson, *Real-Time Collision Detection* (2004), Ch. 5. - **P2. Potential fields.** Quadratic-then-conic attraction and Khatib repulsion from obstacle proximity readings as a clamped task-space velocity; local minima are detected and reported. - *Algorithm / paper:* O. Khatib, *IJRR*, 5(1), 1986. - *Reuses / builds on:* P1. diff --git a/doc/planning/PrimitiveDistance.md b/doc/planning/PrimitiveDistance.md new file mode 100644 index 0000000..1c2d1bd --- /dev/null +++ b/doc/planning/PrimitiveDistance.md @@ -0,0 +1,105 @@ +# Primitive Distance Queries + +## Overview & Motivation + +Collision checking on a robot rarely uses the real link meshes. Arm links are wrapped in **capsules** (a sphere swept along a segment), grippers and heads in **spheres**, and tables, walls and fixtures in **oriented boxes**. These checks run every control cycle in self-collision avoidance, human–robot speed and separation monitoring, and reactive obstacle avoidance. They must be exact, constant-time, branch-light and allocation-free. + +The queries return the **closest points** on both shapes and a **signed distance**: positive when the shapes are apart, negative when they overlap, where the magnitude is the penetration depth. The signed value lets one smooth function serve both clearance monitoring and contact response. + +## Mathematical Theory + +### Rounded Shapes + +Every rounded shape is a **core** (a point or a segment) inflated by a radius $r$. For two rounded shapes with core closest points $c_1, c_2$ at core distance $d = \|c_2 - c_1\|$: + +$$\delta = d - r_1 - r_2, \qquad n = \frac{c_2 - c_1}{d}, \qquad p_1 = c_1 + r_1 n, \qquad p_2 = c_2 - r_2 n$$ + +This is exact while the cores are apart. When the cores intersect, the true penetration direction is undefined from the cores alone. The value is then a lower bound on the depth, and a fixed normal is reported when $d \to 0$. + +### Point to Segment + +For the segment $a + t(b - a)$, $t \in [0, 1]$, the closest parameter is the clamped projection + +$$t^\star = \operatorname{clamp}\left(\frac{(p - a)\cdot(b - a)}{\|b - a\|^2},\ 0,\ 1\right)$$ + +with $t^\star = 0$ for a zero-length segment. + +### Segment to Segment + +For segments $p_1 + s\,d_1$ and $p_2 + t\,d_2$ with $r = p_1 - p_2$, minimizing $\|p_1 + s d_1 - p_2 - t d_2\|^2$ gives the linear system + +$$\begin{bmatrix} a & -b \\ b & -e \end{bmatrix}\begin{bmatrix} s \\ t \end{bmatrix} = \begin{bmatrix} -c \\ -f \end{bmatrix}, \quad a = d_1 \cdot d_1,\ b = d_1 \cdot d_2,\ c = d_1 \cdot r,\ e = d_2 \cdot d_2,\ f = d_2 \cdot r$$ + +The unconstrained solution is $s = (bf - ce)/(ae - b^2)$. Clamping it to $[0, 1]$, solving $t = (bs + f)/e$, and re-clamping $s$ when $t$ leaves $[0, 1]$ yields the constrained optimum (Ericson, §5.1.9). The function is convex in $(s, t)$, so one pass of this clamp–resolve sequence is enough. Two special cases need their own branch: + +- **Parallel segments** ($ae - b^2 \approx 0$): a continuum of closest pairs. Fixing $s = 0$ and resolving $t$ returns one valid pair. +- **Degenerate segments** ($a \approx 0$ or $e \approx 0$): the problem reduces to point–segment or point–point. + +### Point to Oriented Box + +With box pose $(R, c)$ and half extents $h$, the point is expressed in the box frame, $l = R^T(p - c)$, which needs no matrix inverse. + +- **Outside** (some $|l_i| > h_i$): the closest point is the component-wise clamp $q = \operatorname{clamp}(l, -h, h)$ and the distance is $\|l - q\|$. +- **Inside**: the point is pushed out through the nearest face $k = \arg\min_i (h_i - |l_i|)$. The signed distance is $-(h_k - |l_k|)$ and the outward normal is $\operatorname{sign}(l_k)\,R\,e_k$. + +A sphere against a box is the point query on its centre, rounded by the radius. Inside the box, the sphere point is offset by $-r$ along the face normal. + +## Complexity Analysis + +| Query | Time | Space | Notes | +|-------------------------|--------|--------|----------------------------------------------------------| +| Point–segment | $O(1)$ | $O(1)$ | One division, one square root | +| Segment–segment | $O(1)$ | $O(1)$ | Up to three divisions, one square root, no iteration | +| Point–box, sphere–box | $O(1)$ | $O(1)$ | One $3 \times 3$ rotation, three clamps | +| Rounded pairs | $O(1)$ | $O(1)$ | Core query plus one normalization | + +There are no loops beyond the fixed three box axes, so timing is fully deterministic. + +## Step-by-Step Walkthrough + +**Segments** $[(0,0,0), (1,2,3)]$ and $[(2,0,1), (0,1,-1)]$: + +1. $d_1 = (1,2,3)$, $d_2 = (-2,1,-2)$, $r = (-2,0,-1)$, giving $a = 14$, $b = -6$, $c = -5$, $e = 9$, $f = 6$. +2. $ae - b^2 = 90$, so $s = (bf - ce)/90 = (-36 + 45)/90 = 0.1$, inside $[0,1]$. +3. $t = (bs + f)/e = (-0.6 + 6)/9 = 0.6$, also inside. +4. Closest points $(0.1, 0.2, 0.3)$ and $(0.8, 0.6, -0.2)$, distance $\sqrt{0.9} = 0.9487$. + +**Point** $(2, 1, 0)$ against a box rotated $30°$ about $z$, centred at $(1,0,0)$, half extents $(0.5, 0.25, 0.1)$: + +1. $l = R^T (1, 1, 0) = (1.366, 0.366, 0)$, outside on $x$ and $y$. +2. Clamp to $q = (0.5, 0.25, 0)$, so $l - q = (0.866, 0.116, 0)$ and the distance is $0.8738$. +3. The closest surface point is $c + Rq = (1.308, 0.4665, 0)$. + +## Pitfalls & Edge Cases + +- **Tolerances on squared lengths**: degeneracy tests compare squared lengths with a tiny threshold. A threshold on the length itself would reject legitimately short links. +- **Parallel segments**: the solver must not divide by $ae - b^2$ when it vanishes. The pair it returns is one of many, so callers must not rely on which one. +- **Penetrating cores**: for capsules whose axes cross, the reported depth is a lower bound and the normal is arbitrary. Use a convex-penetration algorithm (EPA) when the exact depth matters. +- **Points on a box face**: classified as inside with zero depth, so the distance is continuous across the surface. +- **Pairs without a closed form**: capsule–box, box–box and convex hulls have no compact closed form. Use GJK/EPA rather than approximating them with these formulas. +- **Single precision**: distances near zero lose relative accuracy. Apply safety margins in the monitor, not by shrinking the shapes. + +## Variants & Generalizations + +- **Swept volumes**: continuous collision checking sweeps a capsule over a time step, so it becomes a segment–segment query in space-time. +- **Axis-aligned boxes**: the rotation step disappears, which is useful for broad-phase culling. +- **Distance derivatives**: the gradient of the signed distance with respect to a point is the unit normal $n$, which repulsive potential fields and collision-avoidance constraints use. +- **Spheres as degenerate capsules**: a zero-length capsule axis reproduces the sphere queries, so one capsule–capsule kernel can serve both. + +## Applications + +- Self-collision checking of an arm modelled as one capsule per link. +- Speed and separation monitoring between the robot and a human tracked as capsules and spheres. +- Repulsive terms in artificial potential fields and distance constraints in reactive controllers. +- Narrow-phase checks in sampling-based planners after a coarse broad phase. + +## Connections to Other Algorithms + +- [Forward Kinematics](../kinematics/ForwardKinematics.md): supplies the joint positions that become the capsule axes of an arm. +- The oriented-box pose uses the SE(3) rigid transform from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Ericson, C. (2004). *Real-Time Collision Detection*. Morgan Kaufmann. Chapter 5. +- Eberly, D. (2006). *3D Game Engine Design*. 2nd ed. Morgan Kaufmann. Chapter 14. +- Gilbert, E., Johnson, D. and Keerthi, S. (1988). "A fast procedure for computing the distance between complex objects in three-dimensional space." *IEEE J. Robotics and Automation*, 4(2), 193–203. diff --git a/doc/planning/README.md b/doc/planning/README.md new file mode 100644 index 0000000..4b1efca --- /dev/null +++ b/doc/planning/README.md @@ -0,0 +1,9 @@ +# Motion Planning & Collision + +Geometric queries and planners for collision-free robot motion. + +## Algorithms + +| Algorithm | Description | +|----------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------| +| [Primitive Distance Queries](PrimitiveDistance.md) | Closed-form closest points and signed distances between points, segments, spheres, capsules and oriented boxes — $O(1)$ | diff --git a/roadmap/README.md b/roadmap/README.md index 4664593..d10db42 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -101,7 +101,7 @@ prerequisites. `PlanarOdometry` (F1) · `DifferentialDrive` (W1) · `CarLikeKinematics` (W2) · `OmnidirectionalKinematics` (W3) · `SkidSteerKinematics` (W4) · `PurePursuit` (W5) · `StanleyController` (W6) · `KanayamaController` (W7) ### `planning` -`PrimitiveDistance` (P1) · `PotentialField` (P2) · `GridSearch` (P3) · `SelfCollision` (P4) · `ConvexCollision` (P5) · `RrtConnect` (P6) · `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) +`PotentialField` (P2) · `GridSearch` (P3) · `SelfCollision` (P4) · `ConvexCollision` (P5) · `RrtConnect` (P6) · `DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) ### `localization` `OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) diff --git a/roadmap/planning/PrimitiveDistance/explanation.md b/roadmap/planning/PrimitiveDistance/explanation.md deleted file mode 100644 index 58a6939..0000000 --- a/roadmap/planning/PrimitiveDistance/explanation.md +++ /dev/null @@ -1,30 +0,0 @@ -# Primitive Distance Queries — Overview - -## What it is -Exact formulas for the closest points and the distance between simple shapes: points, line segments, -spheres, capsules (a sphere swept along a segment) and oriented boxes. A negative distance means the -shapes overlap, and its size is how deep. - -## Why it matters (embedded) -Robots are usually wrapped in capsules (arm links), spheres (grippers, heads) and boxes (tables, walls) for -collision checking. These checks run every control cycle in self-collision avoidance, human–robot speed -and separation monitoring, and reactive obstacle avoidance, so they must be exact, branch-light and -allocation-free. - -## How it works (intuition) -Every rounded shape is a core (a point or a segment) inflated by a radius. The distance between two such -shapes is the distance between their cores minus the two radii, and the closest points move outwards along -the line joining the core points. The core distances are classic geometry: project a point onto a -segment and clamp it, or solve the two-parameter problem of two segments with careful handling of -parallel and zero-length cases. For a box, the point is moved into the box's own frame and clamped to its -half extents; points already inside are pushed out through the nearest face. - -## Key parameters -- **shape sizes** — radii, segment endpoints, box pose and half extents. - -## Reference -C. Ericson, *Real-Time Collision Detection* (2004), Ch. 5. - -## See also -`ConvexCollision` (P5, GJK/EPA for pairs without a closed form), `SelfCollision` (P4), `PotentialField` (P2), -`math::SE3Transform` (NT53). diff --git a/roadmap/planning/PrimitiveDistance/implementation.md b/roadmap/planning/PrimitiveDistance/implementation.md deleted file mode 100644 index 93ceef2..0000000 --- a/roadmap/planning/PrimitiveDistance/implementation.md +++ /dev/null @@ -1,114 +0,0 @@ -# Primitive Distance Queries (Point, Segment, Sphere, Capsule, Box) — Implementation Pseudocode - -> Roadmap ref: #P1 (★★☆☆☆) · Target: `robotics/planning` · Namespace `planning` · Type: `float` (templated on `T`, instantiated for `float` only) - -Closed-form closest points and signed distances between the convex primitives that robot links, -grippers, people and obstacles are usually wrapped in. Everything is reduced to the core geometry -(point and segment) plus a radius, the "rounded" representation that keeps capsule queries closed-form. -P4 (self-collision), P2 (potential fields) and H2 (speed and separation monitoring) build on these. Pairs -without a closed form (capsule–box, box–box, convex hulls) go to GJK/EPA (P5). - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct Segment: math::Vector3 start, end -struct Sphere: math::Vector3 center; T radius -struct Capsule: Segment axis; T radius # a sphere swept along the segment -struct Box: math::SE3Transform pose; math::Vector3 halfExtents # oriented box (NT53) - -template -struct ClosestPoints: - math::Vector3 onFirst, onSecond # on the surfaces (cores for points and segments) - T distance # signed: < 0 is penetration depth - -template -struct SegmentPair: - math::Vector3 onFirst, onSecond - T s, t # parameters along the two segments, in [0, 1] - T distance -``` - -## Interface - -```cpp -template class PrimitiveDistance # stateless; static members only - static ClosestPoints PointSegment(const math::Vector3& p, const Segment& segment) - static SegmentPair SegmentSegment(const Segment& a, const Segment& b) # hot path - static ClosestPoints PointBox(const math::Vector3& p, const Box& box) # signed - static ClosestPoints SphereSphere(const Sphere& a, const Sphere& b) - static ClosestPoints SphereCapsule(const Sphere& a, const Capsule& b) - static ClosestPoints CapsuleCapsule(const Capsule& a, const Capsule& b) # hot path - static ClosestPoints SphereBox(const Sphere& a, const Box& b) -``` - -## Algorithm (pseudocode) - -```text -ε = 1e-12 (squared lengths); Dot/Norm from Geometry3D - -function PointSegment(p, [a, b]): - d = b − a; L = d·d; t = (L ≤ ε) ? 0 : clamp((p − a)·d / L, 0, 1) - c = a + t·d; return { p, c, ‖p − c‖ } - -function SegmentSegment([p₁, q₁], [p₂, q₂]): # Ericson (2004) §5.1.9, handles degenerate segments - d₁ = q₁ − p₁; d₂ = q₂ − p₂; r = p₁ − p₂; a = d₁·d₁; e = d₂·d₂; f = d₂·r - if a ≤ ε and e ≤ ε: s = t = 0 - elif a ≤ ε: s = 0; t = clamp(f/e, 0, 1) - else: - c = d₁·r - if e ≤ ε: t = 0; s = clamp(−c/a, 0, 1) - else: - b = d₁·d₂; den = a·e − b² - s = (den > ε) ? clamp((b·f − c·e)/den, 0, 1) : 0 # parallel: any s, pick 0 - t = (b·s + f)/e - if t < 0: t = 0; s = clamp(−c/a, 0, 1) - elif t > 1: t = 1; s = clamp((b − c)/a, 0, 1) - c₁ = p₁ + s·d₁; c₂ = p₂ + t·d₂; return { c₁, c₂, s, t, ‖c₁ − c₂‖ } - -function PointBox(p, box): # Ericson §5.1.4, signed - l = Rᵀ·(p − c) # box frame - if |lᵢ| ≤ hᵢ for all i: # inside: push to the nearest face - k = argminᵢ (hᵢ − |lᵢ|); q = l with q_k = sign(l_k)·h_k (sign(0) = +1) - return { p, c + R·q, −(h_k − |l_k|) } - q = clamp(l, −h, h); return { p, c + R·q, ‖l − q‖ } - -function Rounded(core₁, core₂, coreDistance, r₁, r₂): # shared by the sphere and capsule pairs - n = (coreDistance > √ε) ? (core₂ − core₁)/coreDistance : (0, 0, 1) # coincident cores: any normal - return { core₁ + r₁·n, core₂ − r₂·n, coreDistance − r₁ − r₂ } - -SphereSphere: Rounded(a.center, b.center, ‖b.center − a.center‖, a.r, b.r) -SphereCapsule: q = PointSegment(a.center, b.axis); Rounded(a.center, q.onSecond, q.distance, a.r, b.r) -CapsuleCapsule: s = SegmentSegment(a.axis, b.axis); Rounded(s.onFirst, s.onSecond, s.distance, a.r, b.r) -SphereBox: q = PointBox(a.center, b) - outside: Rounded(a.center, q.onSecond, q.distance, a.r, 0) - inside: { a.center − a.r·n_face, q.onSecond, q.distance − a.r } # n_face = outward normal of face k -``` - -## Complexity & memory - -- Every query is `O(1)`: at most two divisions and one square root. No heap, no iteration. - -## Numerical / embedded notes - -- **Signed distances.** Negative values are penetration depths along the reported points, which is what - repulsive fields (P2) and safety monitors (H2) need near contact. For capsule pairs the depth is exact - while the cores stay apart; with intersecting cores it is a lower bound (use P5 if that matters). -- **Degenerate inputs.** Zero-length segments (spheres as capsules) and parallel segments take explicit - branches. The parallel branch returns one valid pair of closest points out of a continuum. -- **Box frame.** The box pose is an SE(3) transform, so `Rᵀ` comes from the transform's rotation, and no - matrix inverse is formed. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/planning/PrimitiveDistance.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `SegmentSegment`/`CapsuleCapsule`, and `extern template class PrimitiveDistance;` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/planning/PrimitiveDistance.cpp` → `template class PrimitiveDistance;` -- Test: `robotics/planning/test/TestPrimitiveDistance.cpp` -- Doc: `doc/planning/PrimitiveDistance.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources(robotics.planning ...)`; `.cpp` → `robotics_add_coverage_sources`; - test → `robotics.planning_test` (the `planning` module is created by the first planning item deployed). -- Depends on: NT53 (`SE3Transform`); upstream `Geometry3D`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/planning/PrimitiveDistance/tests.md b/roadmap/planning/PrimitiveDistance/tests.md deleted file mode 100644 index c4b39b8..0000000 --- a/roadmap/planning/PrimitiveDistance/tests.md +++ /dev/null @@ -1,49 +0,0 @@ -# Primitive Distance Queries — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestPrimitiveDistance : public ::testing::Test: - Box box{ { RotationAboutAxis(ẑ, π/6), (1, 0, 0) }, (0.5, 0.25, 0.1) } -# each case below is a TEST_F(TestPrimitiveDistance, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -point_segment_projects_and_clamps: - Assert: PointSegment((1, 1, 0), [(0,0,0), (2,0,0)]) ⇒ onSecond (1, 0, 0), distance 1 - PointSegment((−1, 1, 0), same) ⇒ onSecond (0, 0, 0), distance √2 -segment_segment_matches_brute_force: - Assert: [(0,0,0), (1,2,3)] vs [(2,0,1), (0,1,−1)] ⇒ s = 0.1, t = 0.6, points (0.1, 0.2, 0.3), (0.8, 0.6, −0.2), - distance ≈ 0.948683 # 801 × 801 sampling gives 0.948683 - skew [(0,0,0), (1,0,0)] vs [(0.5,−1,1), (0.5,1,1)] ⇒ (0.5, 0, 0), (0.5, 0, 1), distance 1 -segment_segment_handles_parallel_and_degenerate_inputs: - Assert: parallel [(0,0,0), (2,0,0)] vs [(1,1,0), (3,1,0)] ⇒ distance 1 (one valid pair, e.g. (1,0,0)–(1,1,0)) - endpoint case [(0,0,0), (1,0,0)] vs [(2,1,0), (3,2,0)] ⇒ s = 1, t = 0, distance √2 - point–point and point–segment (zero-length inputs) ⇒ √3 and 1 -point_box_is_signed: - Assert: PointBox((2, 1, 0), box) ⇒ onSecond (1.308013, 0.466506, 0), distance 0.873763 - PointBox((1.05, 0, 0.02), box) ⇒ onSecond (1.05, 0, 0.1), distance −0.08 # inside: nearest face z = +0.1 -rounded_pairs_subtract_the_radii: - Assert: SphereSphere(((0,0,0), 1), ((3,1,0), 0.5)) ⇒ distance √10 − 1.5 = 1.662278, onFirst (0.948683, 0.316228, 0) - CapsuleCapsule(([(0,0,0),(1,0,0)], 0.2), ([(0.5,−1,1),(0.5,1,1)], 0.3)) ⇒ distance 0.5 - the same with the second axis at z = 0.4 ⇒ distance −0.1 (overlap) - SphereBox(((2, 1, 0), 0.3), box) ⇒ distance 0.573763 -sphere_inside_a_box_reports_depth: - Assert: SphereBox(((1.05, 0, 0.02), 0.05), box) ⇒ distance −0.13 -coincident_cores_still_return_a_normal: - Assert: SphereSphere(((1,1,1), 0.2), ((1,1,1), 0.3)) ⇒ distance −0.5, points separated along +z -``` - -## Reference vectors - -- Box `R = Rz(30°)`, centre `(1, 0, 0)`, half extents `(0.5, 0.25, 0.1)`; `(2, 1, 0)` is `0.873763` from its surface. -- The general segment pair agrees with brute-force sampling to `1e-7`. - -## Edge cases - -- Point exactly on a box face ⇒ distance 0 (inside branch with zero depth). -- Segment endpoint touching the other segment ⇒ distance 0, `s` or `t` at an end. diff --git a/robotics/CMakeLists.txt b/robotics/CMakeLists.txt index f66850e..e228377 100644 --- a/robotics/CMakeLists.txt +++ b/robotics/CMakeLists.txt @@ -1,3 +1,4 @@ add_subdirectory(dynamics) add_subdirectory(kinematics) +add_subdirectory(planning) add_subdirectory(trajectory) diff --git a/robotics/planning/CMakeLists.txt b/robotics/planning/CMakeLists.txt new file mode 100644 index 0000000..7b172cf --- /dev/null +++ b/robotics/planning/CMakeLists.txt @@ -0,0 +1,21 @@ +robotics_add_header_library(robotics.planning) + +target_include_directories(robotics.planning ${ROBOTICS_VISIBILITY} + "$" + "$" +) + +target_link_libraries(robotics.planning ${ROBOTICS_VISIBILITY} + infra.util + numerical.math +) + +target_sources(robotics.planning PRIVATE + PrimitiveDistance.hpp +) + +robotics_add_coverage_sources(robotics.planning + PrimitiveDistance.cpp +) + +add_subdirectory(test) diff --git a/robotics/planning/PrimitiveDistance.cpp b/robotics/planning/PrimitiveDistance.cpp new file mode 100644 index 0000000..aa10621 --- /dev/null +++ b/robotics/planning/PrimitiveDistance.cpp @@ -0,0 +1,6 @@ +#include "robotics/planning/PrimitiveDistance.hpp" + +namespace planning +{ + template class PrimitiveDistance; +} diff --git a/robotics/planning/PrimitiveDistance.hpp b/robotics/planning/PrimitiveDistance.hpp new file mode 100644 index 0000000..48c5ac0 --- /dev/null +++ b/robotics/planning/PrimitiveDistance.hpp @@ -0,0 +1,259 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE3Transform.hpp" +#include +#include +#include + +namespace planning +{ + template + struct Segment + { + static_assert(std::is_floating_point_v, "Segment supports floating-point types"); + + math::Vector3 start{}; + math::Vector3 end{}; + }; + + template + struct Sphere + { + static_assert(std::is_floating_point_v, "Sphere supports floating-point types"); + + math::Vector3 center{}; + T radius{}; + }; + + template + struct Capsule + { + static_assert(std::is_floating_point_v, "Capsule supports floating-point types"); + + Segment axis{}; + T radius{}; + }; + + template + struct Box + { + static_assert(std::is_floating_point_v, "Box supports floating-point types"); + + math::SE3Transform pose{}; + math::Vector3 halfExtents{}; + }; + + template + struct ClosestPoints + { + static_assert(std::is_floating_point_v, "ClosestPoints supports floating-point types"); + + math::Vector3 onFirst{}; + math::Vector3 onSecond{}; + T distance{}; + }; + + template + struct SegmentPair + { + static_assert(std::is_floating_point_v, "SegmentPair supports floating-point types"); + + math::Vector3 onFirst{}; + math::Vector3 onSecond{}; + T s{}; + T t{}; + T distance{}; + }; + + template + class PrimitiveDistance + { + static_assert(std::is_floating_point_v, "PrimitiveDistance supports floating-point types"); + + public: + using Vector3 = math::Vector3; + + static ClosestPoints PointSegment(const Vector3& point, const Segment& segment); + OPTIMIZE_FOR_SPEED static SegmentPair SegmentSegment(const Segment& first, const Segment& second); + static ClosestPoints PointBox(const Vector3& point, const Box& box); + static ClosestPoints SphereSphere(const Sphere& first, const Sphere& second); + static ClosestPoints SphereCapsule(const Sphere& sphere, const Capsule& capsule); + OPTIMIZE_FOR_SPEED static ClosestPoints CapsuleCapsule(const Capsule& first, const Capsule& second); + static ClosestPoints SphereBox(const Sphere& sphere, const Box& box); + + private: + struct Parameters + { + T s; + T t; + }; + + struct BoxProjection + { + Vector3 surface; + Vector3 outwardNormal; + T distance; + bool inside; + }; + + static constexpr T epsilon{ static_cast(1e-12) }; + + static T Clamp01(T value); + static Parameters SegmentParameters(const Vector3& d1, const Vector3& d2, const Vector3& r); + static Parameters NonDegenerateParameters(T a, T b, T c, T e, T f); + static BoxProjection ProjectOnBox(const Vector3& point, const Box& box); + static BoxProjection ProjectInsideBox(const Vector3& local, const Box& box); + static ClosestPoints Rounded(const Vector3& firstCore, const Vector3& secondCore, T coreDistance, T firstRadius, T secondRadius); + }; + + template + ClosestPoints PrimitiveDistance::PointSegment(const Vector3& point, const Segment& segment) + { + const Vector3 direction{ segment.end - segment.start }; + const T lengthSquared{ math::DotProduct(direction, direction) }; + const T t{ lengthSquared <= epsilon ? T{} : Clamp01(math::DotProduct(point - segment.start, direction) / lengthSquared) }; + const Vector3 closest{ segment.start + direction * t }; + + return { point, closest, math::VectorNorm(Vector3{ point - closest }) }; + } + + template + OPTIMIZE_FOR_SPEED SegmentPair PrimitiveDistance::SegmentSegment(const Segment& first, const Segment& second) + { + const Vector3 d1{ first.end - first.start }; + const Vector3 d2{ second.end - second.start }; + const auto [s, t]{ SegmentParameters(d1, d2, first.start - second.start) }; + const Vector3 onFirst{ first.start + d1 * s }; + const Vector3 onSecond{ second.start + d2 * t }; + + return { onFirst, onSecond, s, t, math::VectorNorm(Vector3{ onSecond - onFirst }) }; + } + + template + ClosestPoints PrimitiveDistance::PointBox(const Vector3& point, const Box& box) + { + const auto projection{ ProjectOnBox(point, box) }; + return { point, projection.surface, projection.distance }; + } + + template + ClosestPoints PrimitiveDistance::SphereSphere(const Sphere& first, const Sphere& second) + { + return Rounded(first.center, second.center, math::VectorNorm(Vector3{ second.center - first.center }), first.radius, second.radius); + } + + template + ClosestPoints PrimitiveDistance::SphereCapsule(const Sphere& sphere, const Capsule& capsule) + { + const auto core{ PointSegment(sphere.center, capsule.axis) }; + return Rounded(sphere.center, core.onSecond, core.distance, sphere.radius, capsule.radius); + } + + template + OPTIMIZE_FOR_SPEED ClosestPoints PrimitiveDistance::CapsuleCapsule(const Capsule& first, const Capsule& second) + { + const auto core{ SegmentSegment(first.axis, second.axis) }; + return Rounded(core.onFirst, core.onSecond, core.distance, first.radius, second.radius); + } + + template + ClosestPoints PrimitiveDistance::SphereBox(const Sphere& sphere, const Box& box) + { + const auto projection{ ProjectOnBox(sphere.center, box) }; + if (!projection.inside) + return Rounded(sphere.center, projection.surface, projection.distance, sphere.radius, T{}); + + return { sphere.center - projection.outwardNormal * sphere.radius, projection.surface, projection.distance - sphere.radius }; + } + + template + T PrimitiveDistance::Clamp01(T value) + { + return std::clamp(value, T{}, T{ 1 }); + } + + template + typename PrimitiveDistance::Parameters PrimitiveDistance::SegmentParameters(const Vector3& d1, const Vector3& d2, const Vector3& r) + { + const T a{ math::DotProduct(d1, d1) }; + const T e{ math::DotProduct(d2, d2) }; + const T f{ math::DotProduct(d2, r) }; + + if (a <= epsilon && e <= epsilon) + return { T{}, T{} }; + if (a <= epsilon) + return { T{}, Clamp01(f / e) }; + + const T c{ math::DotProduct(d1, r) }; + if (e <= epsilon) + return { Clamp01(-c / a), T{} }; + + return NonDegenerateParameters(a, math::DotProduct(d1, d2), c, e, f); + } + + template + typename PrimitiveDistance::Parameters PrimitiveDistance::NonDegenerateParameters(T a, T b, T c, T e, T f) + { + const T denominator{ a * e - b * b }; + const T s{ denominator > epsilon ? Clamp01((b * f - c * e) / denominator) : T{} }; + const T t{ (b * s + f) / e }; + + if (t < T{}) + return { Clamp01(-c / a), T{} }; + if (t > T{ 1 }) + return { Clamp01((b - c) / a), T{ 1 } }; + + return { s, t }; + } + + template + typename PrimitiveDistance::BoxProjection PrimitiveDistance::ProjectOnBox(const Vector3& point, const Box& box) + { + const Vector3 local{ box.pose.rotation.Transpose() * (point - box.pose.translation) }; + + bool inside{ true }; + Vector3 clamped{}; + for (std::size_t i = 0; i < 3; ++i) + { + inside = inside && math::Abs(local[i]) <= box.halfExtents[i]; + clamped[i] = std::clamp(local[i], -box.halfExtents[i], box.halfExtents[i]); + } + + if (inside) + return ProjectInsideBox(local, box); + + return { box.pose.Apply(clamped), Vector3{}, math::VectorNorm(Vector3{ local - clamped }), false }; + } + + template + typename PrimitiveDistance::BoxProjection PrimitiveDistance::ProjectInsideBox(const Vector3& local, const Box& box) + { + std::size_t face{ 0 }; + for (std::size_t i = 1; i < 3; ++i) + if (box.halfExtents[i] - math::Abs(local[i]) < box.halfExtents[face] - math::Abs(local[face])) + face = i; + + const T sign{ local[face] < T{} ? T{ -1 } : T{ 1 } }; + Vector3 surface{ local }; + surface[face] = sign * box.halfExtents[face]; + Vector3 normal{}; + normal[face] = sign; + + return { box.pose.Apply(surface), box.pose.Rotate(normal), math::Abs(local[face]) - box.halfExtents[face], true }; + } + + template + ClosestPoints PrimitiveDistance::Rounded(const Vector3& firstCore, const Vector3& secondCore, T coreDistance, T firstRadius, T secondRadius) + { + const Vector3 normal{ coreDistance > math::Sqrt(epsilon) ? Vector3{ (secondCore - firstCore) * (T{ 1 } / coreDistance) } : Vector3{ T{}, T{}, T{ 1 } } }; + return { firstCore + normal * firstRadius, secondCore - normal * secondRadius, coreDistance - firstRadius - secondRadius }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class PrimitiveDistance; +#endif +} diff --git a/robotics/planning/test/CMakeLists.txt b/robotics/planning/test/CMakeLists.txt new file mode 100644 index 0000000..327bebd --- /dev/null +++ b/robotics/planning/test/CMakeLists.txt @@ -0,0 +1,12 @@ +add_executable(robotics.planning_test) +emil_build_for(robotics.planning_test BOOL ROBOTICS_TOOLBOX_BUILD_TESTS) +emil_add_test(robotics.planning_test) + +target_link_libraries(robotics.planning_test PUBLIC + gmock_main + robotics.planning +) + +target_sources(robotics.planning_test PRIVATE + TestPrimitiveDistance.cpp +) diff --git a/robotics/planning/test/TestPrimitiveDistance.cpp b/robotics/planning/test/TestPrimitiveDistance.cpp new file mode 100644 index 0000000..dd67229 --- /dev/null +++ b/robotics/planning/test/TestPrimitiveDistance.cpp @@ -0,0 +1,118 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Tolerance.hpp" +#include "robotics/planning/PrimitiveDistance.hpp" +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Distance = planning::PrimitiveDistance; + using Segment = planning::Segment; + using Sphere = planning::Sphere; + using Capsule = planning::Capsule; + + class TestPrimitiveDistance + : public ::testing::Test + { + protected: + void ExpectNear(const Vector3& actual, const Vector3& expected) const + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], math::Tolerance()); + } + + planning::Box box{ { math::RotationAboutAxis(Vector3{ 0.0f, 0.0f, 1.0f }, std::numbers::pi_v / 6.0f), Vector3{ 1.0f, 0.0f, 0.0f } }, + Vector3{ 0.5f, 0.25f, 0.1f } }; + }; +} + +TEST_F(TestPrimitiveDistance, point_segment_projects_and_clamps) +{ + const Segment segment{ Vector3{}, Vector3{ 2.0f, 0.0f, 0.0f } }; + + const auto interior{ Distance::PointSegment(Vector3{ 1.0f, 1.0f, 0.0f }, segment) }; + const auto clamped{ Distance::PointSegment(Vector3{ -1.0f, 1.0f, 0.0f }, segment) }; + + ExpectNear(interior.onSecond, Vector3{ 1.0f, 0.0f, 0.0f }); + EXPECT_NEAR(interior.distance, 1.0f, math::Tolerance()); + ExpectNear(clamped.onSecond, Vector3{}); + EXPECT_NEAR(clamped.distance, std::sqrt(2.0f), math::Tolerance()); +} + +TEST_F(TestPrimitiveDistance, segment_segment_matches_brute_force) +{ + const auto general{ Distance::SegmentSegment({ Vector3{}, Vector3{ 1.0f, 2.0f, 3.0f } }, { Vector3{ 2.0f, 0.0f, 1.0f }, Vector3{ 0.0f, 1.0f, -1.0f } }) }; + const auto skew{ Distance::SegmentSegment({ Vector3{}, Vector3{ 1.0f, 0.0f, 0.0f } }, { Vector3{ 0.5f, -1.0f, 1.0f }, Vector3{ 0.5f, 1.0f, 1.0f } }) }; + + EXPECT_NEAR(general.s, 0.1f, math::Tolerance()); + EXPECT_NEAR(general.t, 0.6f, math::Tolerance()); + ExpectNear(general.onFirst, Vector3{ 0.1f, 0.2f, 0.3f }); + ExpectNear(general.onSecond, Vector3{ 0.8f, 0.6f, -0.2f }); + EXPECT_NEAR(general.distance, 0.948683f, math::Tolerance()); + ExpectNear(skew.onFirst, Vector3{ 0.5f, 0.0f, 0.0f }); + ExpectNear(skew.onSecond, Vector3{ 0.5f, 0.0f, 1.0f }); + EXPECT_NEAR(skew.distance, 1.0f, math::Tolerance()); +} + +TEST_F(TestPrimitiveDistance, segment_segment_handles_parallel_and_degenerate_inputs) +{ + const auto parallel{ Distance::SegmentSegment({ Vector3{}, Vector3{ 2.0f, 0.0f, 0.0f } }, { Vector3{ 1.0f, 1.0f, 0.0f }, Vector3{ 3.0f, 1.0f, 0.0f } }) }; + const auto endpoints{ Distance::SegmentSegment({ Vector3{}, Vector3{ 1.0f, 0.0f, 0.0f } }, { Vector3{ 2.0f, 1.0f, 0.0f }, Vector3{ 3.0f, 2.0f, 0.0f } }) }; + const auto pointPoint{ Distance::SegmentSegment({ Vector3{}, Vector3{} }, { Vector3{ 1.0f, 1.0f, 1.0f }, Vector3{ 1.0f, 1.0f, 1.0f } }) }; + const auto pointSegment{ Distance::SegmentSegment({ Vector3{ 0.0f, 0.0f, 1.0f }, Vector3{ 0.0f, 0.0f, 1.0f } }, { Vector3{ -1.0f, 0.0f, 0.0f }, Vector3{ 1.0f, 0.0f, 0.0f } }) }; + const auto segmentPoint{ Distance::SegmentSegment({ Vector3{ -1.0f, 0.0f, 0.0f }, Vector3{ 1.0f, 0.0f, 0.0f } }, { Vector3{ 0.0f, 0.0f, 1.0f }, Vector3{ 0.0f, 0.0f, 1.0f } }) }; + + EXPECT_NEAR(parallel.distance, 1.0f, math::Tolerance()); + EXPECT_NEAR(endpoints.s, 1.0f, math::Tolerance()); + EXPECT_NEAR(endpoints.t, 0.0f, math::Tolerance()); + EXPECT_NEAR(endpoints.distance, std::sqrt(2.0f), math::Tolerance()); + EXPECT_NEAR(pointPoint.distance, std::sqrt(3.0f), math::Tolerance()); + EXPECT_NEAR(pointSegment.distance, 1.0f, math::Tolerance()); + EXPECT_NEAR(segmentPoint.distance, 1.0f, math::Tolerance()); +} + +TEST_F(TestPrimitiveDistance, point_box_is_signed) +{ + const auto outside{ Distance::PointBox(Vector3{ 2.0f, 1.0f, 0.0f }, box) }; + const auto inside{ Distance::PointBox(Vector3{ 1.05f, 0.0f, 0.02f }, box) }; + + ExpectNear(outside.onSecond, Vector3{ 1.308013f, 0.466506f, 0.0f }); + EXPECT_NEAR(outside.distance, 0.873763f, math::Tolerance()); + ExpectNear(inside.onSecond, Vector3{ 1.05f, 0.0f, 0.1f }); + EXPECT_NEAR(inside.distance, -0.08f, math::Tolerance()); +} + +TEST_F(TestPrimitiveDistance, rounded_pairs_subtract_the_radii) +{ + const auto spheres{ Distance::SphereSphere({ Vector3{}, 1.0f }, { Vector3{ 3.0f, 1.0f, 0.0f }, 0.5f }) }; + const Capsule first{ { Vector3{}, Vector3{ 1.0f, 0.0f, 0.0f } }, 0.2f }; + const auto apart{ Distance::CapsuleCapsule(first, { { Vector3{ 0.5f, -1.0f, 1.0f }, Vector3{ 0.5f, 1.0f, 1.0f } }, 0.3f }) }; + const auto overlapping{ Distance::CapsuleCapsule(first, { { Vector3{ 0.5f, -1.0f, 0.4f }, Vector3{ 0.5f, 1.0f, 0.4f } }, 0.3f }) }; + const auto sphereCapsule{ Distance::SphereCapsule({ Vector3{ 1.0f, 1.0f, 0.0f }, 0.25f }, { { Vector3{}, Vector3{ 2.0f, 0.0f, 0.0f } }, 0.25f }) }; + const auto sphereBox{ Distance::SphereBox({ Vector3{ 2.0f, 1.0f, 0.0f }, 0.3f }, box) }; + + EXPECT_NEAR(spheres.distance, std::sqrt(10.0f) - 1.5f, math::Tolerance()); + ExpectNear(spheres.onFirst, Vector3{ 0.948683f, 0.316228f, 0.0f }); + EXPECT_NEAR(apart.distance, 0.5f, math::Tolerance()); + EXPECT_NEAR(overlapping.distance, -0.1f, math::Tolerance()); + EXPECT_NEAR(sphereCapsule.distance, 0.5f, math::Tolerance()); + EXPECT_NEAR(sphereBox.distance, 0.573763f, math::Tolerance()); +} + +TEST_F(TestPrimitiveDistance, sphere_inside_a_box_reports_depth) +{ + const auto result{ Distance::SphereBox({ Vector3{ 1.05f, 0.0f, 0.02f }, 0.05f }, box) }; + + EXPECT_NEAR(result.distance, -0.13f, math::Tolerance()); +} + +TEST_F(TestPrimitiveDistance, coincident_cores_still_return_a_normal) +{ + const auto result{ Distance::SphereSphere({ Vector3{ 1.0f, 1.0f, 1.0f }, 0.2f }, { Vector3{ 1.0f, 1.0f, 1.0f }, 0.3f }) }; + + EXPECT_NEAR(result.distance, -0.5f, math::Tolerance()); + ExpectNear(result.onFirst, Vector3{ 1.0f, 1.0f, 1.2f }); + ExpectNear(result.onSecond, Vector3{ 1.0f, 1.0f, 0.7f }); +} From f914ff8acdefca4fe91f6d74a9ed68cf39e8ecc8 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 09:54:02 +0000 Subject: [PATCH 4/7] feat(controllers): add PdGravityCompensation (M5) Globally stable set-point regulator tau = Kp e - Kd qdot + g(q) on the shipped EulerLagrangeDynamics interface. Creates the controllers/manipulator module and a MockEulerLagrangeDynamics test double. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 13 +- ROADMAP.md | 5 - .../manipulator/PdGravityCompensation.md | 95 +++++++++++ doc/controllers/manipulator/README.md | 9 ++ roadmap/README.md | 4 +- .../PdGravityCompensation/explanation.md | 32 ---- .../PdGravityCompensation/implementation.md | 76 --------- .../PdGravityCompensation/tests.md | 64 -------- robotics/CMakeLists.txt | 1 + robotics/controllers/CMakeLists.txt | 1 + .../controllers/manipulator/CMakeLists.txt | 21 +++ .../manipulator/PdGravityCompensation.cpp | 7 + .../manipulator/PdGravityCompensation.hpp | 48 ++++++ .../manipulator/test/CMakeLists.txt | 13 ++ .../test/TestPdGravityCompensation.cpp | 152 ++++++++++++++++++ robotics/dynamics/CMakeLists.txt | 1 + robotics/dynamics/test_doubles/CMakeLists.txt | 11 ++ .../MockEulerLagrangeDynamics.hpp | 20 +++ 18 files changed, 388 insertions(+), 185 deletions(-) create mode 100644 doc/controllers/manipulator/PdGravityCompensation.md create mode 100644 doc/controllers/manipulator/README.md delete mode 100644 roadmap/controllers/manipulator/PdGravityCompensation/explanation.md delete mode 100644 roadmap/controllers/manipulator/PdGravityCompensation/implementation.md delete mode 100644 roadmap/controllers/manipulator/PdGravityCompensation/tests.md create mode 100644 robotics/controllers/CMakeLists.txt create mode 100644 robotics/controllers/manipulator/CMakeLists.txt create mode 100644 robotics/controllers/manipulator/PdGravityCompensation.cpp create mode 100644 robotics/controllers/manipulator/PdGravityCompensation.hpp create mode 100644 robotics/controllers/manipulator/test/CMakeLists.txt create mode 100644 robotics/controllers/manipulator/test/TestPdGravityCompensation.cpp create mode 100644 robotics/dynamics/test_doubles/CMakeLists.txt create mode 100644 robotics/dynamics/test_doubles/MockEulerLagrangeDynamics.hpp diff --git a/README.md b/README.md index 17daeb2..ea44bc3 100644 --- a/README.md +++ b/README.md @@ -35,12 +35,13 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|----------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Composite Rigid Body Algorithm (CRBA) | -| [Planning](doc/planning/README.md) | Primitive Distance Queries | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory | +| Category | Description | +|--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | +| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Composite Rigid Body Algorithm (CRBA) | +| [Planning](doc/planning/README.md) | Primitive Distance Queries | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory | +| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 13663c9..83aa9e3 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -117,7 +117,6 @@ existing `dynamics/` convention. | # | Component | Target module | Depends on | Difficulty | |-----|-----------------------------------------------------------------|---------------------------|----------------|------------| | M1 | Generic joint link (prismatic, limits, armature) | `dynamics` + `kinematics` | — | ★☆☆☆☆ | -| M5 | PD + gravity compensation control | `controllers/manipulator` | M29 | ★☆☆☆☆ | | M32 | Inverse dynamics with an external tool wrench | `dynamics` | — | ★☆☆☆☆ | | M29 | Chain dynamics model (link chain → M, C, g, inverse dynamics) | `dynamics` | M28 | ★★☆☆☆ | | M30 | Chain pose kinematics (full-pose FK, frame chain) | `kinematics` | NT53 | ★★☆☆☆ | @@ -153,10 +152,6 @@ retired here. **M1. Generic joint link.** Evolve `RevoluteJointLink` (compatibly, via trailing defaulted fields) into a link with a joint type (revolute/prismatic), joint limits and armature, and add the prismatic branches to FK, RNEA, ABA, CRBA and the Jacobian. - *Algorithm / paper:* J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 4th ed., Ch. 3 and 6; Featherstone (2008), Ch. 4. -**M5. PD + gravity compensation control.** The simplest globally-stable set-point regulator: `τ = Kp·e − Kd·q̇ + g(q)`. -- *Algorithm / paper:* M. Takegaki, S. Arimoto, *ASME J. Dyn. Sys. Meas. Control*, 1981. -- *Reuses / builds on:* `g(q)` from M29; optional friction feedforward from NT57. - **M32. Inverse dynamics with an external tool wrench.** RNEA overload taking the environment's wrench on the tool; satisfies `τ(w) − τ(0) = −Jᵀw`. - *Algorithm / paper:* Luh, Walker, Paul (1980). diff --git a/doc/controllers/manipulator/PdGravityCompensation.md b/doc/controllers/manipulator/PdGravityCompensation.md new file mode 100644 index 0000000..6efd6a0 --- /dev/null +++ b/doc/controllers/manipulator/PdGravityCompensation.md @@ -0,0 +1,95 @@ +# PD Control with Gravity Compensation + +## Overview & Motivation + +Holding a manipulator at a commanded pose is the most common joint-control task: pick-and-place approach points, teach-in positions, parking and safe-stop poses. A plain PD loop on each joint behaves like a virtual spring and damper. Under gravity that spring sags, and each loaded joint settles short of its target by the deflection needed to carry the load. + +**PD control with gravity compensation** adds the gravity torque $g(q)$ of the arm as a feed-forward term: + +$$\tau = K_p\,(q_d - q) - K_d\,\dot{q} + g(q)$$ + +Takegaki and Arimoto (1981) proved that this law drives the arm to **any** constant set-point $q_d$ from **any** initial state, for **any** symmetric positive-definite gains. It needs only $g(q)$, not the mass matrix or Coriolis terms, which makes it the cheapest model-based joint controller with a global stability guarantee. On a microcontroller it is a handful of multiply–adds per joint plus one gravity evaluation. + +## Mathematical Theory + +### Closed-Loop Dynamics + +The manipulator obeys $M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau$. With the error $e = q_d - q$ (constant $q_d$, so $\dot{e} = -\dot{q}$), substituting the control law cancels gravity exactly: + +$$M(q)\ddot{q} + C(q, \dot{q})\dot{q} + K_d\,\dot{q} = K_p\,e$$ + +The closed loop is a nonlinear mass–spring–damper with the spring anchored at the set-point. + +### Lyapunov Stability + +Take the total energy of that virtual system as the candidate function: + +$$V(e, \dot{q}) = \tfrac12\,\dot{q}^T M(q)\,\dot{q} + \tfrac12\,e^T K_p\,e$$ + +It is positive definite because $M$ and $K_p$ are. Differentiating along trajectories and using the skew-symmetry of $\dot{M} - 2C$ (a property of the Christoffel form of $C$): + +$$\dot{V} = \dot{q}^T\left(K_p e - C\dot{q} - K_d\dot{q}\right) + \tfrac12\,\dot{q}^T \dot{M}\,\dot{q} - \dot{q}^T K_p\,e = -\dot{q}^T K_d\,\dot{q} \le 0$$ + +$\dot{V}$ vanishes only when $\dot{q} = 0$. On that set the dynamics reduce to $M\ddot{q} = K_p e$, so the motion stays at rest only if $e = 0$. By LaSalle's invariance principle the equilibrium $(e, \dot{q}) = (0, 0)$ is **globally asymptotically stable**. + +### Steady State Without Compensation + +Dropping $g(q)$ leaves the equilibrium condition $K_p e_{ss} = g(q_d - e_{ss})$. To first order, the arm settles with the droop $e_{ss} \approx K_p^{-1} g(q_d)$. Compensation removes this offset without raising the gains, so the loop can stay soft and well damped. + +## Complexity Analysis + +| Case | Time | Space | Notes | +|----------------|------------------------|-------------|----------------------------------------------------------------| +| Full gains | $O(n^2) + T_g$ | $O(n^2)$ | Two gain matrix–vector products plus one gravity evaluation | +| Diagonal gains | $O(n) + T_g$ | $O(n)$ | Joints decouple; $2n$ multiplies plus the gravity evaluation | + +$T_g$ is the cost of the gravity vector: $O(n)$ with a recursive Newton–Euler pass at zero velocity and acceleration, or a closed form for short chains. + +## Step-by-Step Walkthrough + +A two-joint arm with $K_p = \mathrm{diag}(100, 100)$, $K_d = \mathrm{diag}(20, 20)$, $q = (0.3, -0.6)$, $\dot{q} = (0.4, -1.0)$, $q_d = (0.5, -0.2)$ and gravity torque $g(q) = (1.5, -2.5)$ N·m: + +1. Position error $e = q_d - q = (0.2, 0.4)$, so the spring torque is $K_p e = (20, 40)$. +2. Damping torque $-K_d \dot{q} = (-8, 20)$. Joint 2 moving negative is pushed back positive. +3. Gravity feed-forward $(1.5, -2.5)$. +4. Command $\tau = (13.5, 57.5)$ N·m. + +At the set-point at rest, the spring and damping terms vanish and $\tau = g(q_d)$, exactly the holding torque. + +## Pitfalls & Edge Cases + +- **Set-point regulator only**: the stability proof needs constant $q_d$. Tracking a moving reference with this law lags, and the lag grows with speed. Use computed-torque or passivity-based tracking control instead. +- **Model mismatch**: an error $\Delta g$ in the gravity model leaves a steady offset $e_{ss} = K_p^{-1}\Delta g$. Payload changes are the usual source. Integral action or online payload estimation removes it, at the cost of the simple proof. +- **Actuator saturation**: large initial errors command $K_p e$ torques the motors cannot deliver. Saturated PD+g still converges for bounded gravity when the limits exceed $\max|g|$, but overshoot grows. Clamp or ramp the set-point. +- **Velocity noise**: $K_d$ multiplies a differentiated encoder signal. Filter it, or use a velocity observer, before raising $K_d$. +- **Friction**: Coulomb friction creates a dead band of radius $K_p^{-1}\tau_c$ around the set-point. Add friction feed-forward if that matters. +- **Gain choice**: per joint, $K_{d,i} \approx 2\sqrt{K_{p,i} M_{ii}}$ gives roughly critical damping around the operating pose. + +## Variants & Generalizations + +- **Desired gravity compensation**: use $g(q_d)$ instead of $g(q)$. It is cheaper, since it can be precomputed per set-point, and it is still globally stable when $K_p$ exceeds the Lipschitz constant of $g$ (Tomei, 1991). +- **PID + gravity**: integral action rejects constant model errors. Stability becomes semi-global and needs gain conditions. +- **Task-space PD + gravity**: $\tau = J^T(K_p \tilde{x} - K_d \dot{x}) + g(q)$ regulates the end-effector pose instead, the basis of impedance control without inertia shaping. +- **Saturated PD + gravity**: bounded spring functions (e.g. $\tanh$) keep commands within actuator limits while preserving global stability. +- **Energy shaping**: the PD term reshapes the potential energy into a bowl centred at $q_d$, and the damping injects dissipation. This is the starting point of passivity-based control. + +## Applications + +- Holding and positioning tasks: approach poses, parking, teach-in and inspection stops. +- A safe fallback when the full dynamic model is unavailable or untrusted. +- Gravity-compensated "float" or hand-guiding modes, with $K_p = 0$ and small $K_d$. +- Inner loop for set-point sequences generated by a higher-level planner. + +## Connections to Other Algorithms + +- [Euler-Lagrange](../../dynamics/EulerLagrange.md): the controller draws its gravity torque from the Euler–Lagrange model interface. +- [Recursive Newton-Euler](../../dynamics/RecursiveNewtonEuler.md): with zero velocity and acceleration, inverse dynamics returns exactly $g(q)$ in $O(n)$. +- [Composite Rigid Body Algorithm](../../dynamics/CompositeRigidBodyAlgorithm.md): the mass matrix in the Lyapunov function; it shapes the transient but is not needed by the law. +- Friction feed-forward and the generic PID building blocks come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Takegaki, M. and Arimoto, S. (1981). "A New Feedback Method for Dynamic Control of Manipulators." *ASME J. Dynamic Systems, Measurement, and Control*, 103(2), 119–125. +- Tomei, P. (1991). "Adaptive PD Controller for Robot Manipulators." *IEEE Trans. Robotics and Automation*, 7(4), 565–570. +- Spong, M.W., Hutchinson, S. and Vidyasagar, M. (2020). *Robot Modeling and Control*. 2nd ed. Wiley. Chapter 9. +- Kelly, R., Santibáñez, V. and Loría, A. (2005). *Control of Robot Manipulators in Joint Space*. Springer. Chapter 7. diff --git a/doc/controllers/manipulator/README.md b/doc/controllers/manipulator/README.md new file mode 100644 index 0000000..079be7a --- /dev/null +++ b/doc/controllers/manipulator/README.md @@ -0,0 +1,9 @@ +# Manipulator Control + +Joint- and task-space controllers for robot manipulators. + +## Algorithms + +| Algorithm | Description | +|------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------| +| [PD Control with Gravity Compensation](PdGravityCompensation.md) | Globally stable set-point regulator $\tau = K_p e - K_d \dot{q} + g(q)$ — needs only the gravity torque | diff --git a/roadmap/README.md b/roadmap/README.md index d10db42..8658a77 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -23,7 +23,7 @@ All pseudocode respects the library constraints: **no heap**, bounded containers (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. The canonical worked example is -[controllers/manipulator/PdGravityCompensation](controllers/manipulator/PdGravityCompensation/implementation.md). +[controllers/manipulator/ComputedTorqueControl](controllers/manipulator/ComputedTorqueControl/implementation.md). Conventions shared by every spec: 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches `(f; n)` — as fixed by upstream [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md) (NT53), and planar @@ -107,4 +107,4 @@ prerequisites. `OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) ### `controllers/manipulator` -`PdGravityCompensation` (M5) · `ComputedTorqueControl` (M12) · `ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) +`ComputedTorqueControl` (M12) · `ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md b/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md deleted file mode 100644 index 93d3426..0000000 --- a/roadmap/controllers/manipulator/PdGravityCompensation/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# PD + Gravity Compensation — Overview - -## What it is -The simplest globally-stable manipulator set-point regulator: a PD loop on the joint error plus an -exact gravity-cancelling feedforward term — `τ = Kp·e − Kd·q̇ + g(q)`. - -## Why it matters (embedded) -It is the cheapest *model-based* joint controller in robotics. It needs only the gravity vector -`g(q)` — not the full mass matrix or Coriolis terms — so it runs in a handful of flops on a modest -MCU, yet it drives the arm to any pose with **zero steady-state droop** and a proof of global -stability. Ideal for hold/positioning tasks and as a safe fallback controller. - -## How it works (intuition) -The PD term behaves like a virtual spring-damper pulling each joint toward its target and bleeding -off velocity. Left alone, that spring would sag under gravity and settle short of the goal. The -`g(q)` feedforward supplies exactly the torque needed to counter gravity at every configuration, so -the spring no longer fights it and the joint lands precisely on the set-point. A Lyapunov -energy argument (kinetic + spring potential) guarantees the arm always converges. - -## Key parameters -- **model** — injected dynamics object supplying the gravity term `g(q)` (typically `ChainDynamicsModel`, M29). -- **Kp (stiffness)** — how hard the controller pulls toward the target. -- **Kd (damping)** — how strongly it resists velocity; pick `Kd ≈ 2√(Kp·inertia)` for critical damping. - -## Reference -M. Takegaki, S. Arimoto, "A New Feedback Method for Dynamic Control of Manipulators," -*ASME J. Dynamic Systems, Measurement, and Control*, 1981. - -## See also -`ComputedTorqueControl` (full inverse-dynamics tracking); `ImpedanceControl` (compliant contact); -upstream `controllers::FrictionCompensation` (NT57, adds a friction feedforward); `dynamics/EulerLagrangeDynamics` (the injected -interface); `ChainDynamicsModel` (M29, its link-chain implementation). diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md b/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md deleted file mode 100644 index 59a6f26..0000000 --- a/roadmap/controllers/manipulator/PdGravityCompensation/implementation.md +++ /dev/null @@ -1,76 +0,0 @@ -# PD + Gravity Compensation — Implementation Pseudocode - -> Roadmap ref: #M5 (Tier 1) · Target: `robotics/controllers/manipulator` · Namespace `controllers` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class PdGravityCompensation: - const dynamics::EulerLagrangeDynamics& model # only g(q) is queried - math::SquareMatrix Kp # proportional gain (SPD) - math::SquareMatrix Kd # derivative gain (SPD) -``` - -## Interface - -```cpp -# Dynamics model injected (DIP); the set-point regulator needs only its gravity term: -PdGravityCompensation(const EulerLagrangeDynamics& model, - const SquareMatrix& Kp, const SquareMatrix& Kd) - -Vector ComputeTorque(const StateVector& q, - const StateVector& qDot, - const StateVector& qDesired) # hot path -``` - -## Algorithm (pseudocode) - -```text -function ComputeTorque(q, qDot, qd): # OPTIMIZE_FOR_SPEED - # set-point regulation: qd is constant, so the desired velocity is zero - e = qd - q - # τ = Kp·e − Kd·q̇ + g(q) - g = model.ComputeGravityTerms(q) - return Kp * e - Kd * qDot + g -``` - -## Complexity & memory - -- Time: `O(Dof²)` for the two gain products (`O(Dof)` when the gains are diagonal); gravity - evaluation is `O(Dof)`–`O(Dof²)` via the injected RNEA / Euler-Lagrange model. -- Memory: `O(Dof²)` for the two gains; no dynamic state, no heap. - -## Numerical / embedded notes - -- Globally asymptotically stable for *any* SPD `Kp`, `Kd` (Takegaki–Arimoto): the Lyapunov function - `V = ½q̇ᵀM(q)q̇ + ½eᵀKp e` decreases as the `Kd·q̇` term drains kinetic energy. -- The `g(q)` term removes the static "droop"; without it a gravity-loaded joint settles with a - steady offset `e_ss = Kp⁻¹·g`. -- **Set-point regulator only** — `qd` is constant. For trajectory tracking use `ComputedTorqueControl`. -- Diagonal `Kp`, `Kd` decouple the joints and keep the hot path to `Dof` multiplies. -- Only `g(q)` is drawn from the injected `EulerLagrangeDynamics`, typically a `dynamics::ChainDynamicsModel` - (M29), which also implements the `InverseDynamicsModel` used by `ComputedTorqueControl` — one model - object serves both controllers (ISP / DIP). -- The interface is a virtual seam for StrictMock tests; hard real-time builds may template the controller - on the concrete `ChainDynamicsModel` to avoid the virtual call (AGENTS.md: no virtual calls in real-time paths). -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `robotics/controllers/manipulator/PdGravityCompensation.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `ComputeTorque`, and - `extern template class PdGravityCompensation;` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/controllers/manipulator/PdGravityCompensation.cpp` → - `template class PdGravityCompensation;` / `` -- Test: `robotics/controllers/manipulator/test/TestPdGravityCompensation.cpp` -- Doc: `doc/controllers/manipulator/PdGravityCompensation.md` (expand to follow `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; - `TestPdGravityCompensation.cpp` → the `_test` target. -- New module: create `robotics/controllers/manipulator/CMakeLists.txt` via `robotics_add_header_library(...)`, - add a `test/` subdir, register it in `robotics/CMakeLists.txt`, and add a - `doc/controllers/manipulator/` folder. -- Depends on: shipped `dynamics::EulerLagrangeDynamics`; model typically M29 (`ChainDynamicsModel`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/controllers/manipulator/PdGravityCompensation/tests.md b/roadmap/controllers/manipulator/PdGravityCompensation/tests.md deleted file mode 100644 index 8e4136b..0000000 --- a/roadmap/controllers/manipulator/PdGravityCompensation/tests.md +++ /dev/null @@ -1,64 +0,0 @@ -# PD + Gravity Compensation — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestPdGravityCompensation : public ::testing::Test: - # Injected model mocked so torque math is verified in isolation: - StrictMock> model - SquareMatrix Kp = diag(100, 100) - SquareMatrix Kd = diag( 20, 20) - PdGravityCompensation controller{ model, Kp, Kd } -# each case below is a TEST_F(TestPdGravityCompensation, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -gravity_only_at_matched_setpoint: - Arrange: q = qd, qDot = 0, model g(q) = [0, mgL] - Act: τ = ComputeTorque(q, 0, qd) - Assert: τ == g (pure holding torque, no PD contribution) - -proportional_term_on_position_error: - Arrange: e = qd - q != 0, qDot = 0, g = 0 - Assert: τ == Kp · e - -derivative_term_damps_velocity: - Arrange: qDot != 0, e = 0, g = 0 - Assert: τ == -Kd · qDot - -superposition_of_all_terms: - Arrange: e != 0, qDot != 0, g != 0 - Assert: τ == Kp·e - Kd·qDot + g - -diagonal_gains_decouple_joints: - Arrange: error only in joint 0, g = 0 - Assert: torque appears only in joint 0 - -zero_error_zero_velocity_holds_gravity: - Arrange: e = 0, qDot = 0 - Assert: τ == g (equilibrium holding torque) - -only_gravity_method_queried: - Arrange: any state - Assert: ComputeGravityTerms called once; ComputeMassMatrix / ComputeCoriolisTerms - never called (StrictMock enforces) - -regulates_to_setpoint_closed_loop: - Arrange: wrap plant q̈ = M⁻¹(τ − Cq̇ − g); run K steps from q0 != qd - Assert: q -> qd, ||e|| -> 0 with zero steady-state error -``` - -## Reference vectors - -- Single-pendulum hold: `g(q) = m g L sin(q)`; at `qd` with `e = 0`, `q̇ = 0` ⇒ `τ = m g L sin(qd)`. -- Unique closed-loop equilibrium at `e = 0` (SPD `Kp` invertible), independent of `Kd`. - -## Edge cases - -- Large initial error: `τ` bounded by `Kp·e_max + g_max` (external actuator clamp documented). -- `Kp → 0`: convergence slows but stays stable while `Kp` remains SPD. -- Gravity model mismatch (mock `g` scaled 1.1): bounded steady droop `e_ss = Kp⁻¹·Δg`. diff --git a/robotics/CMakeLists.txt b/robotics/CMakeLists.txt index e228377..202fc50 100644 --- a/robotics/CMakeLists.txt +++ b/robotics/CMakeLists.txt @@ -1,3 +1,4 @@ +add_subdirectory(controllers) add_subdirectory(dynamics) add_subdirectory(kinematics) add_subdirectory(planning) diff --git a/robotics/controllers/CMakeLists.txt b/robotics/controllers/CMakeLists.txt new file mode 100644 index 0000000..c3e5341 --- /dev/null +++ b/robotics/controllers/CMakeLists.txt @@ -0,0 +1 @@ +add_subdirectory(manipulator) diff --git a/robotics/controllers/manipulator/CMakeLists.txt b/robotics/controllers/manipulator/CMakeLists.txt new file mode 100644 index 0000000..605c2a8 --- /dev/null +++ b/robotics/controllers/manipulator/CMakeLists.txt @@ -0,0 +1,21 @@ +robotics_add_header_library(robotics.controllers.manipulator) + +target_include_directories(robotics.controllers.manipulator ${ROBOTICS_VISIBILITY} + "$" + "$" +) + +target_link_libraries(robotics.controllers.manipulator ${ROBOTICS_VISIBILITY} + numerical.math + robotics.dynamics +) + +target_sources(robotics.controllers.manipulator PRIVATE + PdGravityCompensation.hpp +) + +robotics_add_coverage_sources(robotics.controllers.manipulator + PdGravityCompensation.cpp +) + +add_subdirectory(test) diff --git a/robotics/controllers/manipulator/PdGravityCompensation.cpp b/robotics/controllers/manipulator/PdGravityCompensation.cpp new file mode 100644 index 0000000..e462ca2 --- /dev/null +++ b/robotics/controllers/manipulator/PdGravityCompensation.cpp @@ -0,0 +1,7 @@ +#include "robotics/controllers/manipulator/PdGravityCompensation.hpp" + +namespace controllers +{ + template class PdGravityCompensation; + template class PdGravityCompensation; +} diff --git a/robotics/controllers/manipulator/PdGravityCompensation.hpp b/robotics/controllers/manipulator/PdGravityCompensation.hpp new file mode 100644 index 0000000..51ed845 --- /dev/null +++ b/robotics/controllers/manipulator/PdGravityCompensation.hpp @@ -0,0 +1,48 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "robotics/dynamics/EulerLagrangeDynamics.hpp" +#include +#include + +namespace controllers +{ + template + class PdGravityCompensation + { + static_assert(std::is_floating_point_v, "PdGravityCompensation supports floating-point types"); + static_assert(Dof > 0, "Degrees of freedom must be positive"); + + public: + using StateVector = math::Vector; + using GainMatrix = math::SquareMatrix; + + PdGravityCompensation(const dynamics::EulerLagrangeDynamics& model, const GainMatrix& kp, const GainMatrix& kd); + + OPTIMIZE_FOR_SPEED StateVector ComputeTorque(const StateVector& q, const StateVector& qDot, const StateVector& qDesired) const; + + private: + const dynamics::EulerLagrangeDynamics& model; + GainMatrix kp; + GainMatrix kd; + }; + + template + PdGravityCompensation::PdGravityCompensation(const dynamics::EulerLagrangeDynamics& model, const GainMatrix& kp, const GainMatrix& kd) + : model{ model } + , kp{ kp } + , kd{ kd } + {} + + template + OPTIMIZE_FOR_SPEED typename PdGravityCompensation::StateVector PdGravityCompensation::ComputeTorque(const StateVector& q, const StateVector& qDot, const StateVector& qDesired) const + { + return kp * StateVector{ qDesired - q } - kd * qDot + model.ComputeGravityTerms(q); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class PdGravityCompensation; + extern template class PdGravityCompensation; +#endif +} diff --git a/robotics/controllers/manipulator/test/CMakeLists.txt b/robotics/controllers/manipulator/test/CMakeLists.txt new file mode 100644 index 0000000..e6b4d63 --- /dev/null +++ b/robotics/controllers/manipulator/test/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(robotics.controllers.manipulator_test) +emil_build_for(robotics.controllers.manipulator_test BOOL ROBOTICS_TOOLBOX_BUILD_TESTS) +emil_add_test(robotics.controllers.manipulator_test) + +target_link_libraries(robotics.controllers.manipulator_test PUBLIC + gmock_main + robotics.controllers.manipulator + robotics.dynamics_test_doubles +) + +target_sources(robotics.controllers.manipulator_test PRIVATE + TestPdGravityCompensation.cpp +) diff --git a/robotics/controllers/manipulator/test/TestPdGravityCompensation.cpp b/robotics/controllers/manipulator/test/TestPdGravityCompensation.cpp new file mode 100644 index 0000000..a19296b --- /dev/null +++ b/robotics/controllers/manipulator/test/TestPdGravityCompensation.cpp @@ -0,0 +1,152 @@ +#include "numerical/math/Tolerance.hpp" +#include "robotics/controllers/manipulator/PdGravityCompensation.hpp" +#include "robotics/dynamics/EulerLagrangeSolver.hpp" +#include "robotics/dynamics/test_doubles/MockEulerLagrangeDynamics.hpp" +#include +#include +#include + +namespace +{ + using Vector2 = math::Vector; + using Matrix2 = math::SquareMatrix; + + class VerticalTwoLinkArm + : public dynamics::EulerLagrangeDynamics + { + public: + MassMatrix ComputeMassMatrix(const StateVector& q) const override + { + const float c2{ std::cos(q[1]) }; + const float m11{ inertia + inertia + mass * lc * lc + mass * (length * length + lc * lc + 2.0f * length * lc * c2) }; + const float m12{ inertia + mass * (lc * lc + length * lc * c2) }; + return MassMatrix{ { m11, m12 }, { m12, inertia + mass * lc * lc } }; + } + + StateVector ComputeCoriolisTerms(const StateVector& q, const StateVector& qDot) const override + { + const float h{ mass * length * lc * std::sin(q[1]) }; + return StateVector{ -h * (2.0f * qDot[0] * qDot[1] + qDot[1] * qDot[1]), h * qDot[0] * qDot[0] }; + } + + StateVector ComputeGravityTerms(const StateVector& q) const override + { + const float distal{ mass * lc * gravity * std::cos(q[0] + q[1]) }; + return StateVector{ (mass * lc + mass * length) * gravity * std::cos(q[0]) + distal, distal }; + } + + private: + static constexpr float mass{ 1.0f }; + static constexpr float length{ 1.0f }; + static constexpr float lc{ 0.5f }; + static constexpr float inertia{ mass * length * length / 12.0f }; + static constexpr float gravity{ 9.81f }; + }; + + class TestPdGravityCompensation + : public ::testing::Test + { + protected: + void ExpectGravity(const Vector2& g) + { + EXPECT_CALL(model, ComputeGravityTerms(testing::_)).WillOnce(testing::Return(g)); + } + + void ExpectNear(const Vector2& actual, const Vector2& expected) const + { + EXPECT_NEAR(actual[0], expected[0], math::Tolerance()); + EXPECT_NEAR(actual[1], expected[1], math::Tolerance()); + } + + testing::StrictMock> model; + Matrix2 kp{ { 100.0f, 0.0f }, { 0.0f, 100.0f } }; + Matrix2 kd{ { 20.0f, 0.0f }, { 0.0f, 20.0f } }; + controllers::PdGravityCompensation controller{ model, kp, kd }; + Vector2 q{ 0.3f, -0.6f }; + Vector2 qDesired{ 0.5f, -0.2f }; + Vector2 qDot{ 0.4f, -1.0f }; + }; +} + +TEST_F(TestPdGravityCompensation, gravity_only_at_matched_setpoint) +{ + const Vector2 g{ 0.0f, 4.905f }; + ExpectGravity(g); + + ExpectNear(controller.ComputeTorque(q, Vector2{}, q), g); +} + +TEST_F(TestPdGravityCompensation, proportional_term_on_position_error) +{ + ExpectGravity(Vector2{}); + + ExpectNear(controller.ComputeTorque(q, Vector2{}, qDesired), Vector2{ 20.0f, 40.0f }); +} + +TEST_F(TestPdGravityCompensation, derivative_term_damps_velocity) +{ + ExpectGravity(Vector2{}); + + ExpectNear(controller.ComputeTorque(q, qDot, q), Vector2{ -8.0f, 20.0f }); +} + +TEST_F(TestPdGravityCompensation, superposition_of_all_terms) +{ + ExpectGravity(Vector2{ 1.5f, -2.5f }); + + ExpectNear(controller.ComputeTorque(q, qDot, qDesired), Vector2{ 20.0f - 8.0f + 1.5f, 40.0f + 20.0f - 2.5f }); +} + +TEST_F(TestPdGravityCompensation, diagonal_gains_decouple_joints) +{ + ExpectGravity(Vector2{}); + + const auto tau{ controller.ComputeTorque(q, Vector2{}, Vector2{ q[0] + 0.1f, q[1] }) }; + + EXPECT_NEAR(tau[0], 10.0f, math::Tolerance()); + EXPECT_NEAR(tau[1], 0.0f, math::Tolerance()); +} + +TEST_F(TestPdGravityCompensation, zero_error_zero_velocity_holds_gravity) +{ + const float mgL{ 1.0f * 9.81f * 0.5f }; + const Vector2 g{ mgL * std::sin(qDesired[0]), mgL * std::sin(qDesired[1]) }; + ExpectGravity(g); + + ExpectNear(controller.ComputeTorque(qDesired, Vector2{}, qDesired), g); +} + +TEST_F(TestPdGravityCompensation, only_gravity_method_queried) +{ + EXPECT_CALL(model, ComputeGravityTerms(testing::Truly([this](const Vector2& argument) + { + return argument[0] == q[0] && argument[1] == q[1]; + }))) + .Times(1) + .WillOnce(testing::Return(Vector2{})); + + controller.ComputeTorque(q, qDot, qDesired); +} + +TEST_F(TestPdGravityCompensation, regulates_to_setpoint_closed_loop) +{ + const VerticalTwoLinkArm plant; + const dynamics::EulerLagrangeSolver solver; + const float dt{ 1e-3f }; + EXPECT_CALL(model, ComputeGravityTerms(testing::_)).WillRepeatedly([&plant](const Vector2& state) + { + return plant.ComputeGravityTerms(state); + }); + + Vector2 position{ -0.4f, 0.8f }; + Vector2 velocity{}; + for (std::size_t step = 0; step < 6000; ++step) + { + const auto tau{ controller.ComputeTorque(position, velocity, qDesired) }; + velocity = velocity + solver.ForwardDynamics(plant, position, velocity, tau) * dt; + position = position + velocity * dt; + } + + ExpectNear(position, qDesired); + ExpectNear(velocity, Vector2{}); +} diff --git a/robotics/dynamics/CMakeLists.txt b/robotics/dynamics/CMakeLists.txt index d2c5df8..2dee4c9 100644 --- a/robotics/dynamics/CMakeLists.txt +++ b/robotics/dynamics/CMakeLists.txt @@ -32,3 +32,4 @@ robotics_add_coverage_sources(robotics.dynamics ) add_subdirectory(test) +add_subdirectory(test_doubles) diff --git a/robotics/dynamics/test_doubles/CMakeLists.txt b/robotics/dynamics/test_doubles/CMakeLists.txt new file mode 100644 index 0000000..e02acfa --- /dev/null +++ b/robotics/dynamics/test_doubles/CMakeLists.txt @@ -0,0 +1,11 @@ +add_library(robotics.dynamics_test_doubles INTERFACE) +emil_build_for(robotics.dynamics_test_doubles BOOL ROBOTICS_TOOLBOX_BUILD_TESTS) + +target_link_libraries(robotics.dynamics_test_doubles INTERFACE + gmock + robotics.dynamics +) + +target_sources(robotics.dynamics_test_doubles PRIVATE + MockEulerLagrangeDynamics.hpp +) diff --git a/robotics/dynamics/test_doubles/MockEulerLagrangeDynamics.hpp b/robotics/dynamics/test_doubles/MockEulerLagrangeDynamics.hpp new file mode 100644 index 0000000..461510a --- /dev/null +++ b/robotics/dynamics/test_doubles/MockEulerLagrangeDynamics.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include "robotics/dynamics/EulerLagrangeDynamics.hpp" +#include + +namespace dynamics +{ + template + class MockEulerLagrangeDynamics + : public EulerLagrangeDynamics + { + public: + using typename EulerLagrangeDynamics::StateVector; + using typename EulerLagrangeDynamics::MassMatrix; + + MOCK_METHOD(MassMatrix, ComputeMassMatrix, (const StateVector& q), (const, override)); + MOCK_METHOD(StateVector, ComputeCoriolisTerms, (const StateVector& q, const StateVector& qDot), (const, override)); + MOCK_METHOD(StateVector, ComputeGravityTerms, (const StateVector& q), (const, override)); + }; +} From 736a5aade63aa482e48934fa2f0a18a0af335c15 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 09:58:22 +0000 Subject: [PATCH 5/7] feat(dynamics): add ForwardDynamicsIntegrator (M34) Fixed-step simulation step for a link chain: ABA exposed as upstream SecondOrderSystem/OdeSystem and advanced by SemiImplicitEuler (NT58) or RungeKutta4, with the step-start acceleration logged. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 14 +- ROADMAP.md | 8 +- doc/dynamics/ForwardDynamicsIntegrator.md | 96 ++++++++++++ doc/dynamics/README.md | 15 +- roadmap/README.md | 2 +- .../ForwardDynamicsIntegrator/explanation.md | 28 ---- .../implementation.md | 79 ---------- .../ForwardDynamicsIntegrator/tests.md | 39 ----- robotics/dynamics/CMakeLists.txt | 2 + .../dynamics/ForwardDynamicsIntegrator.cpp | 9 ++ .../dynamics/ForwardDynamicsIntegrator.hpp | 137 ++++++++++++++++++ robotics/dynamics/test/CMakeLists.txt | 1 + .../test/TestForwardDynamicsIntegrator.cpp | 130 +++++++++++++++++ 13 files changed, 393 insertions(+), 167 deletions(-) create mode 100644 doc/dynamics/ForwardDynamicsIntegrator.md delete mode 100644 roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md delete mode 100644 roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md delete mode 100644 roadmap/dynamics/ForwardDynamicsIntegrator/tests.md create mode 100644 robotics/dynamics/ForwardDynamicsIntegrator.cpp create mode 100644 robotics/dynamics/ForwardDynamicsIntegrator.hpp create mode 100644 robotics/dynamics/test/TestForwardDynamicsIntegrator.cpp diff --git a/README.md b/README.md index ea44bc3..d8f185a 100644 --- a/README.md +++ b/README.md @@ -35,13 +35,13 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Composite Rigid Body Algorithm (CRBA) | -| [Planning](doc/planning/README.md) | Primitive Distance Queries | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory | -| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation | +| Category | Description | +|--------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Inverse Kinematics (Damped Least Squares) | +| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Composite Rigid Body Algorithm (CRBA), Forward-Dynamics Integration | +| [Planning](doc/planning/README.md) | Primitive Distance Queries | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory | +| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 83aa9e3..4f3c029 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -92,7 +92,8 @@ docs; the current pin (`63c9b91`) includes them. The library already has the core of manipulator **modelling**: forward/inverse dynamics ([RNEA](robotics/dynamics/RecursiveNewtonEuler.hpp), [ABA](robotics/dynamics/ArticulatedBodyAlgorithm.hpp), [Euler-Lagrange](robotics/dynamics/EulerLagrangeSolver.hpp)), the joint-space mass matrix -([CRBA](robotics/dynamics/CompositeRigidBodyAlgorithm.hpp)), position forward kinematics with base and +([CRBA](robotics/dynamics/CompositeRigidBodyAlgorithm.hpp)), a fixed-step simulation step +([ForwardDynamicsIntegrator](robotics/dynamics/ForwardDynamicsIntegrator.hpp)), position forward kinematics with base and tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). What is missing is the **full-pose kinematics, model plumbing, planning and control layer** that turns those models into a usable manipulator stack. Two current limitations gate many items below: @@ -125,7 +126,6 @@ existing `dynamics/` convention. | M10 | Cartesian path + orientation (SLERP) interpolation | `trajectory` (new) | NT53, NT55 | ★★☆☆☆ | | M11 | Manipulability ellipsoid / Yoshikawa index | `kinematics` | M8 | ★★☆☆☆ | | M14 | Redundancy resolution / null-space projection | `kinematics` | M8, NT62 | ★★☆☆☆ | -| M34 | Forward-dynamics integrator (simulation step) | `dynamics` | NT58 | ★★☆☆☆ | | M12 | Computed-torque (inverse-dynamics) control | `controllers/manipulator` | M29 | ★★★☆☆ | | M13 | Full 6-DOF pose IK (position + orientation) | `kinematics` | NT53, NT64, M8 | ★★★☆☆ | | M15 | Product-of-Exponentials forward kinematics | `kinematics` | NT53 | ★★★☆☆ | @@ -180,10 +180,6 @@ retired here. - *Algorithm / paper:* A. Liégeois, *IEEE Trans. SMC*, 7(12), 1977. - *Reuses / builds on:* upstream `solvers::DampedPseudoInverse` (NT62). -**M34. Forward-dynamics integrator.** Fixed-step semi-implicit Euler / RK4 simulation step on ABA for model-in-the-loop tests. -- *Algorithm / paper:* Hairer, Lubich, Wanner (2006); Featherstone (2008), Ch. 7. -- *Reuses / builds on:* upstream `solvers::SemiImplicitEuler` (NT58) and `solvers::RungeKutta4`. - ### Tier 3 — Moderate ★★★☆☆ **M12. Computed-torque (inverse-dynamics) control.** `τ = ID(q, q̇, q̈_d + Kd·ė + Kp·e)` in one `O(n)` call, yielding decoupled error dynamics. diff --git a/doc/dynamics/ForwardDynamicsIntegrator.md b/doc/dynamics/ForwardDynamicsIntegrator.md new file mode 100644 index 0000000..1d8ae80 --- /dev/null +++ b/doc/dynamics/ForwardDynamicsIntegrator.md @@ -0,0 +1,96 @@ +# Forward-Dynamics Integration + +## Overview & Motivation + +A controller should meet a simulated plant before it meets hardware. Simulating a manipulator means advancing its state $(q, \dot{q})$ in time under applied joint torques $\tau$: + +$$\ddot{q} = \mathrm{FD}(q, \dot{q}, \tau), \qquad \frac{d}{dt}\begin{bmatrix} q \\ \dot{q} \end{bmatrix} = \begin{bmatrix} \dot{q} \\ \mathrm{FD}(q, \dot{q}, \tau) \end{bmatrix}$$ + +The [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) evaluates $\mathrm{FD}$ in $O(n)$. What remains is a **time-stepping rule**. The choice matters more than it looks. A naive explicit Euler step pumps energy into an undamped pendulum until it spins over the top. A good rule keeps the simulated energy honest over long runs, or reaches high accuracy with few steps. + +This component pairs the articulated-body dynamics with two fixed-step integrators. **Semi-implicit (symplectic) Euler** is cheap and keeps the energy error bounded. **Classical fourth-order Runge–Kutta (RK4)** is accurate per step. Both are deterministic and allocation-free, so the same step function serves unit tests, host simulators and hardware-in-the-loop rigs. + +## Mathematical Theory + +### Zero-Order Hold + +Digital controllers update $\tau$ once per sample period $h$ and hold it in between. The integrator therefore treats $\tau$ as constant over each step. The system is time-invariant, so the step only depends on the state and the held input. + +### Semi-Implicit Euler + +$$\dot{q}_{k+1} = \dot{q}_k + h\,\mathrm{FD}(q_k, \dot{q}_k, \tau_k), \qquad q_{k+1} = q_k + h\,\dot{q}_{k+1}$$ + +The velocity is updated first and the **new** velocity moves the position. For separable mechanical systems (velocity-independent forces) the map is **symplectic**: it preserves phase-space area. By backward error analysis, it exactly conserves a modified energy $\tilde{H} = H + O(h)$. The true energy error therefore oscillates within an $O(h)$ band for exponentially long times instead of drifting (Hairer, Lubich, Wanner). The method is first-order accurate and costs one dynamics evaluation per step. + +Velocity-dependent terms of a manipulator (Coriolis, centrifugal) break exact separability. In practice the energy error stays bounded for the step sizes used in control simulation. + +### Runge–Kutta 4 + +On the first-order state $x = (q; \dot{q})$ with $f(x) = (\dot{q}; \mathrm{FD}(q, \dot{q}, \tau))$: + +$$\begin{aligned} +k_1 &= f(x_k), & k_2 &= f\!\left(x_k + \tfrac{h}{2}k_1\right), & k_3 &= f\!\left(x_k + \tfrac{h}{2}k_2\right), & k_4 &= f(x_k + h\,k_3) \\ +x_{k+1} &= x_k + \tfrac{h}{6}\left(k_1 + 2k_2 + 2k_3 + k_4\right) +\end{aligned}$$ + +The local error is $O(h^5)$ and the global error $O(h^4)$, at four dynamics evaluations per step. RK4 is not symplectic, so energy slowly drifts (usually dissipates) over long horizons, but at fine steps the drift is far below the float resolution of a typical run. + +### Logged Acceleration + +Each step also reports $\ddot{q}_k = \mathrm{FD}(q_k, \dot{q}_k, \tau_k)$ at the start of the step. This is useful for logging, for cross-checking with inverse dynamics, and for acceleration feed-forward in model-in-the-loop tests. + +## Complexity Analysis + +| Method | Dynamics calls / step | Time / step | Space | Accuracy (global) | +|---------------------|-----------------------|-------------|----------|-------------------| +| Semi-implicit Euler | 1 (+1 logged) | $O(n)$ | $O(n)$ | $O(h)$, bounded energy error | +| Runge–Kutta 4 | 4 (+1 logged) | $O(n)$ | $O(n)$ | $O(h^4)$, slow energy drift | + +The logged acceleration costs one extra evaluation. Where cycles matter it can be dropped, or taken from the integrator's first stage. + +## Step-by-Step Walkthrough + +A uniform rod ($m = 1$ kg, $L = 1$ m) pivots about a horizontal axis and hangs straight down. Its moment of inertia about the pivot is $I = mL^2/3$ and its centre of mass is $L/2$ below the pivot, so small oscillations have + +$$\omega_0 = \sqrt{\frac{m g L/2}{m L^2/3}} = \sqrt{\frac{3g}{2L}} = 3.836\ \text{rad/s}, \qquad T = \frac{2\pi}{\omega_0} = 1.638\ \text{s}$$ + +1. Release it from rest at $0.05$ rad off vertical with $h = 1$ ms. +2. RK4 evaluates the articulated-body dynamics four times per step. The measured period matches $1.638$ s to within $0.02\,\%$, which is the finite-amplitude correction $\theta_0^2/16$. After 10 s (about six periods) the relative energy error is near $10^{-7}$. +3. The same experiment with semi-implicit Euler has an energy error that oscillates around zero at the oscillation frequency and never grows. +4. For a rod spinning about a vertical axis under constant torque $\tau$ (gravity is parallel to the axis and does no work), the angle follows $q(t) = \tfrac12(\tau/I)t^2$ exactly, since RK4 integrates polynomials up to degree four without error. + +## Pitfalls & Edge Cases + +- **Step size versus stiffness**: explicit methods are stable only while $h\,\omega_{\max}$ stays small, where $\omega_{\max}$ is the highest natural frequency, including stiff contact springs and high-gain controllers in the loop. Semi-implicit Euler needs $h\,\omega_{\max} < 2$; RK4 tolerates about $2.8$. +- **Damping belongs in the torque**: joint friction and viscous damping should enter as torques ($\tau - b\dot{q}$), not as ad-hoc velocity scaling after the step, which breaks the integrator's structure. +- **Controller sample rate**: when the plant step is finer than the controller period, hold $\tau$ across several plant steps. Mixing the two rates silently changes the closed-loop dynamics. +- **Long horizons in float**: angles grow without bound under continuous rotation, and float spacing grows with magnitude. Wrap revolute angles for long simulations. +- **Singular configurations**: the articulated-body recursion needs a positive joint-space inertia at every joint. Massless distal links make it ill-conditioned; add armature. + +## Variants & Generalizations + +- **Velocity Verlet / Störmer–Verlet**: a second-order symplectic method at one extra evaluation per step. +- **Adaptive steps**: embedded Runge–Kutta pairs (Dormand–Prince) control the local error, at the price of non-deterministic step counts. +- **Implicit and semi-implicit (linearized) methods**: needed for stiff contact and compliant joints. They solve with the mass matrix or its articulated-body equivalent each step. +- **Lie-group integrators**: for floating bases, integrate the base pose on SE(3) with the exponential map instead of adding velocities to orientation parameters. +- **Constraint stabilization**: closed chains and contacts need Baumgarte or projection terms to keep constraint drift bounded. + +## Applications + +- Model-in-the-loop unit tests of joint controllers (PD with gravity compensation, computed torque, impedance) on a deterministic plant. +- Host simulators and visualizers of manipulator motion. +- Hardware-in-the-loop rigs where the plant model runs on the target alongside the controller. +- Generating synthetic data for identification and observer development. + +## Connections to Other Algorithms + +- [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md): supplies $\mathrm{FD}(q, \dot{q}, \tau)$ in $O(n)$ at every stage. +- [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md): gives the kinetic energy $\tfrac12\dot{q}^T M \dot{q}$ used to monitor integration quality. +- [Recursive Newton-Euler](RecursiveNewtonEuler.md): inverse dynamics of the logged acceleration must return the applied torque, a consistency check for the whole loop. +- The semi-implicit Euler and Runge–Kutta integrators are generic solvers from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Hairer, E., Lubich, C. and Wanner, G. (2006). *Geometric Numerical Integration*. 2nd ed. Springer. Chapters I and IX. +- Featherstone, R. (2008). *Rigid Body Dynamics Algorithms*. Springer. Chapter 7. +- Butcher, J.C. (2016). *Numerical Methods for Ordinary Differential Equations*. 3rd ed. Wiley. diff --git a/doc/dynamics/README.md b/doc/dynamics/README.md index d0139cd..e10761a 100644 --- a/doc/dynamics/README.md +++ b/doc/dynamics/README.md @@ -4,10 +4,11 @@ Rigid-body dynamics algorithms for robotics and mechanical systems. ## Algorithms -| Algorithm | Description | -|------------------------------------------------------------------|-----------------------------------------------------------------------------------------------| -| [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) | $O(n)$ forward dynamics for serial chains — computes joint accelerations from applied torques | -| [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | -| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | -| [Newton-Euler](NewtonEuler.md) | Single rigid-body forward and inverse dynamics using Newton's and Euler's equations | -| [Recursive Newton-Euler](RecursiveNewtonEuler.md) | $O(n)$ inverse dynamics for serial kinematic chains via recursive velocity/force propagation | +| Algorithm | Description | +|------------------------------------------------------------------|------------------------------------------------------------------------------------------------| +| [Articulated Body Algorithm](ArticulatedBodyAlgorithm.md) | $O(n)$ forward dynamics for serial chains — computes joint accelerations from applied torques | +| [Composite Rigid Body Algorithm](CompositeRigidBodyAlgorithm.md) | $O(n^2)$ joint-space mass matrix $M(q)$ from composite inertias, exactly symmetric | +| [Euler-Lagrange](EulerLagrange.md) | Forward and inverse dynamics via the Lagrangian formulation $M\ddot{q} + C\dot{q} + g = \tau$ | +| [Forward-Dynamics Integration](ForwardDynamicsIntegrator.md) | Fixed-step simulation of a chain on the articulated-body dynamics — semi-implicit Euler or RK4 | +| [Newton-Euler](NewtonEuler.md) | Single rigid-body forward and inverse dynamics using Newton's and Euler's equations | +| [Recursive Newton-Euler](RecursiveNewtonEuler.md) | $O(n)$ inverse dynamics for serial kinematic chains via recursive velocity/force propagation | diff --git a/roadmap/README.md b/roadmap/README.md index 8658a77..4769531 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -92,7 +92,7 @@ prerequisites. `DenavitHartenberg` (M7) · `GeometricJacobian` (M8) · `ManipulabilityIndex` (M11) · `PoseInverseKinematics` (M13) · `RedundancyResolution` (M14) · `ProductOfExponentials` (M15) · `AnalyticalIkOpw` (M21) · `ParallelManipulatorKinematics` (M23) · `MobileManipulatorKinematics` (M24) · `ContinuumKinematics` (M26) · `ChainPoseKinematics` (M30) ### `dynamics` -`GenericJointLink` (M1) · `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) · `ForwardDynamicsIntegrator` (M34) +`GenericJointLink` (M1) · `MomentumObserver` (M16) · `DynamicParameterIdentification` (M22) · `ChainDynamicsModel` (M29) · `CoriolisMatrixAndRegressor` (M31) · `RneaExternalWrench` (M32) ### `trajectory` `CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md b/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md deleted file mode 100644 index f20fff2..0000000 --- a/roadmap/dynamics/ForwardDynamicsIntegrator/explanation.md +++ /dev/null @@ -1,28 +0,0 @@ -# Forward-Dynamics Integrator — Overview - -## What it is -A fixed-step time-stepping routine that advances a robot arm's joint positions and velocities under -given joint torques, using the articulated body algorithm for the accelerations. - -## Why it matters (embedded) -Controllers are best tested against a simulated plant before touching hardware. A deterministic, -heap-free step function lets the same code run in unit tests, on a host simulator and in -hardware-in-the-loop rigs. - -## How it works (intuition) -Each step asks the dynamics for the joint accelerations at the current state and torque, then moves -the state forward in time. Semi-implicit Euler updates the velocity first and uses the new velocity -for the position, which keeps the energy error bounded; the classical fourth-order Runge-Kutta -method samples the dynamics four times per step for much higher accuracy. - -## Key parameters -- **Step size** — accuracy and stability versus cost. -- **Method** — semi-implicit Euler (cheap, bounded energy error) or RK4 (accurate). - -## Reference -E. Hairer, C. Lubich, G. Wanner, *Geometric Numerical Integration* (2006), Ch. I; -R. Featherstone, *Rigid Body Dynamics Algorithms* (2008), Ch. 7. - -## See also -`ArticulatedBodyAlgorithm`, upstream `SymplecticIntegrators` (NT58) and Runge-Kutta integrators -(numerical-toolbox-cpp), the Robot Arm simulator. diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md b/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md deleted file mode 100644 index 7c0e5f8..0000000 --- a/roadmap/dynamics/ForwardDynamicsIntegrator/implementation.md +++ /dev/null @@ -1,79 +0,0 @@ -# Forward-Dynamics Integrator (Simulation Step) — Implementation Pseudocode - -> Roadmap ref: #M34 (Tier 2) · Target: `robotics/dynamics` · Namespace `dynamics` · Type: `float` (templated on `T`, instantiated for `float` only) - -A fixed-step simulator for a link chain: state `(q, q̇)`, dynamics `q̈ = ABA(q, q̇, τ)`. The simulator -application currently hand-rolls this; a library version makes model-in-the-loop tests of the -controllers (M5, M12, M16–M20) reproducible. The integrators are upstream — `solvers::SemiImplicitEuler` -(NT58) and `solvers::RungeKutta4` — and this item is the adapter that exposes ABA as their system. - -## Data structures - -```cpp -enum class IntegrationMethod : uint8_t { SemiImplicitEuler, RungeKutta4 } - -template # static_assert(std::is_floating_point_v); instantiated for float -struct ChainState: - JointVector q, qDot, qDDot # qDDot of the last evaluation (for logging / RNEA checks) - -template # implements solvers::SecondOrderSystem (NT58) -class ChainAccelerationSystem: # and solvers::OdeSystem for RK4 - const LinkArray& links - Vector3 gravity - ArticulatedBodyAlgorithm aba - Acceleration(q, q̇, τ) = aba.ForwardDynamics(links, q, q̇, τ, gravity) - Derivative(x = (q; q̇), τ, t) = (q̇; Acceleration(q, q̇, τ)) - -template -class ForwardDynamicsIntegrator: - ChainAccelerationSystem system - solvers::SemiImplicitEuler or solvers::RungeKutta4 # by Method -``` - -## Interface - -```text -ForwardDynamicsIntegrator(const LinkArray& links, const Vector3& gravity, T dt) -ChainState Step(const ChainState& state, const JointVector& tau) # hot path, τ held over dt -``` - -## Algorithm (pseudocode) - -```cpp -function Step(x, τ): # OPTIMIZE_FOR_SPEED - a = system.Acceleration(x.q, x.q̇, τ) # acceleration at the step start (logged) - if Method == SemiImplicitEuler: # symplectic (NT58): bounded energy error - (q, q̇) = semiImplicitEuler.Step({ x.q, x.q̇ }, τ) - else: # RungeKutta4 on (q; q̇), zero-order-hold τ - (q, q̇) = rungeKutta4.Step((x.q; x.q̇), τ, 0) # time-invariant system - return { q, q̇, a } -``` - -## Complexity & memory - -- Semi-implicit Euler: one ABA pass (`O(N)`) per step plus the logged one; RK4: four plus one. -- Memory: two state copies; no heap. - -## Numerical / embedded notes - -- Both integrators call the system through a virtual interface; acceptable off the real-time path - (simulation, MIL tests). -- The logged `qDDot` costs one extra ABA pass; drop it (or reuse the integrator's first evaluation) where - cycles matter. -- Semi-implicit Euler keeps energy error bounded (no secular drift) at first-order accuracy; RK4 is - fourth-order but slowly dissipative/drifting over long runs — choose by use case. -- Joint friction or damping belongs in `τ` (e.g. `τ − b·q̇`), not as ad-hoc velocity scaling. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/dynamics/ForwardDynamicsIntegrator.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Step`, `extern template` for - `` and `` - under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/dynamics/ForwardDynamicsIntegrator.cpp` → the same instantiations. -- Depends on: NT58 (`solvers::SemiImplicitEuler`, `SecondOrderSystem`); upstream `solvers::RungeKutta4`. -- Test: `robotics/dynamics/test/TestForwardDynamicsIntegrator.cpp` -- Doc: `doc/dynamics/ForwardDynamicsIntegrator.md` (per `doc/TEMPLATE.md`) -- The simulator's `RobotArmSimulator::StepChain` should switch to this integrator once it exists. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md b/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md deleted file mode 100644 index 90e78f8..0000000 --- a/roadmap/dynamics/ForwardDynamicsIntegrator/tests.md +++ /dev/null @@ -1,39 +0,0 @@ -# Forward-Dynamics Integrator — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestForwardDynamicsIntegrator : public ::testing::Test: - # single uniform rod about ŷ (pendulum) and a 2-link rod chain, gravity (0, 0, −9.81) - ForwardDynamicsIntegrator pendulum{ rod, gravity, 1e-3f } - ForwardDynamicsIntegrator doubleRod{ rods, gravity, 1e-3f } -# each case below is a TEST_F(TestForwardDynamicsIntegrator, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -small_oscillation_period_matches_physical_pendulum: - Arrange: rod length L hanging at q = π/2, released from π/2 + 0.05 rad, dt = 1 ms - Assert: measured period ≈ 2π·sqrt(2L/(3g)) within 0.5 % -rk4_energy_drift_is_small: - Assert: |E(10 s) − E(0)| / |E(0)| < 1e-4 for the pendulum at dt = 1 ms -semi_implicit_euler_energy_stays_bounded: - Assert: energy error over 10 s never exceeds its maximum over the first second by more than 2× -step_acceleration_equals_articulated_body_algorithm: - Assert: Step(x, τ).qDDot == ABA(links, x.q, x.q̇, τ, gravity) -constant_torque_on_free_link_gives_quadratic_angle: - Arrange: z-axis rod (gravity ⟂ motion), τ constant - Assert: q(t) ≈ ½·(τ / (mL²/3))·t² -``` - -## Reference vectors - -- Uniform rod pendulum: `ω₀ = sqrt(3g / (2L))`; `L = 1 m`, `g = 9.81` ⇒ period `1.638 s`. - -## Edge cases - -- `dt` is fixed at construction (the upstream integrators take `h` there); `dt ≤ 0` is a precondition violation. -- Zero gravity, zero torque ⇒ constant velocity. diff --git a/robotics/dynamics/CMakeLists.txt b/robotics/dynamics/CMakeLists.txt index 2dee4c9..15ae812 100644 --- a/robotics/dynamics/CMakeLists.txt +++ b/robotics/dynamics/CMakeLists.txt @@ -16,6 +16,7 @@ target_sources(robotics.dynamics PRIVATE CompositeRigidBodyAlgorithm.hpp EulerLagrangeDynamics.hpp EulerLagrangeSolver.hpp + ForwardDynamicsIntegrator.hpp NewtonEulerBody.hpp NewtonEulerSolver.hpp RecursiveNewtonEuler.hpp @@ -27,6 +28,7 @@ robotics_add_coverage_sources(robotics.dynamics ArticulatedBodyAlgorithm.cpp CompositeRigidBodyAlgorithm.cpp EulerLagrangeSolver.cpp + ForwardDynamicsIntegrator.cpp NewtonEulerSolver.cpp RecursiveNewtonEuler.cpp ) diff --git a/robotics/dynamics/ForwardDynamicsIntegrator.cpp b/robotics/dynamics/ForwardDynamicsIntegrator.cpp new file mode 100644 index 0000000..d89942f --- /dev/null +++ b/robotics/dynamics/ForwardDynamicsIntegrator.cpp @@ -0,0 +1,9 @@ +#include "robotics/dynamics/ForwardDynamicsIntegrator.hpp" + +namespace dynamics +{ + template class ChainAccelerationSystem; + template class ChainAccelerationSystem; + template class ForwardDynamicsIntegrator; + template class ForwardDynamicsIntegrator; +} diff --git a/robotics/dynamics/ForwardDynamicsIntegrator.hpp b/robotics/dynamics/ForwardDynamicsIntegrator.hpp new file mode 100644 index 0000000..89829b3 --- /dev/null +++ b/robotics/dynamics/ForwardDynamicsIntegrator.hpp @@ -0,0 +1,137 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/solvers/OdeSystem.hpp" +#include "numerical/solvers/RungeKuttaIntegrators.hpp" +#include "numerical/solvers/SecondOrderSystem.hpp" +#include "numerical/solvers/SymplecticIntegrators.hpp" +#include "robotics/dynamics/ArticulatedBodyAlgorithm.hpp" +#include "robotics/dynamics/RevoluteJointLink.hpp" +#include +#include +#include +#include + +namespace dynamics +{ + enum class IntegrationMethod : std::uint8_t + { + SemiImplicitEuler, + RungeKutta4 + }; + + template + struct ChainState + { + static_assert(std::is_floating_point_v, "ChainState supports floating-point types"); + + math::Vector q{}; + math::Vector qDot{}; + math::Vector qDDot{}; + }; + + template + class ChainAccelerationSystem + : public solvers::SecondOrderSystem + , public solvers::OdeSystem + { + static_assert(std::is_floating_point_v, "ChainAccelerationSystem supports floating-point types"); + + public: + using Vector3 = math::Vector; + using JointVector = math::Vector; + using StateVector = math::Vector; + using LinkArray = std::array, NumLinks>; + + ChainAccelerationSystem(const LinkArray& links, const Vector3& gravity); + + JointVector Acceleration(const JointVector& q, const JointVector& qDot, const JointVector& tau) override; + StateVector Derivative(const StateVector& x, const JointVector& tau, T t) override; + + private: + const LinkArray& links; + Vector3 gravity; + ArticulatedBodyAlgorithm aba; + }; + + template + class ForwardDynamicsIntegrator + { + static_assert(std::is_floating_point_v, "ForwardDynamicsIntegrator supports floating-point types"); + + public: + using Vector3 = math::Vector; + using JointVector = math::Vector; + using LinkArray = std::array, NumLinks>; + using State = ChainState; + + ForwardDynamicsIntegrator(const LinkArray& links, const Vector3& gravity, T dt); + + OPTIMIZE_FOR_SPEED State Step(const State& state, const JointVector& tau); + + private: + using Integrator = std::conditional_t, + solvers::RungeKutta4>; + + ChainAccelerationSystem system; + Integrator integrator; + }; + + template + ChainAccelerationSystem::ChainAccelerationSystem(const LinkArray& links, const Vector3& gravity) + : links{ links } + , gravity{ gravity } + {} + + template + typename ChainAccelerationSystem::JointVector ChainAccelerationSystem::Acceleration(const JointVector& q, const JointVector& qDot, const JointVector& tau) + { + return aba.ForwardDynamics(links, q, qDot, tau, gravity); + } + + template + typename ChainAccelerationSystem::StateVector ChainAccelerationSystem::Derivative(const StateVector& x, const JointVector& tau, T) + { + const auto qDot{ x.template GetBlock(NumLinks, 0) }; + + StateVector derivative{}; + derivative.SetBlock(qDot, 0, 0); + derivative.SetBlock(Acceleration(x.template GetBlock(0, 0), qDot, tau), NumLinks, 0); + return derivative; + } + + template + ForwardDynamicsIntegrator::ForwardDynamicsIntegrator(const LinkArray& links, const Vector3& gravity, T dt) + : system{ links, gravity } + , integrator{ system, dt } + {} + + template + OPTIMIZE_FOR_SPEED typename ForwardDynamicsIntegrator::State ForwardDynamicsIntegrator::Step(const State& state, const JointVector& tau) + { + const auto acceleration{ system.Acceleration(state.q, state.qDot, tau) }; + + if constexpr (Method == IntegrationMethod::SemiImplicitEuler) + { + const auto next{ integrator.Step({ state.q, state.qDot }, tau) }; + return { next.q, next.qDot, acceleration }; + } + else + { + math::Vector x{}; + x.SetBlock(state.q, 0, 0); + x.SetBlock(state.qDot, NumLinks, 0); + const auto next{ integrator.Step(x, tau, T{}) }; + return { next.template GetBlock(0, 0), next.template GetBlock(NumLinks, 0), acceleration }; + } + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ChainAccelerationSystem; + extern template class ChainAccelerationSystem; + extern template class ForwardDynamicsIntegrator; + extern template class ForwardDynamicsIntegrator; +#endif +} diff --git a/robotics/dynamics/test/CMakeLists.txt b/robotics/dynamics/test/CMakeLists.txt index 4ce1ebd..6f11a03 100644 --- a/robotics/dynamics/test/CMakeLists.txt +++ b/robotics/dynamics/test/CMakeLists.txt @@ -11,6 +11,7 @@ target_sources(robotics.dynamics_test PRIVATE TestArticulatedBodyAlgorithm.cpp TestCompositeRigidBodyAlgorithm.cpp TestEulerLagrangeSolver.cpp + TestForwardDynamicsIntegrator.cpp TestNewtonEulerSolver.cpp TestRecursiveNewtonEuler.cpp ) diff --git a/robotics/dynamics/test/TestForwardDynamicsIntegrator.cpp b/robotics/dynamics/test/TestForwardDynamicsIntegrator.cpp new file mode 100644 index 0000000..c4bf0c8 --- /dev/null +++ b/robotics/dynamics/test/TestForwardDynamicsIntegrator.cpp @@ -0,0 +1,130 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Tolerance.hpp" +#include "robotics/dynamics/ArticulatedBodyAlgorithm.hpp" +#include "robotics/dynamics/CompositeRigidBodyAlgorithm.hpp" +#include "robotics/dynamics/ForwardDynamicsIntegrator.hpp" +#include +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector; + using Matrix3 = math::SquareMatrix; + using Link = dynamics::RevoluteJointLink; + using Method = dynamics::IntegrationMethod; + + constexpr float g{ 9.81f }; + constexpr float halfPi{ std::numbers::pi_v / 2.0f }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeRod(const Vector3& axis, const Vector3& parentToJoint) + { + return Link{ 1.0f, Matrix3{ { 0.0f, 0.0f, 0.0f }, { 0.0f, 1.0f / 12.0f, 0.0f }, { 0.0f, 0.0f, 1.0f / 12.0f } }, axis, parentToJoint, Vector3{ 0.5f, 0.0f, 0.0f } }; + } + + class TestForwardDynamicsIntegrator + : public ::testing::Test + { + protected: + float PendulumEnergy(const dynamics::ChainState& state) const + { + return 0.5f * (1.0f / 3.0f) * state.qDot[0] * state.qDot[0] - g * 0.5f * std::sin(state.q[0]); + } + + float DoubleRodEnergy(const dynamics::ChainState& state) const + { + const Matrix3 R0{ math::RotationAboutAxis(rods[0].jointAxis, state.q[0]) }; + const Matrix3 R1{ R0 * math::RotationAboutAxis(rods[1].jointAxis, state.q[1]) }; + const Vector3 com0{ R0 * rods[0].jointToCoM }; + const Vector3 com1{ R0 * rods[1].parentToJoint + R1 * rods[1].jointToCoM }; + const auto M{ crba.Compute(rods, state.q) }; + return 0.5f * (state.qDot.Transpose() * (M * state.qDot)).at(0, 0) + g * (com0[2] + com1[2]); + } + + Vector3 gravity{ 0.0f, 0.0f, -g }; + std::array rod{ MakeRod(yAxis, Vector3{}) }; + std::array rods{ MakeRod(yAxis, Vector3{}), MakeRod(yAxis, Vector3{ 1.0f, 0.0f, 0.0f }) }; + dynamics::CompositeRigidBodyAlgorithm crba; + dynamics::ForwardDynamicsIntegrator pendulum{ rod, gravity, 1e-3f }; + dynamics::ForwardDynamicsIntegrator doubleRod{ rods, gravity, 1e-3f }; + }; +} + +TEST_F(TestForwardDynamicsIntegrator, small_oscillation_period_matches_physical_pendulum) +{ + dynamics::ChainState state{ math::Vector{ halfPi + 0.05f } }; + std::array upwardCrossings{}; + std::size_t crossings{ 0 }; + + for (std::size_t step = 1; step <= 5000 && crossings < upwardCrossings.size(); ++step) + { + const float before{ state.q[0] - halfPi }; + state = pendulum.Step(state, math::Vector{}); + const float after{ state.q[0] - halfPi }; + if (before < 0.0f && after >= 0.0f) + upwardCrossings[crossings++] = 1e-3f * (static_cast(step) - after / (after - before)); + } + + const float expected{ 2.0f * std::numbers::pi_v * std::sqrt(2.0f / (3.0f * g)) }; + ASSERT_EQ(crossings, upwardCrossings.size()); + EXPECT_NEAR(upwardCrossings[1] - upwardCrossings[0], expected, 0.005f * expected); +} + +TEST_F(TestForwardDynamicsIntegrator, rk4_energy_drift_is_small) +{ + dynamics::ChainState state{ math::Vector{ halfPi + 0.05f } }; + const float initial{ PendulumEnergy(state) }; + + for (std::size_t step = 0; step < 10000; ++step) + state = pendulum.Step(state, math::Vector{}); + + EXPECT_LT(std::abs(PendulumEnergy(state) - initial) / std::abs(initial), 1e-4f); +} + +TEST_F(TestForwardDynamicsIntegrator, semi_implicit_euler_energy_stays_bounded) +{ + dynamics::ChainState state{ math::Vector{ halfPi + 0.3f, 0.2f } }; + const float initial{ DoubleRodEnergy(state) }; + float firstSecondError{ 0.0f }; + float overallError{ 0.0f }; + + for (std::size_t step = 1; step <= 10000; ++step) + { + state = doubleRod.Step(state, math::Vector{}); + const float error{ std::abs(DoubleRodEnergy(state) - initial) }; + overallError = std::max(overallError, error); + if (step <= 1000) + firstSecondError = overallError; + } + + EXPECT_LE(overallError, 2.0f * firstSecondError); +} + +TEST_F(TestForwardDynamicsIntegrator, step_acceleration_equals_articulated_body_algorithm) +{ + const dynamics::ChainState state{ math::Vector{ 0.4f, -0.9f }, math::Vector{ 1.2f, -0.5f } }; + const math::Vector tau{ 0.7f, -0.3f }; + const dynamics::ArticulatedBodyAlgorithm aba; + + const auto next{ doubleRod.Step(state, tau) }; + const auto expected{ aba.ForwardDynamics(rods, state.q, state.qDot, tau, gravity) }; + + EXPECT_NEAR(next.qDDot[0], expected[0], math::Tolerance()); + EXPECT_NEAR(next.qDDot[1], expected[1], math::Tolerance()); +} + +TEST_F(TestForwardDynamicsIntegrator, constant_torque_on_free_link_gives_quadratic_angle) +{ + const std::array freeRod{ MakeRod(zAxis, Vector3{}) }; + dynamics::ForwardDynamicsIntegrator integrator{ freeRod, gravity, 1e-3f }; + const float torque{ 0.5f }; + dynamics::ChainState state{}; + + for (std::size_t step = 0; step < 1000; ++step) + state = integrator.Step(state, math::Vector{ torque }); + + EXPECT_NEAR(state.q[0], 0.5f * (torque / (1.0f / 3.0f)) * 1.0f, math::Tolerance()); +} From ecf1b33bce11b685c4190bc3cb871a928f987c3f Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 10:54:01 +0000 Subject: [PATCH 6/7] docs: align tables and wrap long lines flagged by MegaLinter Apply the markdown-table-formatter suggestions on PR #33 (ROADMAP planning table and the five new complexity tables) and wrap four paragraph lines that exceeded markdownlint's 400-character MD013 limit. Text is unchanged. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- ROADMAP.md | 14 +++++++------- .../manipulator/PdGravityCompensation.md | 11 ++++++----- doc/dynamics/CompositeRigidBodyAlgorithm.md | 10 +++++----- doc/dynamics/ForwardDynamicsIntegrator.md | 11 ++++++----- doc/planning/PrimitiveDistance.md | 15 ++++++++------- doc/trajectory/CubicSplineTrajectory.md | 14 ++++++++------ 6 files changed, 40 insertions(+), 35 deletions(-) diff --git a/ROADMAP.md b/ROADMAP.md index 4f3c029..fe5d651 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -443,13 +443,13 @@ Cross-family building blocks that several of the families below depend on. ## Motion planning and collision -| # | Component | Target module | Depends on | Difficulty | -|----|-------------------------------------------------------------------|---------------|------------|------------| -| P2 | Artificial potential fields | `planning` | P1 | ★★☆☆☆ | -| P3 | A* / Dijkstra on bounded occupancy grids | `planning` | — | ★★★☆☆ | -| P4 | Self-collision checking for a link chain (capsule model) | `planning` | P1, M30 | ★★★☆☆ | -| P5 | GJK + EPA convex distance and penetration | `planning` | — | ★★★★☆ | -| P6 | RRT-Connect in joint space with a fixed node pool | `planning` | P4 | ★★★★☆ | +| # | Component | Target module | Depends on | Difficulty | +|----|----------------------------------------------------------|---------------|------------|------------| +| P2 | Artificial potential fields | `planning` | P1 | ★★☆☆☆ | +| P3 | A* / Dijkstra on bounded occupancy grids | `planning` | — | ★★★☆☆ | +| P4 | Self-collision checking for a link chain (capsule model) | `planning` | P1, M30 | ★★★☆☆ | +| P5 | GJK + EPA convex distance and penetration | `planning` | — | ★★★★☆ | +| P6 | RRT-Connect in joint space with a fixed node pool | `planning` | P4 | ★★★★☆ | **P2. Potential fields.** Quadratic-then-conic attraction and Khatib repulsion from obstacle proximity readings as a clamped task-space velocity; local minima are detected and reported. - *Algorithm / paper:* O. Khatib, *IJRR*, 5(1), 1986. diff --git a/doc/controllers/manipulator/PdGravityCompensation.md b/doc/controllers/manipulator/PdGravityCompensation.md index 6efd6a0..3f837be 100644 --- a/doc/controllers/manipulator/PdGravityCompensation.md +++ b/doc/controllers/manipulator/PdGravityCompensation.md @@ -8,7 +8,8 @@ Holding a manipulator at a commanded pose is the most common joint-control task: $$\tau = K_p\,(q_d - q) - K_d\,\dot{q} + g(q)$$ -Takegaki and Arimoto (1981) proved that this law drives the arm to **any** constant set-point $q_d$ from **any** initial state, for **any** symmetric positive-definite gains. It needs only $g(q)$, not the mass matrix or Coriolis terms, which makes it the cheapest model-based joint controller with a global stability guarantee. On a microcontroller it is a handful of multiply–adds per joint plus one gravity evaluation. +Takegaki and Arimoto (1981) proved that this law drives the arm to **any** constant set-point $q_d$ from **any** initial state, for **any** symmetric positive-definite gains. +It needs only $g(q)$, not the mass matrix or Coriolis terms, which makes it the cheapest model-based joint controller with a global stability guarantee. On a microcontroller it is a handful of multiply–adds per joint plus one gravity evaluation. ## Mathematical Theory @@ -38,10 +39,10 @@ Dropping $g(q)$ leaves the equilibrium condition $K_p e_{ss} = g(q_d - e_{ss})$. ## Complexity Analysis -| Case | Time | Space | Notes | -|----------------|------------------------|-------------|----------------------------------------------------------------| -| Full gains | $O(n^2) + T_g$ | $O(n^2)$ | Two gain matrix–vector products plus one gravity evaluation | -| Diagonal gains | $O(n) + T_g$ | $O(n)$ | Joints decouple; $2n$ multiplies plus the gravity evaluation | +| Case | Time | Space | Notes | +|----------------|----------------|----------|--------------------------------------------------------------| +| Full gains | $O(n^2) + T_g$ | $O(n^2)$ | Two gain matrix–vector products plus one gravity evaluation | +| Diagonal gains | $O(n) + T_g$ | $O(n)$ | Joints decouple; $2n$ multiplies plus the gravity evaluation | $T_g$ is the cost of the gravity vector: $O(n)$ with a recursive Newton–Euler pass at zero velocity and acceleration, or a closed form for short chains. diff --git a/doc/dynamics/CompositeRigidBodyAlgorithm.md b/doc/dynamics/CompositeRigidBodyAlgorithm.md index a407380..06a0d65 100644 --- a/doc/dynamics/CompositeRigidBodyAlgorithm.md +++ b/doc/dynamics/CompositeRigidBodyAlgorithm.md @@ -57,11 +57,11 @@ projected on each ancestor axis gives $M_{ji} = M_{ij} = \hat{z}_j^T n$ for $j < ## Complexity Analysis -| Phase | Time | Space | Notes | -|--------------------|-----------|----------|-------------------------------------------------------------------| -| Rotations | $O(n)$ | $O(n)$ | One axis–angle rotation per joint | -| Composite inertias | $O(n)$ | $O(n)$ | One inertia transform (a few $3 \times 3$ products) per joint | -| Columns | $O(n^2)$ | $O(n^2)$ | $n(n-1)/2$ wrench transforms, $O(1)$ each, plus the $n \times n$ result | +| Phase | Time | Space | Notes | +|--------------------|----------|----------|-------------------------------------------------------------------------| +| Rotations | $O(n)$ | $O(n)$ | One axis–angle rotation per joint | +| Composite inertias | $O(n)$ | $O(n)$ | One inertia transform (a few $3 \times 3$ products) per joint | +| Columns | $O(n^2)$ | $O(n^2)$ | $n(n-1)/2$ wrench transforms, $O(1)$ each, plus the $n \times n$ result | For short chains ($n \le 6$) the constant matters more than the order: CRBA's per-entry work is one $3 \times 3$ rotation and a cross product, far below a full inverse-dynamics sweep per column. diff --git a/doc/dynamics/ForwardDynamicsIntegrator.md b/doc/dynamics/ForwardDynamicsIntegrator.md index 1d8ae80..314811d 100644 --- a/doc/dynamics/ForwardDynamicsIntegrator.md +++ b/doc/dynamics/ForwardDynamicsIntegrator.md @@ -20,7 +20,8 @@ Digital controllers update $\tau$ once per sample period $h$ and hold it in betw $$\dot{q}_{k+1} = \dot{q}_k + h\,\mathrm{FD}(q_k, \dot{q}_k, \tau_k), \qquad q_{k+1} = q_k + h\,\dot{q}_{k+1}$$ -The velocity is updated first and the **new** velocity moves the position. For separable mechanical systems (velocity-independent forces) the map is **symplectic**: it preserves phase-space area. By backward error analysis, it exactly conserves a modified energy $\tilde{H} = H + O(h)$. The true energy error therefore oscillates within an $O(h)$ band for exponentially long times instead of drifting (Hairer, Lubich, Wanner). The method is first-order accurate and costs one dynamics evaluation per step. +The velocity is updated first and the **new** velocity moves the position. For separable mechanical systems (velocity-independent forces) the map is **symplectic**: it preserves phase-space area. By backward error analysis, it exactly conserves a modified energy $\tilde{H} = H + O(h)$. +The true energy error therefore oscillates within an $O(h)$ band for exponentially long times instead of drifting (Hairer, Lubich, Wanner). The method is first-order accurate and costs one dynamics evaluation per step. Velocity-dependent terms of a manipulator (Coriolis, centrifugal) break exact separability. In practice the energy error stays bounded for the step sizes used in control simulation. @@ -41,10 +42,10 @@ Each step also reports $\ddot{q}_k = \mathrm{FD}(q_k, \dot{q}_k, \tau_k)$ at the ## Complexity Analysis -| Method | Dynamics calls / step | Time / step | Space | Accuracy (global) | -|---------------------|-----------------------|-------------|----------|-------------------| -| Semi-implicit Euler | 1 (+1 logged) | $O(n)$ | $O(n)$ | $O(h)$, bounded energy error | -| Runge–Kutta 4 | 4 (+1 logged) | $O(n)$ | $O(n)$ | $O(h^4)$, slow energy drift | +| Method | Dynamics calls / step | Time / step | Space | Accuracy (global) | +|---------------------|-----------------------|-------------|--------|------------------------------| +| Semi-implicit Euler | 1 (+1 logged) | $O(n)$ | $O(n)$ | $O(h)$, bounded energy error | +| Runge–Kutta 4 | 4 (+1 logged) | $O(n)$ | $O(n)$ | $O(h^4)$, slow energy drift | The logged acceleration costs one extra evaluation. Where cycles matter it can be dropped, or taken from the integrator's first stage. diff --git a/doc/planning/PrimitiveDistance.md b/doc/planning/PrimitiveDistance.md index 1c2d1bd..39028c7 100644 --- a/doc/planning/PrimitiveDistance.md +++ b/doc/planning/PrimitiveDistance.md @@ -2,7 +2,8 @@ ## Overview & Motivation -Collision checking on a robot rarely uses the real link meshes. Arm links are wrapped in **capsules** (a sphere swept along a segment), grippers and heads in **spheres**, and tables, walls and fixtures in **oriented boxes**. These checks run every control cycle in self-collision avoidance, human–robot speed and separation monitoring, and reactive obstacle avoidance. They must be exact, constant-time, branch-light and allocation-free. +Collision checking on a robot rarely uses the real link meshes. Arm links are wrapped in **capsules** (a sphere swept along a segment), grippers and heads in **spheres**, and tables, walls and fixtures in **oriented boxes**. +These checks run every control cycle in self-collision avoidance, human–robot speed and separation monitoring, and reactive obstacle avoidance. They must be exact, constant-time, branch-light and allocation-free. The queries return the **closest points** on both shapes and a **signed distance**: positive when the shapes are apart, negative when they overlap, where the magnitude is the penetration depth. The signed value lets one smooth function serve both clearance monitoring and contact response. @@ -46,12 +47,12 @@ A sphere against a box is the point query on its centre, rounded by the radius. ## Complexity Analysis -| Query | Time | Space | Notes | -|-------------------------|--------|--------|----------------------------------------------------------| -| Point–segment | $O(1)$ | $O(1)$ | One division, one square root | -| Segment–segment | $O(1)$ | $O(1)$ | Up to three divisions, one square root, no iteration | -| Point–box, sphere–box | $O(1)$ | $O(1)$ | One $3 \times 3$ rotation, three clamps | -| Rounded pairs | $O(1)$ | $O(1)$ | Core query plus one normalization | +| Query | Time | Space | Notes | +|-----------------------|--------|--------|------------------------------------------------------| +| Point–segment | $O(1)$ | $O(1)$ | One division, one square root | +| Segment–segment | $O(1)$ | $O(1)$ | Up to three divisions, one square root, no iteration | +| Point–box, sphere–box | $O(1)$ | $O(1)$ | One $3 \times 3$ rotation, three clamps | +| Rounded pairs | $O(1)$ | $O(1)$ | Core query plus one normalization | There are no loops beyond the fixed three box axes, so timing is fully deterministic. diff --git a/doc/trajectory/CubicSplineTrajectory.md b/doc/trajectory/CubicSplineTrajectory.md index 7ceb995..c529f6c 100644 --- a/doc/trajectory/CubicSplineTrajectory.md +++ b/doc/trajectory/CubicSplineTrajectory.md @@ -2,7 +2,9 @@ ## Overview & Motivation -Teach-in programs, planners and resampled optimal profiles hand a controller a list of **via points**: joint vectors $q_k$ that should be reached at times $t_k$. Stopping at every via point wastes time and excites the structure; fitting one high-order polynomial through all of them rings between the knots. A **cubic spline** is the standard middle ground. Between consecutive knots each joint follows a cubic polynomial, and the pieces join with continuous position, velocity **and** acceleration ($C^2$). +Teach-in programs, planners and resampled optimal profiles hand a controller a list of **via points**: joint vectors $q_k$ that should be reached at times $t_k$. +Stopping at every via point wastes time and excites the structure; fitting one high-order polynomial through all of them rings between the knots. A **cubic spline** is the standard middle ground. +Between consecutive knots each joint follows a cubic polynomial, and the pieces join with continuous position, velocity **and** acceleration ($C^2$). Taking time as the abscissa makes the spline a trajectory directly. Its slope is the joint velocity and its curvature is the joint acceleration. Planning is a single linear-time pass, and every servo tick costs one interval search plus a cubic evaluation per joint. Both are bounded and deterministic, which suits embedded controllers. @@ -37,11 +39,11 @@ Requests outside $[t_0, t_{K-1}]$ are clamped to the end states. ## Complexity Analysis -| Phase | Time | Space | Notes | -|-------------|-------------------|----------------|----------------------------------------------------------| -| Planning | $O(nK)$ | $O(nK)$ | One tridiagonal factorization, $n$ back-substitutions | -| Sampling | $O(\log K + n)$ | $O(1)$ | Binary search for the segment, then a cubic per joint | -| Storage | — | $K(2 + 2n)$ | Knot times, spans, values and knot accelerations | +| Phase | Time | Space | Notes | +|----------|-----------------|-------------|-------------------------------------------------------| +| Planning | $O(nK)$ | $O(nK)$ | One tridiagonal factorization, $n$ back-substitutions | +| Sampling | $O(\log K + n)$ | $O(1)$ | Binary search for the segment, then a cubic per joint | +| Storage | — | $K(2 + 2n)$ | Knot times, spans, values and knot accelerations | Storage is sized by the compile-time capacity, so the memory footprint is fixed no matter how many via points a plan uses. From 92f6c4b93134c5beeda2b81a83b94586c7dd4bf4 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 5 Nov 2026 10:56:32 +0000 Subject: [PATCH 7/7] fix(dynamics): mark the stateless ABA member [[no_unique_address]] Addresses the SonarCloud finding on PR #33: the articulated-body solver held by ChainAccelerationSystem is an empty class, so it takes no storage. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- robotics/dynamics/ForwardDynamicsIntegrator.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robotics/dynamics/ForwardDynamicsIntegrator.hpp b/robotics/dynamics/ForwardDynamicsIntegrator.hpp index 89829b3..bd3deab 100644 --- a/robotics/dynamics/ForwardDynamicsIntegrator.hpp +++ b/robotics/dynamics/ForwardDynamicsIntegrator.hpp @@ -52,7 +52,7 @@ namespace dynamics private: const LinkArray& links; Vector3 gravity; - ArticulatedBodyAlgorithm aba; + [[no_unique_address]] ArticulatedBodyAlgorithm aba; }; template