From 72e7f52c2fa579214fcf5f9737b982ae5c112ba4 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:18:21 +0000 Subject: [PATCH 1/5] feat(motion_profiles): add module with polynomial point-to-point profile Deploy roadmap item 55 and create the motion_profiles module with the shared TrajectoryState / MotionLimits types. PolynomialProfile solves the closed-form cubic (position + velocity) or quintic (+ acceleration) coefficients once and samples position, velocity, acceleration and jerk by Horner evaluation, with time clamped to [0, tf]. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- AGENTS.md | 2 +- README.md | 15 +- ROADMAP.md | 9 -- doc/motion_profiles/PolynomialProfile.md | 107 +++++++++++++++ doc/motion_profiles/README.md | 9 ++ numerical/CMakeLists.txt | 1 + numerical/motion_profiles/CMakeLists.txt | 22 +++ .../motion_profiles/MotionProfileTypes.hpp | 29 ++++ .../motion_profiles/PolynomialProfile.cpp | 6 + .../motion_profiles/PolynomialProfile.hpp | 129 ++++++++++++++++++ numerical/motion_profiles/test/CMakeLists.txt | 14 ++ .../test/TestPolynomialProfile.cpp | 85 ++++++++++++ roadmap/README.md | 2 +- .../PolynomialProfile/explanation.md | 32 ----- .../PolynomialProfile/implementation.md | 112 --------------- .../PolynomialProfile/tests.md | 63 --------- 16 files changed, 412 insertions(+), 225 deletions(-) create mode 100644 doc/motion_profiles/PolynomialProfile.md create mode 100644 doc/motion_profiles/README.md create mode 100644 numerical/motion_profiles/CMakeLists.txt create mode 100644 numerical/motion_profiles/MotionProfileTypes.hpp create mode 100644 numerical/motion_profiles/PolynomialProfile.cpp create mode 100644 numerical/motion_profiles/PolynomialProfile.hpp create mode 100644 numerical/motion_profiles/test/CMakeLists.txt create mode 100644 numerical/motion_profiles/test/TestPolynomialProfile.cpp delete mode 100644 roadmap/motion_profiles/PolynomialProfile/explanation.md delete mode 100644 roadmap/motion_profiles/PolynomialProfile/implementation.md delete mode 100644 roadmap/motion_profiles/PolynomialProfile/tests.md diff --git a/AGENTS.md b/AGENTS.md index 0a13925e..08f5c37e 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -53,7 +53,7 @@ resource-constrained embedded systems. Real-time, deterministic, no heap. `analysis`, `windowing`, `control_analysis`, `controllers`, `estimators`, `filters` (active Kalman family — **not** `filters::active`), `filters::passive`, `math`, `optimization`, `regularization`, `solvers`, and new: -`robust_control`, `nonlinear_control`. +`robust_control`, `nonlinear_control`, `motion_profiles`. ## Math functions diff --git a/README.md b/README.md index d532083e..516969b6 100644 --- a/README.md +++ b/README.md @@ -21,13 +21,14 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer, Friction Compensation (Coulomb + Viscous + Stribeck) | | [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS | | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | -| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | -| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | -| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | -| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | -| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | -| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | -| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | +| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | +| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | +| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | +| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile | +| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | +| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | +| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | +| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | 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 ead769cb..c3aa9ced 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -26,7 +26,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| -| 55 | Polynomial (cubic/quintic) point-to-point profile | `motion_profiles` (new) | ★☆☆☆☆ | | 56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` (new) | ★☆☆☆☆ | | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | | 60 | S-curve (jerk-limited) profile | `motion_profiles` (new) | ★★☆☆☆ | @@ -56,14 +55,6 @@ the library does not yet expose. Detailed below under ## Tier 1 — Trivial primitives ★☆☆☆☆ -### 55. Polynomial point-to-point profile -- **What:** Cubic (position + velocity) or quintic (+ acceleration) time law between two states over a - fixed duration; defines the shared `TrajectoryState` / `MotionLimits` types of `motion_profiles`. -- **Embedded value:** Coefficients solved once in closed form; each servo tick is a Horner evaluation. -- **Algorithm / paper:** M. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 5. -- **Reuses:** Scalar templates. -- **Spec:** [roadmap/motion_profiles/PolynomialProfile](roadmap/motion_profiles/PolynomialProfile/implementation.md). - ### 56. Trapezoidal (LSPB) profile - **What:** Minimum-time velocity/acceleration-limited rest-to-rest move, triangular for short moves, plus a fixed-duration plan for multi-axis synchronization. diff --git a/doc/motion_profiles/PolynomialProfile.md b/doc/motion_profiles/PolynomialProfile.md new file mode 100644 index 00000000..81caffa0 --- /dev/null +++ b/doc/motion_profiles/PolynomialProfile.md @@ -0,0 +1,107 @@ +# Polynomial Point-to-Point Profile + +## Overview & Motivation + +A polynomial profile is a closed-form time law that drives one axis from a start state to a goal state over a fixed duration $T$. A **cubic** matches endpoint position and velocity; a **quintic** additionally matches endpoint acceleration, so the motion has no acceleration step at either end. + +Point-to-point moves are the bread and butter of servo motion: robot joints, CNC and pick-and-place axes, motor position loops. The coefficients come from a handful of algebraic expressions, solved once. Each servo tick is then a fixed number of multiply-adds, with no iteration, no lookup table and a deterministic cycle count. + +## Mathematical Theory + +### Boundary Conditions + +Let $t \in [0, T]$, $\Delta q = q_f - q_0$, and let $\alpha_0, \alpha_f$ be the start and end accelerations. A polynomial of degree $n$ has $n+1$ coefficients, so a cubic satisfies four conditions and a quintic six: + +$$q(0) = q_0,\quad q(T) = q_f,\quad \dot{q}(0) = v_0,\quad \dot{q}(T) = v_f \qquad \text{(quintic also: } \ddot{q}(0) = \alpha_0,\ \ddot{q}(T) = \alpha_f\text{)}$$ + +The conditions are linear in the coefficients $a_k$ of $q(t) = \sum_k a_k t^k$ and the time basis is fixed, so the solution is a plug-in formula rather than a runtime matrix solve. + +### Cubic + +The start conditions give $a_0 = q_0$ and $a_1 = v_0$. The end conditions leave a $2\times 2$ system with the closed-form solution + +$$a_2 = \frac{3\,\Delta q - (2 v_0 + v_f)\,T}{T^2}, \qquad a_3 = \frac{-2\,\Delta q + (v_0 + v_f)\,T}{T^3}$$ + +The acceleration is linear in $t$ and the jerk is the constant $6 a_3$. + +### Quintic + +The start conditions give $a_0 = q_0$, $a_1 = v_0$, $a_2 = \alpha_0 / 2$. The end conditions give + +$$a_3 = \frac{20\,\Delta q - (8 v_f + 12 v_0)\,T - (3\alpha_0 - \alpha_f)\,T^2}{2\,T^3}$$ + +$$a_4 = \frac{-30\,\Delta q + (14 v_f + 16 v_0)\,T + (3\alpha_0 - 2\alpha_f)\,T^2}{2\,T^4}$$ + +$$a_5 = \frac{12\,\Delta q - 6 (v_f + v_0)\,T - (\alpha_0 - \alpha_f)\,T^2}{2\,T^5}$$ + +The acceleration is a cubic matching $\alpha_0$ and $\alpha_f$, hence continuous; the jerk is a quadratic. + +### Sampling + +Position, velocity, acceleration and jerk are four Horner evaluations of the polynomial and its three derivatives: + +$$q = a_0 + t\big(a_1 + t(a_2 + t(a_3 + t(a_4 + t\,a_5)))\big), \qquad \dddot{q} = 6 a_3 + t\,(24 a_4 + 60 a_5\, t)$$ + +The cubic is the same evaluation with $a_4 = a_5 = 0$. The time is first clamped to $[0, T]$. + +### Rest-to-Rest Peaks + +With zero end velocities (and accelerations, for the quintic) the cubic peaks at $1.5\,\Delta q / T$ in velocity and $6\,\Delta q / T^2$ in acceleration; the quintic at $1.875\,\Delta q / T$ and $\tfrac{10}{\sqrt{3}}\,\Delta q / T^2 \approx 5.77\,\Delta q / T^2$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------|------|-------|-------| +| Coefficient solve | $O(1)$ | $O(1)$ | Closed-form expressions, once at construction or reset | +| Sample | $O(\text{degree})$ | $O(1)$ | Four Horner evaluations, at most 14 multiply-adds; no state change, so deterministic | +| Stored state | n/a | $O(1)$ | Six coefficients, the duration and the degree; stack only, no heap | + +## Step-by-Step Walkthrough + +Rest-to-rest cubic with $q_0 = 0$, $q_f = 1$, $T = 2$, $v_0 = v_f = 0$, so $\Delta q = 1$ and $a_0 = a_1 = 0$. + +| Step | Computation | Result | +|------|-------------|--------| +| $a_2$ | $(3\cdot 1 - 0)/2^2$ | $0.75$ | +| $a_3$ | $(-2\cdot 1 + 0)/2^3$ | $-0.25$ | +| Position | $0.75\,t^2 - 0.25\,t^3 = 3(t/2)^2 - 2(t/2)^3$ | $q(1) = 0.5$, $q(2) = 1$ | +| Velocity | $1.5\,t - 0.75\,t^2$ | $v(1) = 0.75 = 1.5\,\Delta q / T$ (the peak) | +| Acceleration | $1.5 - 1.5\,t$ | $+1.5$ at $t=0$, $-1.5$ at $t=2$ | +| Jerk | $6 a_3$ | $-1.5$, constant | + +The position is the smoothstep of $t/2$, symmetric about the midpoint, where the velocity peaks. The acceleration jumps from $0$ at rest to $1.5$ at the first instant and from $-1.5$ back to $0$ at the end. + +## Pitfalls & Edge Cases + +- **Duration must be positive.** The coefficients contain $1/T^k$ terms; a zero or negative duration is rejected at construction and reset. +- **Short durations in single precision.** Higher-order coefficients grow like $1/T^5$ for the quintic, and terms of opposite sign cancel near the end of the move, losing digits. +- **Clamped time holds the endpoint state.** Outside $[0, T]$ the sample is the state at $0$ or $T$, so it reports the end velocity and acceleration, not zero, when they are nonzero. +- **Acceleration steps in the cubic.** Acceleration is not matched, so at rest endpoints it jumps from or to zero, which excites flexible loads. The quintic's jerk is finite but generally nonzero at the ends. +- **No limit enforcement.** Peaks follow from $\Delta q$, $T$ and the boundary values; inconsistent boundary velocities can overshoot $q_f$ before returning. + +## Variants & Generalizations + +| Variant | Key difference | +|---------|----------------| +| **Cubic (this)** | Position and velocity matched; constant jerk; acceleration discontinuous at the ends | +| **Quintic (this)** | Adds acceleration matching; continuous acceleration at about $25\%$ higher peak velocity for rest-to-rest | +| **Path-parameter time law** | A 0 to 1 instance maps time to a normalized path coordinate; with $v_0 = v_f = 1$, $T = 1$ the cubic collapses to $s = t$ | + +## Applications + +- **Joint-space moves** — robot joints and servo axes between taught points; multi-axis moves use one instance per axis with a common $T$ so all axes arrive together. +- **Pick-and-place and CNC positioning** — fixed-time axis moves where smoothness matters more than minimum time. +- **Path timing** — the scalar time law along a geometric path such as an orientation interpolation. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|-----------|--------------| +| Trapezoidal velocity profile | Respects velocity and acceleration limits with a free duration; polynomials fix the duration and let the peaks follow | +| S-curve profile | Bounds jerk explicitly under limits; the quintic only smooths the endpoints | +| Cubic spline interpolation | Handles many via points with continuous acceleration; for two knots it reduces to this cubic | + +## References & Further Reading + +- Craig, J. J., "Introduction to Robotics: Mechanics and Control," 3rd ed., Pearson, 2005, Chapter 7 (trajectory generation). +- Biagiotti, L. and Melchiorri, C., "Trajectory Planning for Automatic Machines and Robots," Springer, 2008, §2.1. diff --git a/doc/motion_profiles/README.md b/doc/motion_profiles/README.md new file mode 100644 index 00000000..07a16fdc --- /dev/null +++ b/doc/motion_profiles/README.md @@ -0,0 +1,9 @@ +# Motion Profiles + +Time laws for point-to-point motion of a single axis: position, velocity, acceleration and jerk as functions of time, planned once and sampled every control tick. + +## Algorithms + +| Algorithm | Description | +|-------------------------------------------------|---------------------------------------------------------------------------------------------| +| [Polynomial Profile](PolynomialProfile.md) | Cubic (position + velocity) or quintic (+ acceleration) boundary-matched time law over a fixed duration | diff --git a/numerical/CMakeLists.txt b/numerical/CMakeLists.txt index 0166abbd..c61f65fb 100644 --- a/numerical/CMakeLists.txt +++ b/numerical/CMakeLists.txt @@ -4,6 +4,7 @@ add_subdirectory(controllers) add_subdirectory(estimators) add_subdirectory(filters) add_subdirectory(math) +add_subdirectory(motion_profiles) add_subdirectory(optimization) add_subdirectory(regularization) add_subdirectory(nonlinear_control) diff --git a/numerical/motion_profiles/CMakeLists.txt b/numerical/motion_profiles/CMakeLists.txt new file mode 100644 index 00000000..529feeec --- /dev/null +++ b/numerical/motion_profiles/CMakeLists.txt @@ -0,0 +1,22 @@ +numerical_add_header_library(numerical.motion_profiles) + +target_include_directories(numerical.motion_profiles ${NUMERICAL_VISIBILITY} + "$" + "$" +) + +target_link_libraries(numerical.motion_profiles ${NUMERICAL_VISIBILITY} + infra.util + numerical.math +) + +target_sources(numerical.motion_profiles PRIVATE + MotionProfileTypes.hpp + PolynomialProfile.hpp +) + +numerical_add_coverage_sources(numerical.motion_profiles + PolynomialProfile.cpp +) + +add_subdirectory(test) diff --git a/numerical/motion_profiles/MotionProfileTypes.hpp b/numerical/motion_profiles/MotionProfileTypes.hpp new file mode 100644 index 00000000..9ebf3030 --- /dev/null +++ b/numerical/motion_profiles/MotionProfileTypes.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include + +namespace motion_profiles +{ + template + struct TrajectoryState + { + static_assert(std::is_floating_point_v, "TrajectoryState supports floating-point types only"); + + T position{}; + T velocity{}; + T acceleration{}; + T jerk{}; + + bool operator==(const TrajectoryState& other) const = default; + }; + + template + struct MotionLimits + { + static_assert(std::is_floating_point_v, "MotionLimits supports floating-point types only"); + + T vMax{}; + T aMax{}; + T jMax{}; + }; +} diff --git a/numerical/motion_profiles/PolynomialProfile.cpp b/numerical/motion_profiles/PolynomialProfile.cpp new file mode 100644 index 00000000..6a756330 --- /dev/null +++ b/numerical/motion_profiles/PolynomialProfile.cpp @@ -0,0 +1,6 @@ +#include "numerical/motion_profiles/PolynomialProfile.hpp" + +namespace motion_profiles +{ + template class PolynomialProfile; +} diff --git a/numerical/motion_profiles/PolynomialProfile.hpp b/numerical/motion_profiles/PolynomialProfile.hpp new file mode 100644 index 00000000..5e4d6934 --- /dev/null +++ b/numerical/motion_profiles/PolynomialProfile.hpp @@ -0,0 +1,129 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/motion_profiles/MotionProfileTypes.hpp" +#include +#include +#include +#include + +namespace motion_profiles +{ + enum class Degree : std::uint8_t + { + Cubic = 3, + Quintic = 5 + }; + + template + struct BoundaryConditions + { + static_assert(std::is_floating_point_v, "BoundaryConditions supports floating-point types only"); + + T q0{}; + T qf{}; + T v0{}; + T vf{}; + T a0{}; + T af{}; + }; + + template + class PolynomialProfile + { + static_assert(std::is_floating_point_v, "PolynomialProfile supports floating-point types only"); + + public: + PolynomialProfile(const BoundaryConditions& bc, T tf, Degree degree); + + OPTIMIZE_FOR_SPEED TrajectoryState Sample(T t) const; + T Duration() const; + void Reset(const BoundaryConditions& bc, T tf); + + private: + void SolveCubic(const BoundaryConditions& bc); + void SolveQuintic(const BoundaryConditions& bc); + + std::array coeff{}; + T duration{}; + Degree degree; + }; + + template + PolynomialProfile::PolynomialProfile(const BoundaryConditions& bc, T tf, Degree degree) + : degree{ degree } + { + Reset(bc, tf); + } + + template + void PolynomialProfile::Reset(const BoundaryConditions& bc, T tf) + { + really_assert(tf > T{}); + + duration = tf; + if (degree == Degree::Cubic) + SolveCubic(bc); + else + SolveQuintic(bc); + } + + template + OPTIMIZE_FOR_SPEED TrajectoryState PolynomialProfile::Sample(T t) const + { + t = std::min(std::max(t, T{}), duration); + + const auto& a{ coeff }; + return { + a[0] + t * (a[1] + t * (a[2] + t * (a[3] + t * (a[4] + t * a[5])))), + a[1] + t * (T{ 2 } * a[2] + t * (T{ 3 } * a[3] + t * (T{ 4 } * a[4] + t * T{ 5 } * a[5]))), + T{ 2 } * a[2] + t * (T{ 6 } * a[3] + t * (T{ 12 } * a[4] + t * T{ 20 } * a[5])), + T{ 6 } * a[3] + t * (T{ 24 } * a[4] + t * T{ 60 } * a[5]) + }; + } + + template + T PolynomialProfile::Duration() const + { + return duration; + } + + template + void PolynomialProfile::SolveCubic(const BoundaryConditions& bc) + { + const T tf{ duration }; + const T distance{ bc.qf - bc.q0 }; + + coeff = { + bc.q0, + bc.v0, + (T{ 3 } * distance - (T{ 2 } * bc.v0 + bc.vf) * tf) / (tf * tf), + (T{ -2 } * distance + (bc.v0 + bc.vf) * tf) / (tf * tf * tf), + T{}, + T{} + }; + } + + template + void PolynomialProfile::SolveQuintic(const BoundaryConditions& bc) + { + const T tf{ duration }; + const T tf2{ tf * tf }; + const T tf3{ tf2 * tf }; + const T distance{ bc.qf - bc.q0 }; + + coeff = { + bc.q0, + bc.v0, + bc.a0 / T{ 2 }, + (T{ 20 } * distance - (T{ 8 } * bc.vf + T{ 12 } * bc.v0) * tf - (T{ 3 } * bc.a0 - bc.af) * tf2) / (T{ 2 } * tf3), + (T{ -30 } * distance + (T{ 14 } * bc.vf + T{ 16 } * bc.v0) * tf + (T{ 3 } * bc.a0 - T{ 2 } * bc.af) * tf2) / (T{ 2 } * tf3 * tf), + (T{ 12 } * distance - T{ 6 } * (bc.vf + bc.v0) * tf - (bc.a0 - bc.af) * tf2) / (T{ 2 } * tf3 * tf2) + }; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class PolynomialProfile; +#endif +} diff --git a/numerical/motion_profiles/test/CMakeLists.txt b/numerical/motion_profiles/test/CMakeLists.txt new file mode 100644 index 00000000..3c34cb39 --- /dev/null +++ b/numerical/motion_profiles/test/CMakeLists.txt @@ -0,0 +1,14 @@ +add_executable(numerical.motion_profiles_test) +emil_build_for(numerical.motion_profiles_test BOOL NUMERICAL_TOOLBOX_BUILD_TESTS) +emil_add_test(numerical.motion_profiles_test) + +target_link_libraries(numerical.motion_profiles_test PUBLIC + gmock_main + numerical.motion_profiles +) + +numerical_link_qemu_runtime(numerical.motion_profiles_test) + +target_sources(numerical.motion_profiles_test PRIVATE + TestPolynomialProfile.cpp +) diff --git a/numerical/motion_profiles/test/TestPolynomialProfile.cpp b/numerical/motion_profiles/test/TestPolynomialProfile.cpp new file mode 100644 index 00000000..0371cfde --- /dev/null +++ b/numerical/motion_profiles/test/TestPolynomialProfile.cpp @@ -0,0 +1,85 @@ +#include "numerical/motion_profiles/PolynomialProfile.hpp" +#include +#include +#include + +namespace +{ + using motion_profiles::BoundaryConditions; + using motion_profiles::Degree; + using motion_profiles::PolynomialProfile; + + class TestPolynomialProfile : public ::testing::Test + { + protected: + static constexpr float tolerance{ 1e-5f }; + const BoundaryConditions restToRest{ 0.0f, 1.0f }; + PolynomialProfile cubic{ restToRest, 2.0f, Degree::Cubic }; + }; +} + +TEST_F(TestPolynomialProfile, endpoints_match_boundary_positions) +{ + EXPECT_NEAR(cubic.Sample(0.0f).position, 0.0f, tolerance); + EXPECT_NEAR(cubic.Sample(cubic.Duration()).position, 1.0f, tolerance); +} + +TEST_F(TestPolynomialProfile, endpoint_velocities_are_matched) +{ + EXPECT_NEAR(cubic.Sample(0.0f).velocity, 0.0f, tolerance); + EXPECT_NEAR(cubic.Sample(cubic.Duration()).velocity, 0.0f, tolerance); +} + +TEST_F(TestPolynomialProfile, cubic_midpoint_is_symmetric) +{ + EXPECT_NEAR(cubic.Sample(1.0f).position, 0.5f, tolerance); +} + +TEST_F(TestPolynomialProfile, cubic_peak_velocity_matches_formula) +{ + float peak{ 0.0f }; + for (int i = 0; i <= 200; ++i) + peak = std::max(peak, std::abs(cubic.Sample(0.01f * static_cast(i)).velocity)); + + EXPECT_NEAR(peak, 1.5f * 1.0f / 2.0f, tolerance); + EXPECT_NEAR(cubic.Sample(1.0f).velocity, 0.75f, tolerance); +} + +TEST_F(TestPolynomialProfile, cubic_jerk_is_constant_third_derivative) +{ + for (float t : { 0.0f, 1.0f, 2.0f }) + EXPECT_NEAR(cubic.Sample(t).jerk, -1.5f, tolerance); +} + +TEST_F(TestPolynomialProfile, quintic_endpoint_accelerations_are_zero) +{ + const PolynomialProfile quintic{ restToRest, 2.0f, Degree::Quintic }; + + EXPECT_NEAR(quintic.Sample(0.0f).acceleration, 0.0f, tolerance); + EXPECT_NEAR(quintic.Sample(2.0f).acceleration, 0.0f, tolerance); + EXPECT_NEAR(quintic.Sample(2.0f).position, 1.0f, tolerance); + EXPECT_NEAR(quintic.Sample(2.0f).velocity, 0.0f, tolerance); +} + +TEST_F(TestPolynomialProfile, nonzero_boundary_velocity_is_honoured) +{ + const PolynomialProfile moving{ BoundaryConditions{ 0.0f, 1.0f, 0.5f, -0.5f }, 2.0f, Degree::Cubic }; + + EXPECT_NEAR(moving.Sample(0.0f).velocity, 0.5f, tolerance); + EXPECT_NEAR(moving.Sample(2.0f).velocity, -0.5f, tolerance); +} + +TEST_F(TestPolynomialProfile, sample_clamps_outside_domain) +{ + EXPECT_EQ(cubic.Sample(-1.0f), cubic.Sample(0.0f)); + EXPECT_EQ(cubic.Sample(cubic.Duration() + 1.0f), cubic.Sample(cubic.Duration())); +} + +TEST_F(TestPolynomialProfile, reset_recomputes_coefficients) +{ + cubic.Reset(BoundaryConditions{ 1.0f, 3.0f }, 1.0f); + + EXPECT_EQ(cubic.Duration(), 1.0f); + EXPECT_NEAR(cubic.Sample(0.0f).position, 1.0f, tolerance); + EXPECT_NEAR(cubic.Sample(1.0f).position, 3.0f, tolerance); +} diff --git a/roadmap/README.md b/roadmap/README.md index c28cde8b..c2814a46 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -86,7 +86,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) ### `motion_profiles` -`PolynomialProfile` (55) · `TrapezoidalProfile` (56) · `SCurveProfile` (60) +`TrapezoidalProfile` (56) · `SCurveProfile` (60) ### `robust_control` `SlidingModeControl` (34) · `DisturbanceObserver` (35) · `ActiveDisturbanceRejection` (36) · `HInfinityStateFeedback` (46) diff --git a/roadmap/motion_profiles/PolynomialProfile/explanation.md b/roadmap/motion_profiles/PolynomialProfile/explanation.md deleted file mode 100644 index a3a048f7..00000000 --- a/roadmap/motion_profiles/PolynomialProfile/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# Polynomial Point-to-Point Profile — Overview - -## What it is -A closed-form time law that drives an axis from a start state to a goal state over a fixed -duration. A **cubic** matches endpoint position and velocity; a **quintic** additionally matches -endpoint acceleration, giving a smoother (bounded-jerk) motion. - -## Why it matters (embedded) -Point-to-point moves are the bread and butter of servo motion — robot joints, CNC and pick-and-place -axes, motor position loops. The coefficients are solved **once** from a handful of algebraic -expressions, after which each servo tick is just a Horner polynomial evaluation — no iteration, no -lookup table, fully deterministic cycle count. - -## How it works (intuition) -A degree-`n` polynomial has `n+1` free coefficients. A cubic (4 coefficients) exactly satisfies -four boundary conditions — start/end position and start/end velocity. A quintic (6 coefficients) -adds start/end acceleration. Because the boundary equations are linear in the coefficients and the -time basis is fixed, the solution is a plug-in formula rather than a matrix solve. - -## Key parameters -- **Boundary conditions** — start/end position, velocity, and (quintic) acceleration. -- **Duration `tf`** — the move time; sets the peak velocity/acceleration for a given distance. -- **Degree** — cubic (velocity-matched) or quintic (acceleration-matched, jerk-limited endpoints). - -## Reference -M. W. Spong, S. Hutchinson, M. Vidyasagar, *Robot Modeling and Control*, Ch. 5 (polynomial -trajectories). - -## See also -`TrapezoidalProfile` (limit-respecting, not fixed-time), `SCurveProfile` (jerk-bounded), -`CubicSpline` (#61, many via points, C² — reduces to this cubic for two knots); robotics-toolbox-cpp's -`CartesianSlerpInterpolation` (task-space paths; a 0 → 1 polynomial can drive its time law). diff --git a/roadmap/motion_profiles/PolynomialProfile/implementation.md b/roadmap/motion_profiles/PolynomialProfile/implementation.md deleted file mode 100644 index b401898b..00000000 --- a/roadmap/motion_profiles/PolynomialProfile/implementation.md +++ /dev/null @@ -1,112 +0,0 @@ -# Polynomial Point-to-Point Profile — Implementation Pseudocode - -> Roadmap ref: #55 (Tier 1) · Target: `numerical/motion_profiles` · Namespace `motion_profiles` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -Shared motion-profile types — **canonical definition**, created once in -`numerical/motion_profiles/MotionProfileTypes.hpp` by the first `motion_profiles` item deployed; every other -profile spec includes it and never redefines these names (robotics-toolbox-cpp's time laws consume them too): - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct TrajectoryState: # one scalar axis - T position{}, velocity{}, acceleration{}, jerk{} # jerk = 0 where a profile leaves it undefined - -template # static_assert(std::is_floating_point_v); instantiated for float -struct MotionLimits: - T vMax{}, aMax{} # > 0 for every limit-driven profile - T jMax{} # > 0 required only by SCurveProfile -``` - -This item: - -```cpp -enum class Degree : uint8_t { Cubic = 3, Quintic = 5 } - -template # static_assert(std::is_floating_point_v); instantiated for float -struct BoundaryConditions: # per axis - T q0, qf # start / end position - T v0 = 0, vf = 0 # start / end velocity - T a0 = 0, af = 0 # start / end acceleration (quintic only) - -template # static_assert(std::is_floating_point_v); instantiated for float -class PolynomialProfile: - array coeff # a0..a5 (cubic uses a0..a3, a4 = a5 = 0) - T duration # tf > 0 - Degree degree -``` - -## Interface - -```text -PolynomialProfile(BoundaryConditions bc, T tf, Degree degree) -TrajectoryState Sample(T t) # hot path -T Duration() -void Reset(BoundaryConditions bc, T tf) # keeps the degree -``` - -## Algorithm (pseudocode) - -```text -function solveCubic(bc, tf): # closed form, no linear solve - a0 = bc.q0 - a1 = bc.v0 - a2 = ( 3*(bc.qf-bc.q0) - (2*bc.v0 + bc.vf)*tf) / tf^2 - a3 = (-2*(bc.qf-bc.q0) + ( bc.v0 + bc.vf)*tf) / tf^3 - a4 = a5 = 0 - -function solveQuintic(bc, tf): # six matched boundary conditions - a0 = bc.q0; a1 = bc.v0; a2 = bc.a0 / 2 - a3 = ( 20*Δq - (8*bc.vf + 12*bc.v0)*tf - (3*bc.a0 - bc.af)*tf^2) / (2*tf^3) - a4 = (-30*Δq + (14*bc.vf + 16*bc.v0)*tf + (3*bc.a0 - 2*bc.af)*tf^2) / (2*tf^4) - a5 = ( 12*Δq - ( 6*bc.vf + 6*bc.v0)*tf - ( bc.a0 - bc.af)*tf^2) / (2*tf^5) - # Δq = qf - q0 - -constructor / Reset: - degree == Degree::Cubic ? solveCubic(bc, tf) : solveQuintic(bc, tf) - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, duration) - pos = Horner(coeff, t) # a0 + a1 t + ... + a5 t^5 - vel = Horner(derivative(coeff), t) - acc = Horner(secondDerivative(coeff), t) - jerk = Horner(thirdDerivative(coeff), t) # 6 a3 + 24 a4 t + 60 a5 t^2 - return { pos, vel, acc, jerk } -``` - -## Complexity & memory - -- Coefficient solve: `O(1)` once at construction (closed-form expressions). -- `Sample`: `O(degree)` — four Horner evaluations, ≤ 14 multiply-adds. -- Memory: `O(1)` — six coefficients, a duration and the degree; all stack-resident, no heap. - -## Numerical / embedded notes - -- Solve coefficients **once** at construction; `Sample` is then pure Horner — deterministic cycles. -- Multi-axis moves: hold one instance per axis; for many via points use `math::CubicSpline` (#61). -- Guard `tf > 0`; the `1/tf^k` terms blow up for a zero-duration segment. -- Quintic gives continuous acceleration (zero jerk endpoints); prefer it when actuators are jerk-sensitive. -- A 0 → 1 instance (e.g. `v0 = vf = 1`, `tf = 1` cubic ⇒ `s = t`) is a valid path-parameter time law - (robotics-toolbox-cpp's `CartesianSlerpInterpolation` uses it that way). -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/motion_profiles/PolynomialProfile.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class PolynomialProfile;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Uses `numerical/motion_profiles/MotionProfileTypes.hpp` (create it from the block above if this is the first - `motion_profiles` item; header-only aggregates, no coverage `.cpp`). -- Coverage: `numerical/motion_profiles/PolynomialProfile.cpp` → `template class PolynomialProfile;` -- Test: `numerical/motion_profiles/test/TestPolynomialProfile.cpp` -- Doc: `doc/motion_profiles/PolynomialProfile.md` (per `doc/TEMPLATE.md`) + row in `doc/motion_profiles/README.md`. -- CMake: `.hpp` → `target_sources(numerical.motion_profiles …)`; - `numerical_add_coverage_sources(numerical.motion_profiles PolynomialProfile.cpp)`; - `TestPolynomialProfile.cpp` → `numerical.motion_profiles_test`. -- New module (first `motion_profiles` item only): `numerical/motion_profiles/CMakeLists.txt` with - `numerical_add_header_library(numerical.motion_profiles)` (links `numerical.math`), `MotionProfileTypes.hpp` in - `target_sources`, a `test/` subdir, `add_subdirectory(motion_profiles)` in `numerical/CMakeLists.txt`, - and a `doc/motion_profiles/` folder. -- Generic pattern: see `roadmap/DEPLOYMENT.md` (step 4, new module). diff --git a/roadmap/motion_profiles/PolynomialProfile/tests.md b/roadmap/motion_profiles/PolynomialProfile/tests.md deleted file mode 100644 index 82180a2c..00000000 --- a/roadmap/motion_profiles/PolynomialProfile/tests.md +++ /dev/null @@ -1,63 +0,0 @@ -# Polynomial Point-to-Point Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestPolynomialProfile : public ::testing::Test: - BoundaryConditions restToRest{ .q0 = 0, .qf = 1 } # v0=vf=0 - PolynomialProfile cubic{ restToRest, 2.0f, Degree::Cubic } -# each case below is a TEST_F(TestPolynomialProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -```text -endpoints_match_boundary_positions: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 1 - -endpoint_velocities_are_matched: - Arrange: rest-to-rest cubic - Assert: Sample(0).velocity ≈ 0 and Sample(tf).velocity ≈ 0 - -cubic_midpoint_is_symmetric: - Arrange: q0=0, qf=1, tf=2 rest-to-rest - Assert: Sample(1.0).position ≈ 0.5 (half distance at half time) - -cubic_peak_velocity_matches_formula: - Arrange: rest-to-rest, distance d, duration tf - Assert: max |velocity| ≈ 1.5 * d / tf (at midpoint) - -cubic_jerk_is_constant_third_derivative: - Arrange: q0=0, qf=1, tf=2 rest-to-rest cubic - Assert: Sample(t).jerk ≈ 6·a3 = -1.5 for t ∈ {0, 1, 2} - -quintic_endpoint_accelerations_are_zero: - Arrange: quintic, a0 = af = 0 - Assert: Sample(0).acceleration ≈ 0 and Sample(tf).acceleration ≈ 0 - -nonzero_boundary_velocity_is_honoured: - Arrange: v0 = 0.5, vf = -0.5 - Assert: Sample(0).velocity ≈ 0.5 and Sample(tf).velocity ≈ -0.5 - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) - -reset_recomputes_coefficients: - Arrange: Reset to q0=1, qf=3, tf=1 - Assert: new endpoints reproduced exactly -``` - -## Reference vectors - -- Rest-to-rest cubic, `q0=0, qf=1, tf=2`: `q(t) = 3(t/2)² − 2(t/2)³`; `q(1) = 0.5`, `v(1) = 0.75`. -- Peak velocity of a rest-to-rest cubic `= 1.5·(qf−q0)/tf`. -- Same cubic: `a3 = −2·(qf−q0)/tf³ = −0.25` ⇒ constant jerk `6·a3 = −1.5`. -- `q0=0, qf=1, v0=vf=1, tf=1` cubic ⇒ `a2 = a3 = 0`, i.e. `s(t) = t` (linear time law). - -## Edge cases - -- Very small `tf` ⇒ large coefficients; assert no overflow / `inf`. -- `q0 == qf` with zero boundary velocities ⇒ constant, zero velocity everywhere. -- Cubic vs quintic on the same move ⇒ quintic has smoother (bounded-jerk) acceleration. From 1e4763431bb36913e48f8b1a3c1ffc2b4370a47e Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:18:36 +0000 Subject: [PATCH 2/5] feat(motion_profiles): add trapezoidal (LSPB) velocity profile Deploy roadmap item 56: minimum-time trapezoid / triangle plan from (vMax, aMax), plus PlanWithDuration, which keeps aMax and lowers the cruise speed so several axes can finish together. The cruise speed uses the cancellation-free conjugate form of the smaller root, and infeasible durations return nullopt. At and after tf the profile holds the goal at rest; before t = 0 it holds the start. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 2 +- ROADMAP.md | 12 -- doc/motion_profiles/README.md | 1 + doc/motion_profiles/TrapezoidalProfile.md | 111 ++++++++++++++++ numerical/motion_profiles/CMakeLists.txt | 2 + .../motion_profiles/TrapezoidalProfile.cpp | 6 + .../motion_profiles/TrapezoidalProfile.hpp | 114 +++++++++++++++++ numerical/motion_profiles/test/CMakeLists.txt | 1 + .../test/TestTrapezoidalProfile.cpp | 118 ++++++++++++++++++ roadmap/README.md | 2 +- .../TrapezoidalProfile/explanation.md | 33 ----- .../TrapezoidalProfile/implementation.md | 108 ---------------- .../TrapezoidalProfile/tests.md | 71 ----------- 13 files changed, 355 insertions(+), 226 deletions(-) create mode 100644 doc/motion_profiles/TrapezoidalProfile.md create mode 100644 numerical/motion_profiles/TrapezoidalProfile.cpp create mode 100644 numerical/motion_profiles/TrapezoidalProfile.hpp create mode 100644 numerical/motion_profiles/test/TestTrapezoidalProfile.cpp delete mode 100644 roadmap/motion_profiles/TrapezoidalProfile/explanation.md delete mode 100644 roadmap/motion_profiles/TrapezoidalProfile/implementation.md delete mode 100644 roadmap/motion_profiles/TrapezoidalProfile/tests.md diff --git a/README.md b/README.md index 516969b6..ea00fc3c 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | | [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | -| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile | +| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | | [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | | [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | diff --git a/ROADMAP.md b/ROADMAP.md index c3aa9ced..e7ce3c75 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -26,7 +26,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| -| 56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` (new) | ★☆☆☆☆ | | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | | 60 | S-curve (jerk-limited) profile | `motion_profiles` (new) | ★★☆☆☆ | | 61 | Cubic spline interpolation (vector-valued) | `math` | ★★☆☆☆ | @@ -53,17 +52,6 @@ unit-test reference [`TESTING.md`](TESTING.md) depends on but which the library does not yet expose. Detailed below under [Evaluation & metrics primitives](#evaluation--metrics-primitives). -## Tier 1 — Trivial primitives ★☆☆☆☆ - -### 56. Trapezoidal (LSPB) profile -- **What:** Minimum-time velocity/acceleration-limited rest-to-rest move, triangular for short moves, plus - a fixed-duration plan for multi-axis synchronization. -- **Embedded value:** The standard servo-axis move (robot joints, CNC, conveyors, motor position loops). -- **Algorithm / paper:** L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and - Robots* (2008), Ch. 3. -- **Reuses:** Item 55 types. -- **Spec:** [roadmap/motion_profiles/TrapezoidalProfile](roadmap/motion_profiles/TrapezoidalProfile/implementation.md). - ## Tier 2 — Easy ★★☆☆☆ ### 53. SE(3) transform, twists and wrenches diff --git a/doc/motion_profiles/README.md b/doc/motion_profiles/README.md index 07a16fdc..5bf9c593 100644 --- a/doc/motion_profiles/README.md +++ b/doc/motion_profiles/README.md @@ -7,3 +7,4 @@ Time laws for point-to-point motion of a single axis: position, velocity, accele | Algorithm | Description | |-------------------------------------------------|---------------------------------------------------------------------------------------------| | [Polynomial Profile](PolynomialProfile.md) | Cubic (position + velocity) or quintic (+ acceleration) boundary-matched time law over a fixed duration | +| [Trapezoidal Profile](TrapezoidalProfile.md) | Minimum-time trapezoid / triangle velocity profile (LSPB) and a fixed-duration plan for multi-axis synchronization | diff --git a/doc/motion_profiles/TrapezoidalProfile.md b/doc/motion_profiles/TrapezoidalProfile.md new file mode 100644 index 00000000..c1c543a5 --- /dev/null +++ b/doc/motion_profiles/TrapezoidalProfile.md @@ -0,0 +1,111 @@ +# Trapezoidal Profile (Linear Segment with Parabolic Blends) + +## Overview & Motivation + +A point-to-point move has to reach its goal as fast as the actuator allows without saturating the drive. The trapezoidal profile — *linear segment with parabolic blends* (LSPB) — does this with the simplest rule that respects both a velocity ceiling and an acceleration ceiling: accelerate at the maximum rate, coast at the maximum speed, decelerate at the maximum rate. Velocity plotted against time is a trapezoid; a move too short to reach the ceiling becomes a triangle. + +From the limits and the distance alone, the plan is the **minimum-time** one. A second mode fixes the duration instead, which multi-axis synchronization needs: all axes must finish together, so the faster ones are slowed rather than the slowest sped up. + +## Mathematical Theory + +### Problem Statement + +Rest-to-rest move of length $d = |q_f - q_0|$ in direction $\sigma = \pm 1$ (positive when $q_f \ge q_0$), with limits $v_{max} > 0$ and $a_{max} > 0$. The profile is planned for the magnitude and mirrored by $\sigma$. + +### Minimum-Time Plan + +A ramp between rest and speed $v$ takes $v/a_{max}$ and covers $v^2/(2a_{max})$, so the two ramps together consume the **blend distance** $v^2/a_{max}$. Comparing $d$ with the blend distance at $v_{max}$ selects the shape: + +$$ +\begin{cases} +d \ge v_{max}^2/a_{max}: & v = v_{max}, \quad t_a = v/a_{max}, \quad t_c = (d - v^2/a_{max})/v \quad\text{(trapezoid)} \\ +d < v_{max}^2/a_{max}: & v = \sqrt{d\,a_{max}}, \quad t_a = v/a_{max}, \quad t_c = 0 \quad\text{(triangle)} +\end{cases} +$$ + +The total time is $T = 2t_a + t_c$. The cases agree at $d = v_{max}^2/a_{max}$, so $T$ is continuous in $d$. + +### Sampling + +With $v$, $t_a$, $t_c$, $T$ precomputed, the unsigned state is: + +| Phase | Interval | Acceleration | Velocity | Position | +|-------|----------|--------------|----------|----------| +| Parabolic ramp-up | $0 \le t < t_a$ | $a_{max}$ | $a_{max}\,t$ | $\tfrac12 a_{max}\,t^2$ | +| Linear cruise | $t_a \le t < t_a + t_c$ | $0$ | $v$ | $v\,(t - t_a/2)$ | +| Parabolic ramp-down | $t_a + t_c \le t < T$ | $-a_{max}$ | $a_{max}\,(T-t)$ | $d - \tfrac12 a_{max}\,(T-t)^2$ | + +Velocity, acceleration and the position offset are multiplied by $\sigma$. At and beyond $t = T$ the profile holds the goal at rest (velocity and acceleration zero); times before $0$ are treated as $t = 0$, holding the start with zero velocity. The reported jerk is zero in every phase. + +### Fixed-Duration Plan + +The acceleration $a_{max}$ is kept and the cruise speed is lowered until the move takes exactly $T$. The ramps cover $v\,t_a$ in total and the cruise covers $v\,(T - 2t_a)$, so with $t_a = v/a_{max}$ + +$$ +d = v\left(T - \frac{v}{a_{max}}\right) \;\Longleftrightarrow\; v^2 - a_{max}T\,v + a_{max}\,d = 0 . +$$ + +The smaller root keeps $t_c \ge 0$. Multiplying it by its conjugate gives a form with no subtraction of nearly equal numbers: + +$$ +v = \frac{a_{max}T - \sqrt{a_{max}^2T^2 - 4a_{max}d}}{2} = \frac{2\,a_{max}\,d}{a_{max}T + \sqrt{a_{max}^2T^2 - 4a_{max}d}} . +$$ + +A real root exists iff $a_{max}\,T^2 \ge 4d$. Equality gives a double root $v = a_{max}T/2$ and a triangle: $T$ is then the shortest duration achievable at this acceleration. Otherwise $t_a = v/a_{max}$ and $t_c = T - 2t_a$. + +### Multi-Axis Synchronization + +Let $T_{sync}$ be the longest axis time and $\lambda = T_{sync}/T_{axis} \ge 1$. Planning an axis again with $(v_{max}/\lambda,\; a_{max}/\lambda^2)$ leaves the blend distance $v_{max}^2/a_{max}$ unchanged, so the same shape is selected, while $t_a$ and $t_c$ both scale by $\lambda$: the duration is exactly $\lambda\,T_{axis}$. Velocity is divided by $\lambda$ and acceleration by $\lambda^2$. + +The fixed-duration plan is the alternative. It keeps $a_{max}$ and only lowers the cruise speed: the smaller root decreases as $T$ grows, so for $T \ge T_{min}$ the cruise speed never exceeds $v_{max}$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------|------|-------|-------| +| Minimum-time plan | $O(1)$ | $O(1)$ | One comparison, one square root (triangle only) | +| Fixed-duration plan | $O(1)$ | $O(1)$ | One square root, one division | +| Sample | $O(1)$ | $O(1)$ | One phase branch, a few multiply-adds; no state change | + +Memory is a fixed handful of scalars: start, direction, distance, acceleration, peak speed and three phase times. + +## Step-by-Step Walkthrough + +**Trapezoid.** $d = 5$, $v_{max} = 1$, $a_{max} = 2$. The blend distance is $1/2 = 0.5 \le 5$, so a cruise exists: $t_a = 0.5$, $t_c = (5 - 0.5)/1 = 4.5$, $T = 2(0.5) + 4.5 = 5.5$. + +| $t$ | Phase | Position | Velocity | Acceleration | +|-----|-------|----------|----------|--------------| +| $0.25$ | ramp-up | $\tfrac12\cdot 2\cdot 0.25^2 = 0.0625$ | $0.5$ | $2$ | +| $3$ | cruise | $1\cdot(3 - 0.25) = 2.75$ | $1$ | $0$ | +| $5.25$ | ramp-down | $5 - \tfrac12\cdot 2\cdot 0.25^2 = 4.9375$ | $0.5$ | $-2$ | +| $\ge 5.5$ | held | $5$ | $0$ | $0$ | + +The ramp-up ends at position $0.25$, and the cruise formula gives the same value at $t = 0.5$. The triangle threshold for these limits is $d = 0.5$; for $d = 0.25$, $v = \sqrt{0.5} \approx 0.7071$ and $T \approx 0.7071$, with no cruise. + +**Fixed duration.** $d = 4$, $a_{max} = 1$, $T = 5$: discriminant $25 - 16 = 9$, $v = 2\cdot 4/(5 + 3) = 1$, $t_a = 1$, $t_c = 3$; check $1\cdot(5 - 1) = 4$. For $d = 5$, $a_{max} = 2$, $T = 6$: discriminant $144 - 40 = 104$, $v = 20/(12 + \sqrt{104}) \approx 0.900980$. + +## Pitfalls & Edge Cases + +- **Non-positive limits.** The minimum-time plan requires $v_{max} > 0$ and $a_{max} > 0$; a violation is an assertion failure, not a recoverable status. The fixed-duration plan returns no result for $a_{max} \le 0$ or $T \le 0$. +- **Infeasible duration.** If $a_{max}T^2 < 4d$ the plan is rejected, never clamped; the shortest feasible duration is $2\sqrt{d/a_{max}}$. At the boundary, single-precision rounding can push the discriminant just below zero, so plan the slowest axis with the minimum-time plan. +- **Cancellation.** The textbook root $(a_{max}T - \sqrt{\cdot})/2$ loses its significant digits in single precision when $4a_{max}d \ll a_{max}^2T^2$; the conjugate form does not. The computed cruise time is clamped at zero. +- **Zero distance.** The minimum-time plan has $v = 0$ and $T = 0$: a zero-length profile that is at rest at the goal for every $t \ge 0$. +- **Discontinuous acceleration.** Position and velocity are continuous, but acceleration jumps between $0$ and $\pm a_{max}$ at the joins. The jerk impulses are not represented in the reported jerk and can excite structural modes; use the S-curve profile in that case. + +## Variants & Generalizations + +The **S-curve** profile adds a jerk limit (seven segments) for continuous acceleration at the cost of a longer move. **Polynomial** profiles (cubic, quintic) are fixed-duration and smooth by construction but enforce no velocity or acceleration ceiling. Unequal acceleration and deceleration limits use the same construction with unequal ramp times. + +## Applications + +- **Point-to-point axis moves** — servo and stepper drives, gantries, pick-and-place, gimbal slews. +- **Multi-axis coordination** — time-scaled or fixed-duration plans make every axis arrive together. + +## Connections to Other Algorithms + +The S-curve profile is the jerk-limited upgrade under the same velocity and acceleration limits. The polynomial profile is fixed-time and smooth but offers no limit guarantees. The saturation rate limiter slews a changing setpoint online, whereas the trapezoid plans a whole move offline. + +## References & Further Reading + +- Biagiotti, L. and Melchiorri, C., "Trajectory Planning for Automatic Machines and Robots," Springer, 2008, §3.2. +- Siciliano, B., Sciavicco, L., Villani, L., and Oriolo, G., "Robotics: Modelling, Planning and Control," Springer, 2009, §4.2. diff --git a/numerical/motion_profiles/CMakeLists.txt b/numerical/motion_profiles/CMakeLists.txt index 529feeec..1fa7ba46 100644 --- a/numerical/motion_profiles/CMakeLists.txt +++ b/numerical/motion_profiles/CMakeLists.txt @@ -13,10 +13,12 @@ target_link_libraries(numerical.motion_profiles ${NUMERICAL_VISIBILITY} target_sources(numerical.motion_profiles PRIVATE MotionProfileTypes.hpp PolynomialProfile.hpp + TrapezoidalProfile.hpp ) numerical_add_coverage_sources(numerical.motion_profiles PolynomialProfile.cpp + TrapezoidalProfile.cpp ) add_subdirectory(test) diff --git a/numerical/motion_profiles/TrapezoidalProfile.cpp b/numerical/motion_profiles/TrapezoidalProfile.cpp new file mode 100644 index 00000000..d9e5ed76 --- /dev/null +++ b/numerical/motion_profiles/TrapezoidalProfile.cpp @@ -0,0 +1,6 @@ +#include "numerical/motion_profiles/TrapezoidalProfile.hpp" + +namespace motion_profiles +{ + template class TrapezoidalProfile; +} diff --git a/numerical/motion_profiles/TrapezoidalProfile.hpp b/numerical/motion_profiles/TrapezoidalProfile.hpp new file mode 100644 index 00000000..494138b8 --- /dev/null +++ b/numerical/motion_profiles/TrapezoidalProfile.hpp @@ -0,0 +1,114 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/motion_profiles/MotionProfileTypes.hpp" +#include +#include +#include + +namespace motion_profiles +{ + template + class TrapezoidalProfile + { + static_assert(std::is_floating_point_v, "TrapezoidalProfile supports floating-point types only"); + + public: + TrapezoidalProfile(T q0, T qf, const MotionLimits& limits); + static std::optional PlanWithDuration(T q0, T qf, T aMax, T duration); + + OPTIMIZE_FOR_SPEED TrajectoryState Sample(T t) const; + T Duration() const; + bool IsTriangular() const; + + private: + TrapezoidalProfile(T q0, T qf, T aMax); + + T q0; + T direction; + T distance; + T aMax; + T vPeak{}; + T tAccel{}; + T tCruise{}; + T tf{}; + }; + + template + TrapezoidalProfile::TrapezoidalProfile(T q0, T qf, T aMax) + : q0{ q0 } + , direction{ qf >= q0 ? T{ 1 } : T{ -1 } } + , distance{ math::Abs(qf - q0) } + , aMax{ aMax } + {} + + template + TrapezoidalProfile::TrapezoidalProfile(T q0, T qf, const MotionLimits& limits) + : TrapezoidalProfile{ q0, qf, limits.aMax } + { + really_assert(limits.vMax > T{}); + really_assert(limits.aMax > T{}); + + const T blendDistance{ limits.vMax * limits.vMax / limits.aMax }; + if (distance >= blendDistance) + { + vPeak = limits.vMax; + tCruise = (distance - blendDistance) / vPeak; + } + else + vPeak = math::Sqrt(distance * aMax); + + tAccel = vPeak / aMax; + tf = T{ 2 } * tAccel + tCruise; + } + + template + std::optional> TrapezoidalProfile::PlanWithDuration(T q0, T qf, T aMax, T duration) + { + TrapezoidalProfile profile{ q0, qf, aMax }; + const T discriminant{ aMax * aMax * duration * duration - T{ 4 } * aMax * profile.distance }; + if (aMax <= T{} || duration <= T{} || discriminant < T{}) + return std::nullopt; + + profile.vPeak = T{ 2 } * aMax * profile.distance / (aMax * duration + math::Sqrt(discriminant)); + profile.tAccel = profile.vPeak / aMax; + profile.tCruise = std::max(duration - T{ 2 } * profile.tAccel, T{}); + profile.tf = duration; + return profile; + } + + template + OPTIMIZE_FOR_SPEED TrajectoryState TrapezoidalProfile::Sample(T t) const + { + t = std::max(t, T{}); + if (t >= tf) + return { q0 + direction * distance, T{}, T{}, T{} }; + + if (t < tAccel) + return { q0 + direction * T{ 0.5 } * aMax * t * t, direction * aMax * t, direction * aMax, T{} }; + + if (t < tAccel + tCruise) + return { q0 + direction * vPeak * (t - T{ 0.5 } * tAccel), direction * vPeak, T{}, T{} }; + + const T remaining{ tf - t }; + return { q0 + direction * (distance - T{ 0.5 } * aMax * remaining * remaining), direction * aMax * remaining, -direction * aMax, T{} }; + } + + template + T TrapezoidalProfile::Duration() const + { + return tf; + } + + template + bool TrapezoidalProfile::IsTriangular() const + { + return tCruise <= T{}; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class TrapezoidalProfile; +#endif +} diff --git a/numerical/motion_profiles/test/CMakeLists.txt b/numerical/motion_profiles/test/CMakeLists.txt index 3c34cb39..fc707667 100644 --- a/numerical/motion_profiles/test/CMakeLists.txt +++ b/numerical/motion_profiles/test/CMakeLists.txt @@ -11,4 +11,5 @@ numerical_link_qemu_runtime(numerical.motion_profiles_test) target_sources(numerical.motion_profiles_test PRIVATE TestPolynomialProfile.cpp + TestTrapezoidalProfile.cpp ) diff --git a/numerical/motion_profiles/test/TestTrapezoidalProfile.cpp b/numerical/motion_profiles/test/TestTrapezoidalProfile.cpp new file mode 100644 index 00000000..75e901b1 --- /dev/null +++ b/numerical/motion_profiles/test/TestTrapezoidalProfile.cpp @@ -0,0 +1,118 @@ +#include "numerical/motion_profiles/TrapezoidalProfile.hpp" +#include +#include +#include + +namespace +{ + using motion_profiles::MotionLimits; + using motion_profiles::TrapezoidalProfile; + + class TestTrapezoidalProfile : public ::testing::Test + { + protected: + static constexpr float tolerance{ 1e-5f }; + const MotionLimits limits{ 1.0f, 2.0f }; + const TrapezoidalProfile profile{ 0.0f, 5.0f, limits }; + + template + static float MaxOver(const TrapezoidalProfile& p, Metric metric) + { + float maximum{ -1e30f }; + for (int i = 0; i <= 1000; ++i) + maximum = std::max(maximum, metric(p.Sample(p.Duration() * static_cast(i) / 1000.0f))); + + return maximum; + } + }; +} + +TEST_F(TestTrapezoidalProfile, endpoints_reached_exactly) +{ + EXPECT_NEAR(profile.Sample(0.0f).position, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).position, 5.0f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, endpoints_are_at_rest) +{ + EXPECT_NEAR(profile.Sample(0.0f).velocity, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).velocity, 0.0f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, cruise_velocity_is_clamped_to_vmax) +{ + const float peak{ MaxOver(profile, [](const auto& s) + { + return s.velocity; + }) }; + + EXPECT_NEAR(peak, limits.vMax, tolerance); + EXPECT_LE(peak, limits.vMax); +} + +TEST_F(TestTrapezoidalProfile, acceleration_is_within_limit) +{ + EXPECT_LE(MaxOver(profile, [](const auto& s) + { + return std::abs(s.acceleration); + }), + limits.aMax); +} + +TEST_F(TestTrapezoidalProfile, short_move_is_triangular) +{ + const TrapezoidalProfile shortMove{ 0.0f, 0.1f, limits }; + + EXPECT_TRUE(shortMove.IsTriangular()); + EXPECT_FALSE(profile.IsTriangular()); + EXPECT_LT(MaxOver(shortMove, [](const auto& s) + { + return s.velocity; + }), + limits.vMax); + EXPECT_NEAR(shortMove.Sample(shortMove.Duration()).position, 0.1f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, phase_durations_match_trapezoid_formula) +{ + EXPECT_NEAR(profile.Duration(), 5.5f, tolerance); + EXPECT_NEAR(profile.Sample(0.25f).velocity, 0.5f, tolerance); + EXPECT_NEAR(profile.Sample(0.5f).velocity, 1.0f, tolerance); + EXPECT_NEAR(profile.Sample(5.0f).velocity, 1.0f, tolerance); + EXPECT_NEAR(profile.Sample(5.25f).velocity, 0.5f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, negative_direction_move) +{ + const TrapezoidalProfile backwards{ 2.0f, 0.0f, limits }; + + EXPECT_LE(MaxOver(backwards, [](const auto& s) + { + return s.velocity; + }), + 0.0f); + EXPECT_NEAR(backwards.Sample(0.0f).position, 2.0f, tolerance); + EXPECT_NEAR(backwards.Sample(backwards.Duration()).position, 0.0f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, plan_with_duration_lowers_cruise_speed) +{ + const auto planned{ TrapezoidalProfile::PlanWithDuration(0.0f, 4.0f, 1.0f, 5.0f) }; + + ASSERT_TRUE(planned.has_value()); + EXPECT_NEAR(planned->Duration(), 5.0f, tolerance); + EXPECT_NEAR(planned->Sample(0.5f).velocity, 0.5f, tolerance); + EXPECT_NEAR(planned->Sample(2.5f).velocity, 1.0f, tolerance); + EXPECT_NEAR(planned->Sample(5.0f).position, 4.0f, tolerance); +} + +TEST_F(TestTrapezoidalProfile, plan_with_duration_rejects_infeasible_time) +{ + EXPECT_FALSE(TrapezoidalProfile::PlanWithDuration(0.0f, 4.0f, 1.0f, 3.0f).has_value()); +} + +TEST_F(TestTrapezoidalProfile, sample_clamps_outside_domain) +{ + EXPECT_EQ(profile.Sample(-1.0f), profile.Sample(0.0f)); + EXPECT_EQ(profile.Sample(profile.Duration() + 1.0f), profile.Sample(profile.Duration())); +} diff --git a/roadmap/README.md b/roadmap/README.md index c2814a46..1f2f8c3f 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -86,7 +86,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) ### `motion_profiles` -`TrapezoidalProfile` (56) · `SCurveProfile` (60) +`SCurveProfile` (60) ### `robust_control` `SlidingModeControl` (34) · `DisturbanceObserver` (35) · `ActiveDisturbanceRejection` (36) · `HInfinityStateFeedback` (46) diff --git a/roadmap/motion_profiles/TrapezoidalProfile/explanation.md b/roadmap/motion_profiles/TrapezoidalProfile/explanation.md deleted file mode 100644 index 69106280..00000000 --- a/roadmap/motion_profiles/TrapezoidalProfile/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Overview - -## What it is -A **linear-segment-with-parabolic-blends** (LSPB) motion profile: accelerate at a constant rate, -cruise at a constant velocity, then decelerate at a constant rate. The velocity-vs-time graph is a -trapezoid (or a triangle for short moves that never reach cruise speed). - -## Why it matters (embedded) -It is the standard "get there as fast as the actuator allows" profile. Instead of fixing the -duration, you fix the **velocity and acceleration limits** and the profile computes the minimum -time that respects them — exactly what a motion controller needs to drive real hardware without -saturating drives. - -## How it works (intuition) -Time splits into three phases with a simple symmetry: the accel and decel ramps are mirror images. -If the move is long enough, velocity saturates at `vMax` and coasts; if it is short, the profile -peaks below `vMax` and immediately decelerates — a triangle. A single distance test -(`d ≥ vMax²/aMax`) decides which case applies. - -## Key parameters -- **`vMax`** — cruise velocity ceiling; caps the flat top of the trapezoid. -- **`aMax`** — ramp acceleration; sets the blend duration `vMax/aMax`. -- **Distance `qf − q0`** — decides trapezoid vs triangle and the total time. -- **Duration `T` (fixed-time variant)** — for synchronizing several axes: keep `aMax`, solve for the - lower cruise speed that finishes exactly at `T` (feasible iff `aMax·T² ≥ 4·distance`). - -## Reference -L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), -Ch. 3 (trapezoidal / LSPB profiles). - -## See also -`PolynomialProfile` (fixed-time, smooth), `SCurveProfile` (jerk-limited upgrade), -`SaturationRateLimiter` (`controllers`, online slew limiting). diff --git a/roadmap/motion_profiles/TrapezoidalProfile/implementation.md b/roadmap/motion_profiles/TrapezoidalProfile/implementation.md deleted file mode 100644 index 3d9a677c..00000000 --- a/roadmap/motion_profiles/TrapezoidalProfile/implementation.md +++ /dev/null @@ -1,108 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Implementation Pseudocode - -> Roadmap ref: #56 (Tier 1) · Target: `numerical/motion_profiles` · Namespace `motion_profiles` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -`TrajectoryState` and `MotionLimits` come from `numerical/motion_profiles/MotionProfileTypes.hpp` -(canonical definition: PolynomialProfile spec → Data structures); not redefined here. This profile -reads `vMax`, `aMax` and ignores `jMax`. - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class TrapezoidalProfile: - T q0, direction, distance # start, sign(qf - q0), |qf - q0| - T vPeak, aMax # cruise velocity actually reached, ramp acceleration - T tAccel, tCruise, tf # phase boundaries (blend, flat, total) -``` - -## Interface - -```cpp -TrapezoidalProfile(T q0, T qf, MotionLimits limits) # minimum-time plan -static std::optional PlanWithDuration(T q0, T qf, T aMax, T duration) - # fixed-time plan (synchronization) -TrajectoryState Sample(T t) # hot path; jerk = 0 -T Duration() -bool IsTriangular() # true when no cruise phase exists -``` - -## Algorithm (pseudocode) - -```cpp -function plan(q0, qf, limits): - distance = |qf - q0|; direction = sign(qf - q0); aMax = limits.aMax - dBlend = limits.vMax^2 / limits.aMax # distance used by accel + decel - if distance >= dBlend: # trapezoid: cruise phase exists - vPeak = limits.vMax - tAccel = vPeak / aMax - tCruise = (distance - dBlend) / vPeak - else: # triangle: peak below vMax - vPeak = sqrt(distance * aMax) - tAccel = vPeak / aMax - tCruise = 0 - tf = 2*tAccel + tCruise - -function PlanWithDuration(q0, qf, aMax, duration): - # d = v·(duration - v/aMax) ⇒ v² - aMax·duration·v + aMax·d = 0, take the smaller root - d = |qf - q0| - disc = aMax^2 * duration^2 - 4*aMax*d - if aMax <= 0 or duration <= 0 or disc < 0: # feasible iff aMax·duration² ≥ 4d - return nullopt - vPeak = 2*aMax*d / (aMax*duration + sqrt(disc)) # = (aMax·duration − sqrt(disc))/2, cancellation-free - tAccel = vPeak / aMax - tCruise = duration - 2*tAccel - tf = duration - return profile{ q0, sign(qf - q0), d, vPeak, aMax, tAccel, tCruise, tf } - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, tf) - if t < tAccel: # parabolic ramp-up - acc = aMax; vel = aMax*t; s = 0.5*aMax*t^2 - else if t < tAccel + tCruise: # linear cruise - acc = 0; vel = vPeak; s = vPeak*(t - 0.5*tAccel) - else: # parabolic ramp-down - td = tf - t - acc = -aMax; vel = aMax*td; s = distance - 0.5*aMax*td^2 - return { q0 + direction*s, direction*vel, direction*acc, 0 } -``` - -## Complexity & memory - -- Planning (both variants): `O(1)` — one `sqrt` and a branch. -- `Sample`: `O(1)` — one phase branch, a couple of multiply-adds. -- Memory: `O(1)` — eight scalars; entirely stack-resident, no heap. - -## Numerical / embedded notes - -- Precompute the three phase boundaries **once**; `Sample` then reduces to one branch per tick. -- Velocity is continuous but acceleration is **discontinuous** at blend joins (bounded jerk spikes) — - upgrade to `SCurveProfile` when that excites structural modes. -- Guard degenerate inputs: `distance == 0` ⇒ zero-length profile; `aMax <= 0` or `vMax <= 0` ⇒ reject. -- **Synchronization:** multi-axis moves finish together by uniform time scaling of every non-slowest - axis by `λ = T_sync / T_axis ≥ 1` (`T_sync` = largest `Duration()`): its velocity, acceleration and - jerk become `v/λ, a/λ², j/λ³` and it is sampled at `t/λ`. Replanning that axis with limits - `(vMax/λ, aMax/λ²)` yields exactly `Duration() = λ·T_axis` (same branch, same shape). - `PlanWithDuration(q0, qf, aMax, T_sync)` is the alternative that keeps `aMax` and only lowers the cruise - speed; for `T_sync ≥` the minimum-time duration its `vPeak ≤ vMax` (the root decreases with duration). -- `PlanWithDuration` uses the conjugate form of the smaller root — the textbook - `(aMax·T − sqrt(disc))/2` cancels catastrophically in `float` when `4·aMax·d ≪ aMax²·T²`. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/motion_profiles/TrapezoidalProfile.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class TrapezoidalProfile;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Uses `numerical/motion_profiles/MotionProfileTypes.hpp` (create it if this is the first `motion_profiles` item). -- Coverage: `numerical/motion_profiles/TrapezoidalProfile.cpp` → `template class TrapezoidalProfile;` -- Test: `numerical/motion_profiles/test/TestTrapezoidalProfile.cpp` -- Doc: `doc/motion_profiles/TrapezoidalProfile.md` (per `doc/TEMPLATE.md`) + row in `doc/motion_profiles/README.md`. -- CMake: `.hpp` → `target_sources(numerical.motion_profiles …)`; - `numerical_add_coverage_sources(numerical.motion_profiles TrapezoidalProfile.cpp)`; - `TestTrapezoidalProfile.cpp` → `numerical.motion_profiles_test`. -- New module (first `motion_profiles` item only): `numerical/motion_profiles/CMakeLists.txt` with - `numerical_add_header_library(numerical.motion_profiles)`, `MotionProfileTypes.hpp` in `target_sources`, a - `test/` subdir, `add_subdirectory(motion_profiles)` in `numerical/CMakeLists.txt`, and a `doc/motion_profiles/` folder. -- Generic pattern: see `roadmap/DEPLOYMENT.md` (step 4, new module). diff --git a/roadmap/motion_profiles/TrapezoidalProfile/tests.md b/roadmap/motion_profiles/TrapezoidalProfile/tests.md deleted file mode 100644 index 60c59bd1..00000000 --- a/roadmap/motion_profiles/TrapezoidalProfile/tests.md +++ /dev/null @@ -1,71 +0,0 @@ -# Trapezoidal (LSPB) Velocity Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestTrapezoidalProfile : public ::testing::Test: - MotionLimits limits{ .vMax = 1.0f, .aMax = 2.0f } - TrapezoidalProfile profile{ 0.0f, 5.0f, limits } # long move ⇒ trapezoid -# each case below is a TEST_F(TestTrapezoidalProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -endpoints_reached_exactly: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 5 - -endpoints_are_at_rest: - Assert: Sample(0).velocity ≈ 0 and Sample(tf).velocity ≈ 0 - -cruise_velocity_is_clamped_to_vmax: - Arrange: long move - Assert: max velocity over the profile ≈ vMax (1.0), never exceeds it - -acceleration_is_within_limit: - Assert: |acceleration| ≤ aMax at all sampled instants - -short_move_is_triangular: - Arrange: q0=0, qf=0.1 (d < vMax²/aMax) - Assert: IsTriangular() == true and peak velocity < vMax - -phase_durations_match_trapezoid_formula: - Arrange: d=5, vMax=1, aMax=2 - Assert: tAccel ≈ 0.5, tCruise ≈ 4.5, Duration ≈ 5.5 - -negative_direction_move: - Arrange: q0=2, qf=0 - Assert: velocity ≤ 0 throughout, endpoints (2 → 0) reached - -plan_with_duration_lowers_cruise_speed: - Arrange: PlanWithDuration(0, 4, aMax=1, duration=5) - Assert: has_value; Duration() ≈ 5; Sample(0.5).velocity ≈ 0.5 (ramp at aMax); - Sample(2.5).velocity ≈ 1 (cruise vPeak = 1 < aMax·T/2); Sample(5).position ≈ 4 - -plan_with_duration_rejects_infeasible_time: - Arrange: PlanWithDuration(0, 4, aMax=1, duration=3) # aMax·T² = 9 < 4d = 16 - Assert: == nullopt - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) -``` - -## Reference vectors - -- `d=5, vMax=1, aMax=2`: `tAccel = vMax/aMax = 0.5`, `tCruise = (d − vMax²/aMax)/vMax = 4.5`, - `tf = 5.5`. -- Triangular threshold distance `= vMax²/aMax = 0.5`. -- `PlanWithDuration`, `d=4, aMax=1, T=5`: `v = (aMax·T − sqrt(aMax²T² − 4·aMax·d))/2 = (5 − 3)/2 = 1`, - `tAccel = 1`, `tCruise = 3`. `d=5, aMax=2, T=6`: `v = 0.900980`, `tAccel = 0.450490`, - `tCruise = 5.099020`. Feasible iff `aMax·T² ≥ 4d` (equality ⇒ triangle, `v = aMax·T/2`). -- Synchronization: replanning with `(vMax/λ, aMax/λ²)` scales `Duration()` by exactly `λ` - (e.g. fixture, `λ = 2` ⇒ `11.0`). - -## Edge cases - -- `d` exactly equal to `vMax²/aMax` ⇒ zero cruise time (trapezoid degenerates to triangle). -- `d == 0` ⇒ `tf == 0`, `Sample(0)` is the start at rest. -- `aMax` very large ⇒ ramps vanish, profile approaches a pure velocity step. -- `PlanWithDuration` with `d == 0` ⇒ `vPeak = 0`, holds `q0` at rest for the whole duration. From df02b790ebaf15f003304345229ae93bed0a6689 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:18:45 +0000 Subject: [PATCH 3/5] feat(motion_profiles): add S-curve (jerk-limited) profile MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 60: seven-phase rest-to-rest profile planned with the closed-form three-step method of Biagiotti & Melchiorri §3.4 (vMax reached; else aMax reached; else neither). The boundary states are integrated once, and each sample evaluates one constant-jerk cubic. Add math::Cbrt (generic wrapper, no ARM override), which the step-3 branch needs. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 2 +- ROADMAP.md | 8 - doc/motion_profiles/README.md | 1 + doc/motion_profiles/SCurveProfile.md | 120 +++++++++++++ numerical/math/Math.cpp | 3 + numerical/math/Math.hpp | 12 ++ numerical/math/test/TestMath.cpp | 6 + numerical/motion_profiles/CMakeLists.txt | 2 + numerical/motion_profiles/SCurveProfile.cpp | 6 + numerical/motion_profiles/SCurveProfile.hpp | 157 ++++++++++++++++++ numerical/motion_profiles/test/CMakeLists.txt | 1 + .../test/TestSCurveProfile.cpp | 154 +++++++++++++++++ roadmap/README.md | 2 - .../SCurveProfile/explanation.md | 35 ---- .../SCurveProfile/implementation.md | 122 -------------- .../motion_profiles/SCurveProfile/tests.md | 87 ---------- 16 files changed, 463 insertions(+), 255 deletions(-) create mode 100644 doc/motion_profiles/SCurveProfile.md create mode 100644 numerical/motion_profiles/SCurveProfile.cpp create mode 100644 numerical/motion_profiles/SCurveProfile.hpp create mode 100644 numerical/motion_profiles/test/TestSCurveProfile.cpp delete mode 100644 roadmap/motion_profiles/SCurveProfile/explanation.md delete mode 100644 roadmap/motion_profiles/SCurveProfile/implementation.md delete mode 100644 roadmap/motion_profiles/SCurveProfile/tests.md diff --git a/README.md b/README.md index ea00fc3c..82e992f6 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | | [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | -| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile | +| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | | [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | | [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | diff --git a/ROADMAP.md b/ROADMAP.md index e7ce3c75..f4f382d3 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -27,7 +27,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | -| 60 | S-curve (jerk-limited) profile | `motion_profiles` (new) | ★★☆☆☆ | | 61 | Cubic spline interpolation (vector-valued) | `math` | ★★☆☆☆ | | 62 | Damped pseudo-inverse + null-space projector | `solvers` | ★★☆☆☆ | | 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ | @@ -66,13 +65,6 @@ the library does not yet expose. Detailed below under - **Reuses:** `Geometry3D` (`RotationAboutAxis`, `SkewSymmetric`), `Quaternion`. - **Spec:** [roadmap/math/SE3Transform](roadmap/math/SE3Transform/implementation.md). -### 60. S-curve (jerk-limited) profile -- **What:** Seven-segment jerk-bounded rest-to-rest move with all short-move cases in closed form. -- **Embedded value:** Continuous acceleration — far less vibration than a trapezoid for precision axes. -- **Algorithm / paper:** L. Biagiotti, C. Melchiorri (2008), §3.4. -- **Reuses:** Items 55 (types) and 56 (synchronization scheme). -- **Spec:** [roadmap/motion_profiles/SCurveProfile](roadmap/motion_profiles/SCurveProfile/implementation.md). - ### 61. Cubic spline interpolation - **What:** C² vector-valued spline through `K` knots, clamped or natural ends, shared factorization across channels. diff --git a/doc/motion_profiles/README.md b/doc/motion_profiles/README.md index 5bf9c593..f1043b1d 100644 --- a/doc/motion_profiles/README.md +++ b/doc/motion_profiles/README.md @@ -8,3 +8,4 @@ Time laws for point-to-point motion of a single axis: position, velocity, accele |-------------------------------------------------|---------------------------------------------------------------------------------------------| | [Polynomial Profile](PolynomialProfile.md) | Cubic (position + velocity) or quintic (+ acceleration) boundary-matched time law over a fixed duration | | [Trapezoidal Profile](TrapezoidalProfile.md) | Minimum-time trapezoid / triangle velocity profile (LSPB) and a fixed-duration plan for multi-axis synchronization | +| [S-Curve Profile](SCurveProfile.md) | Seven-phase jerk-limited rest-to-rest profile with a closed-form three-step planner — continuous acceleration | diff --git a/doc/motion_profiles/SCurveProfile.md b/doc/motion_profiles/SCurveProfile.md new file mode 100644 index 00000000..d36af22d --- /dev/null +++ b/doc/motion_profiles/SCurveProfile.md @@ -0,0 +1,120 @@ +# S-Curve (Jerk-Limited) Profile + +## Overview & Motivation + +A trapezoidal velocity profile bounds speed and acceleration, but acceleration jumps instantaneously at every corner. Those jumps are impulses in jerk: they excite structural resonances, wear gearing, and make payloads wobble or liquids spill. The S-curve (double-S) profile also bounds **jerk**, so acceleration ramps linearly in and out and is continuous, giving far less vibration than a trapezoid. The price is a slightly longer move. + +For a rest-to-rest move the phase durations have a closed form: planning is a handful of arithmetic operations, with no iteration. Three limits drive it: a velocity ceiling $v_{\max}$, an acceleration ceiling $a_{\max}$ and a jerk ceiling $j_{\max}$. Short moves never reach the ceilings; the corresponding phases shrink to zero length. + +## Mathematical Theory + +### Phase Structure + +A rest-to-rest move of length $h > 0$ is built from seven constant-jerk phases: + +| Phase | Jerk | Acceleration | Duration | +|-------|------|--------------|----------| +| 1 | $+j_{\max}$ | ramps $0 \to a_{lim}$ | $T_j$ | +| 2 | $0$ | constant $a_{lim}$ | $T_a - 2T_j$ | +| 3 | $-j_{\max}$ | ramps $a_{lim} \to 0$ | $T_j$ | +| 4 | $0$ | zero, cruise at $v_{lim}$ | $T_v$ | +| 5 | $-j_{\max}$ | ramps $0 \to -a_{lim}$ | $T_j$ | +| 6 | $0$ | constant $-a_{lim}$ | $T_a - 2T_j$ | +| 7 | $+j_{\max}$ | ramps $-a_{lim} \to 0$ | $T_j$ | + +Phases 1–3 form the acceleration stage of length $T_a$; phases 5–7 mirror it, so the profile is symmetric about its midpoint and the total time is $T = 2T_a + T_v$. Integrating the stage gives the peaks and the distance: + +$$a_{lim} = j_{\max} T_j, \qquad v_{lim} = a_{lim}\,(T_a - T_j), \qquad h = v_{lim}\,(T_a + T_v)$$ + +### Closed-Form Three-Step Planner + +This is the case $v_0 = v_1 = 0$ of Biagiotti and Melchiorri, §3.4. Each step assumes one more ceiling is unreachable. + +**Step 1 — assume $v_{\max}$ is reached** ($v_{lim} = v_{\max}$): + +$$ +\text{if } v_{\max} j_{\max} \ge a_{\max}^2:\; T_j = \frac{a_{\max}}{j_{\max}},\; T_a = T_j + \frac{v_{\max}}{a_{\max}} +\qquad\text{else}\qquad +T_j = \sqrt{\frac{v_{\max}}{j_{\max}}},\; T_a = 2T_j +$$ + +$$T_v = \frac{h}{v_{\max}} - T_a$$ + +The first case has an acceleration plateau; the second has a pure triangular ramp with peak $j_{\max}T_j < a_{\max}$. Here $T = h/v_{\max} + T_a$, so the jerk limit costs exactly $a_{\max}/j_{\max}$ over a trapezoid with the same $v_{\max}, a_{\max}$. + +**Step 2 — if $T_v < 0$**, set $T_v = 0$ and assume $a_{\max}$ is still reached, $T_j = a_{\max}/j_{\max}$. Then $h = a_{\max} T_a (T_a - T_j)$ is a quadratic in $T_a$: + +$$\Delta = \frac{a_{\max}^4}{j_{\max}^2} + 4\,a_{\max} h, \qquad T_a = \frac{a_{\max}^2/j_{\max} + \sqrt{\Delta}}{2\,a_{\max}}$$ + +**Step 3 — if $T_a < 2T_j$**, the plateau would have negative length, so $a_{\max}$ is unreachable too. Both ramps are triangles, $T_a = 2T_j$ and $h = 2 j_{\max} T_j^3$: + +$$T_j = \sqrt[3]{\frac{h}{2\,j_{\max}}}, \qquad T_a = 2T_j$$ + +Step 2 applies when $h < v_{\max} T_a$, and step 3 when $h < 2 a_{\max}^3 / j_{\max}^2$ (from $T_a \ge 2T_j \iff \Delta \ge 9 a_{\max}^4/j_{\max}^2$). Neighbouring branches agree on $T$ at each threshold, so $T$ is continuous in $h$. No duration is ever negative: in step 1, $T_a - 2T_j = v_{\max}/a_{\max} - a_{\max}/j_{\max} \ge 0$ is the branch condition itself; step 2 is guarded by the step-3 test; step 3 has $T_a - 2T_j = 0$. Zero-length phases collapse out of the profile. + +### Exact Evaluation + +Within a phase of jerk $j_i$ starting from state $(s_i, v_i, a_i)$, with local time $\tau$: + +$$a = a_i + j_i\tau, \qquad v = v_i + a_i\tau + \tfrac12 j_i\tau^2, \qquad s = s_i + v_i\tau + \tfrac12 a_i\tau^2 + \tfrac16 j_i\tau^3$$ + +The seven boundary states are integrated exactly once, at planning time, with these polynomials. Each sample then locates its phase and evaluates a cubic in time. At and beyond $T$ the profile holds the goal at rest; before $t = 0$ it holds the start. A negative move direction negates the offset, velocity, acceleration and jerk. + +### Time Scaling for Synchronization + +Replanning with $(v_{\max}/\lambda,\; a_{\max}/\lambda^2,\; j_{\max}/\lambda^3)$ stretches an axis by $\lambda \ge 1$. Every planner term scales consistently ($T_j \to \lambda T_j$, $T_a \to \lambda T_a$, $\Delta \to \lambda^{-2}\Delta$) and the branch tests are invariant, so the duration scales by exactly $\lambda$ in every branch. Axes stretched to a common $T$ finish together. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------|------|-------|-------| +| Plan | $O(1)$ | $O(1)$ | At most one square root and one cube root, no iteration | +| Sample | $O(1)$ | $O(1)$ | Locate one of seven phases, evaluate one cubic | + +Stored state is a few fixed-size tables of phase boundaries on the stack; no heap is used. Cost is independent of $h$ and of the branch taken. + +## Step-by-Step Walkthrough + +Limits $(v_{\max}, a_{\max}, j_{\max}) = (1, 2, 10)$, $h = 0.3$. + +1. Step 1: $v_{\max} j_{\max} = 10 \ge 4$, so $T_j = 0.2$, $T_a = 0.7$, $T_v = 0.3 - 0.7 = -0.4 < 0$. +2. Step 2: $T_v = 0$, $\Delta = 0.16 + 2.4 = 2.56$, $T_a = (0.4 + 1.6)/4 = 0.5 \ge 2T_j = 0.4$, so step 3 is not needed. Then $a_{lim} = 2$, $v_{lim} = 2(0.5 - 0.2) = 0.6$, $T = 1.0$. + +Boundary states $(a, v, s)$ of the acceleration stage: at $t = 0.2$, $(2,\, 0.2,\, 0.0133)$; at $t = 0.3$, $(2,\, 0.4,\, 0.0433)$; at $t = 0.5$, $(0,\, 0.6,\, 0.15)$. The mirrored stage ends at $s = 0.3$, at rest. A sample at $t = 0.4$ lies in phase 3 with $\tau = 0.1$: $a = 1$, $v = 0.55$, $s = 0.0917$. + +| $(v_{\max}, a_{\max}, j_{\max})$ | $h$ | Branch | $T_j$ | $T_a$ | $T_v$ | $T$ | +|----------------------------------|-----|--------|-------|-------|-------|-----| +| (1, 2, 10) | 5 | 1, plateau | 0.2 | 0.7 | 4.3 | 5.7 | +| (1, 2, 1) | 5 | 1, triangle | 1 | 2 | 3 | 7 | +| (1, 2, 10) | 0.3 | 2 | 0.2 | 0.5 | 0 | 1.0 | +| (1, 2, 10) | 0.02 | 3 | 0.1 | 0.2 | 0 | 0.4 | + +For $(1, 2, 10)$ the thresholds are $h \ge 0.7$ for step 1 and $h \ge 2a_{\max}^3/j_{\max}^2 = 0.16$ for step 2. The last row has $a_{lim} = 1$ and $v_{lim} = 0.1$. + +## Pitfalls & Edge Cases + +- **Positive limits.** The velocity, acceleration and jerk ceilings must all be strictly positive. +- **Zero distance.** With $h = 0$ every phase has zero length; the profile is the start state, at rest. +- **Rest-to-rest only.** Non-zero boundary velocities need the iterative acceleration reduction of the general case, which is not covered. +- **Float-only.** Only floating-point types are supported; there is no fixed-point variant. +- **Validation.** On 20 000 random $(h, v_{\max}, a_{\max}, j_{\max})$ plans, every plan ended at $h$ at rest and never exceeded the limits. + +## Variants & Generalizations + +- **Trapezoid** — the limit $j_{\max} \to \infty$: jerk phases vanish and acceleration steps. +- **Non-zero boundary velocities** — same seven phases, with the acceleration ceiling reduced iteratively until the move is feasible. + +## Applications + +- **Pick-and-place, CNC, semiconductor handling** — fast moves without exciting resonances or payload sway; time-scaled axes finish together. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|-----------|--------------| +| Trapezoidal profile | Jerk-unbounded predecessor; shares the synchronization scheme | +| Quintic polynomial profile | Fixed duration and smooth, but limits are not enforced directly | + +## References & Further Reading + +- Biagiotti, L. and Melchiorri, C., *Trajectory Planning for Automatic Machines and Robots*, Springer, 2008, §3.4. diff --git a/numerical/math/Math.cpp b/numerical/math/Math.cpp index 6146cec2..1d258b3b 100644 --- a/numerical/math/Math.cpp +++ b/numerical/math/Math.cpp @@ -8,6 +8,9 @@ template float math::Abs(float); #ifndef MATH_SQRT_OVERRIDE template float math::Sqrt(float); #endif +#ifndef MATH_CBRT_OVERRIDE +template float math::Cbrt(float); +#endif #ifndef MATH_SIN_OVERRIDE template float math::Sin(float); #endif diff --git a/numerical/math/Math.hpp b/numerical/math/Math.hpp index eed45f71..0d279a58 100644 --- a/numerical/math/Math.hpp +++ b/numerical/math/Math.hpp @@ -25,6 +25,15 @@ namespace math } #endif +#ifndef MATH_CBRT_OVERRIDE + template + constexpr T Cbrt(T x) + { + static_assert(std::is_floating_point_v, "T must be a floating-point type"); + return std::cbrt(x); + } +#endif + #ifndef MATH_SIN_OVERRIDE template constexpr T Sin(T x) @@ -252,6 +261,9 @@ namespace math #ifndef MATH_SQRT_OVERRIDE extern template float Sqrt(float); #endif +#ifndef MATH_CBRT_OVERRIDE + extern template float Cbrt(float); +#endif #ifndef MATH_SIN_OVERRIDE extern template float Sin(float); #endif diff --git a/numerical/math/test/TestMath.cpp b/numerical/math/test/TestMath.cpp index 8f8d34f8..595d7240 100644 --- a/numerical/math/test/TestMath.cpp +++ b/numerical/math/test/TestMath.cpp @@ -127,6 +127,12 @@ TEST_F(TestMath, Copysign_transfers_sign) EXPECT_FLOAT_EQ(math::Copysign(-3.0f, 1.0f), 3.0f); } +TEST_F(TestMath, Cbrt_known_value) +{ + EXPECT_NEAR(math::Cbrt(27.0f), 3.0f, kEps); + EXPECT_NEAR(math::Cbrt(-0.001f), -0.1f, kEps); +} + TEST_F(TestMath, Fmod_known_value) { EXPECT_NEAR(math::Fmod(5.5f, 2.0f), 1.5f, kEps); diff --git a/numerical/motion_profiles/CMakeLists.txt b/numerical/motion_profiles/CMakeLists.txt index 1fa7ba46..e5022541 100644 --- a/numerical/motion_profiles/CMakeLists.txt +++ b/numerical/motion_profiles/CMakeLists.txt @@ -13,11 +13,13 @@ target_link_libraries(numerical.motion_profiles ${NUMERICAL_VISIBILITY} target_sources(numerical.motion_profiles PRIVATE MotionProfileTypes.hpp PolynomialProfile.hpp + SCurveProfile.hpp TrapezoidalProfile.hpp ) numerical_add_coverage_sources(numerical.motion_profiles PolynomialProfile.cpp + SCurveProfile.cpp TrapezoidalProfile.cpp ) diff --git a/numerical/motion_profiles/SCurveProfile.cpp b/numerical/motion_profiles/SCurveProfile.cpp new file mode 100644 index 00000000..2550fec2 --- /dev/null +++ b/numerical/motion_profiles/SCurveProfile.cpp @@ -0,0 +1,6 @@ +#include "numerical/motion_profiles/SCurveProfile.hpp" + +namespace motion_profiles +{ + template class SCurveProfile; +} diff --git a/numerical/motion_profiles/SCurveProfile.hpp b/numerical/motion_profiles/SCurveProfile.hpp new file mode 100644 index 00000000..88e472d5 --- /dev/null +++ b/numerical/motion_profiles/SCurveProfile.hpp @@ -0,0 +1,157 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/motion_profiles/MotionProfileTypes.hpp" +#include +#include +#include +#include + +namespace motion_profiles +{ + template + class SCurveProfile + { + static_assert(std::is_floating_point_v, "SCurveProfile supports floating-point types only"); + + public: + SCurveProfile(T q0, T qf, const MotionLimits& limits); + + OPTIMIZE_FOR_SPEED TrajectoryState Sample(T t) const; + T Duration() const; + bool ReachesMaxAccel() const; + bool ReachesMaxVel() const; + + private: + static constexpr std::size_t phases{ 7 }; + static constexpr std::array jerkSign{ T{ 1 }, T{}, T{ -1 }, T{}, T{ -1 }, T{}, T{ 1 } }; + + void Plan(const MotionLimits& limits); + void Integrate(T jerkPhase, T accelPhase, T cruisePhase); + + T q0; + T direction; + T distance; + T jMax; + std::array segStart{}; + std::array segPos{}; + std::array segVel{}; + std::array segAcc{}; + T tf{}; + bool reachesMaxAccel{ false }; + bool reachesMaxVel{ false }; + }; + + template + SCurveProfile::SCurveProfile(T q0, T qf, const MotionLimits& limits) + : q0{ q0 } + , direction{ qf >= q0 ? T{ 1 } : T{ -1 } } + , distance{ math::Abs(qf - q0) } + , jMax{ limits.jMax } + { + really_assert(limits.vMax > T{}); + really_assert(limits.aMax > T{}); + really_assert(limits.jMax > T{}); + + if (distance > T{}) + Plan(limits); + } + + template + void SCurveProfile::Plan(const MotionLimits& limits) + { + const T vMax{ limits.vMax }; + const T aMax{ limits.aMax }; + + T tj{ aMax / jMax }; + T ta{ tj + vMax / aMax }; + if (vMax * jMax < aMax * aMax) + { + tj = math::Sqrt(vMax / jMax); + ta = T{ 2 } * tj; + } + + T tv{ distance / vMax - ta }; + if (tv < T{}) + { + tv = T{}; + tj = aMax / jMax; + const T delta{ aMax * aMax * aMax * aMax / (jMax * jMax) + T{ 4 } * aMax * distance }; + ta = (aMax * aMax / jMax + math::Sqrt(delta)) / (T{ 2 } * aMax); + + if (ta < T{ 2 } * tj) + { + tj = math::Cbrt(distance / (T{ 2 } * jMax)); + ta = T{ 2 } * tj; + } + } + + const T accelPhase{ std::max(ta - T{ 2 } * tj, T{}) }; + reachesMaxAccel = accelPhase > T{}; + reachesMaxVel = tv > T{}; + Integrate(tj, accelPhase, tv); + } + + template + void SCurveProfile::Integrate(T jerkPhase, T accelPhase, T cruisePhase) + { + const std::array duration{ jerkPhase, accelPhase, jerkPhase, cruisePhase, jerkPhase, accelPhase, jerkPhase }; + + for (std::size_t i = 0; i < phases; ++i) + { + const T tau{ duration[i] }; + const T jerk{ jerkSign[i] * jMax }; + segPos[i + 1] = segPos[i] + tau * (segVel[i] + tau * (segAcc[i] / T{ 2 } + tau * jerk / T{ 6 })); + segVel[i + 1] = segVel[i] + tau * (segAcc[i] + tau * jerk / T{ 2 }); + segAcc[i + 1] = segAcc[i] + tau * jerk; + segStart[i + 1] = segStart[i] + tau; + } + + tf = segStart[phases]; + } + + template + OPTIMIZE_FOR_SPEED TrajectoryState SCurveProfile::Sample(T t) const + { + t = std::max(t, T{}); + if (t >= tf) + return { q0 + direction * distance, T{}, T{}, T{} }; + + std::size_t i{ 0 }; + while (i + 1 < phases && segStart[i + 1] <= t) + ++i; + + const T tau{ t - segStart[i] }; + const T jerk{ jerkSign[i] * jMax }; + return { + q0 + direction * (segPos[i] + tau * (segVel[i] + tau * (segAcc[i] / T{ 2 } + tau * jerk / T{ 6 }))), + direction * (segVel[i] + tau * (segAcc[i] + tau * jerk / T{ 2 })), + direction * (segAcc[i] + tau * jerk), + direction * jerk + }; + } + + template + T SCurveProfile::Duration() const + { + return tf; + } + + template + bool SCurveProfile::ReachesMaxAccel() const + { + return reachesMaxAccel; + } + + template + bool SCurveProfile::ReachesMaxVel() const + { + return reachesMaxVel; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class SCurveProfile; +#endif +} diff --git a/numerical/motion_profiles/test/CMakeLists.txt b/numerical/motion_profiles/test/CMakeLists.txt index fc707667..24459986 100644 --- a/numerical/motion_profiles/test/CMakeLists.txt +++ b/numerical/motion_profiles/test/CMakeLists.txt @@ -11,5 +11,6 @@ numerical_link_qemu_runtime(numerical.motion_profiles_test) target_sources(numerical.motion_profiles_test PRIVATE TestPolynomialProfile.cpp + TestSCurveProfile.cpp TestTrapezoidalProfile.cpp ) diff --git a/numerical/motion_profiles/test/TestSCurveProfile.cpp b/numerical/motion_profiles/test/TestSCurveProfile.cpp new file mode 100644 index 00000000..ec14baec --- /dev/null +++ b/numerical/motion_profiles/test/TestSCurveProfile.cpp @@ -0,0 +1,154 @@ +#include "numerical/motion_profiles/SCurveProfile.hpp" +#include +#include +#include + +namespace +{ + using motion_profiles::MotionLimits; + using motion_profiles::SCurveProfile; + using motion_profiles::TrajectoryState; + + class TestSCurveProfile : public ::testing::Test + { + protected: + static constexpr float tolerance{ 1e-5f }; + static constexpr int samples{ 5000 }; + const MotionLimits limits{ 1.0f, 2.0f, 10.0f }; + const SCurveProfile profile{ 0.0f, 5.0f, limits }; + + static TrajectoryState SampleAt(const SCurveProfile& p, int i) + { + return p.Sample(p.Duration() * static_cast(i) / static_cast(samples)); + } + + template + static float MaxOver(const SCurveProfile& p, Metric metric) + { + float maximum{ 0.0f }; + for (int i = 0; i <= samples; ++i) + maximum = std::max(maximum, metric(SampleAt(p, i))); + + return maximum; + } + }; +} + +TEST_F(TestSCurveProfile, endpoints_reached_exactly) +{ + EXPECT_NEAR(profile.Sample(0.0f).position, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).position, 5.0f, tolerance); +} + +TEST_F(TestSCurveProfile, endpoints_at_rest_and_zero_accel) +{ + EXPECT_NEAR(profile.Sample(0.0f).velocity, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(0.0f).acceleration, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).velocity, 0.0f, tolerance); + EXPECT_NEAR(profile.Sample(profile.Duration()).acceleration, 0.0f, tolerance); +} + +TEST_F(TestSCurveProfile, velocity_never_exceeds_vmax) +{ + EXPECT_LE(MaxOver(profile, [](const auto& s) + { + return std::abs(s.velocity); + }), + limits.vMax + tolerance); +} + +TEST_F(TestSCurveProfile, acceleration_never_exceeds_amax) +{ + EXPECT_LE(MaxOver(profile, [](const auto& s) + { + return std::abs(s.acceleration); + }), + limits.aMax + tolerance); +} + +TEST_F(TestSCurveProfile, jerk_never_exceeds_jmax) +{ + for (int i = 0; i <= samples; ++i) + { + const float jerk{ std::abs(SampleAt(profile, i).jerk) }; + EXPECT_TRUE(jerk == 0.0f || jerk == limits.jMax) << "jerk = " << jerk; + } +} + +TEST_F(TestSCurveProfile, acceleration_is_continuous) +{ + const float dt{ profile.Duration() / static_cast(samples) }; + + for (int i = 1; i <= samples; ++i) + EXPECT_LE(std::abs(SampleAt(profile, i).acceleration - SampleAt(profile, i - 1).acceleration), limits.jMax * dt + tolerance); +} + +TEST_F(TestSCurveProfile, step1_full_profile_durations) +{ + EXPECT_NEAR(profile.Duration(), 5.7f, tolerance); + EXPECT_NEAR(profile.Sample(0.2f).acceleration, 2.0f, tolerance); + EXPECT_NEAR(profile.Sample(0.7f).velocity, 1.0f, tolerance); + EXPECT_TRUE(profile.ReachesMaxAccel()); + EXPECT_TRUE(profile.ReachesMaxVel()); +} + +TEST_F(TestSCurveProfile, step1_triangular_acceleration_when_jerk_low) +{ + const SCurveProfile lowJerk{ 0.0f, 5.0f, MotionLimits{ 1.0f, 2.0f, 1.0f } }; + + EXPECT_NEAR(lowJerk.Duration(), 7.0f, tolerance); + EXPECT_NEAR(lowJerk.Sample(1.0f).acceleration, 1.0f, tolerance); + EXPECT_NEAR(lowJerk.Sample(3.5f).velocity, 1.0f, tolerance); + EXPECT_FALSE(lowJerk.ReachesMaxAccel()); + EXPECT_TRUE(lowJerk.ReachesMaxVel()); +} + +TEST_F(TestSCurveProfile, step2_short_move_reaches_amax_not_vmax) +{ + const SCurveProfile shortMove{ 0.0f, 0.3f, limits }; + + EXPECT_NEAR(shortMove.Duration(), 1.0f, tolerance); + EXPECT_NEAR(shortMove.Sample(0.5f).velocity, 0.6f, tolerance); + EXPECT_NEAR(MaxOver(shortMove, [](const auto& s) + { + return std::abs(s.acceleration); + }), + 2.0f, tolerance); + EXPECT_TRUE(shortMove.ReachesMaxAccel()); + EXPECT_FALSE(shortMove.ReachesMaxVel()); + EXPECT_NEAR(shortMove.Sample(shortMove.Duration()).position, 0.3f, tolerance); +} + +TEST_F(TestSCurveProfile, step3_very_short_move_reaches_neither) +{ + const SCurveProfile veryShortMove{ 0.0f, 0.02f, limits }; + + EXPECT_NEAR(veryShortMove.Duration(), 0.4f, tolerance); + EXPECT_NEAR(veryShortMove.Sample(0.1f).acceleration, 1.0f, tolerance); + EXPECT_NEAR(veryShortMove.Sample(0.2f).velocity, 0.1f, tolerance); + EXPECT_FALSE(veryShortMove.ReachesMaxAccel()); + EXPECT_FALSE(veryShortMove.ReachesMaxVel()); + EXPECT_NEAR(veryShortMove.Sample(veryShortMove.Duration()).position, 0.02f, tolerance); +} + +TEST_F(TestSCurveProfile, negative_direction_mirrors_profile) +{ + const SCurveProfile backwards{ 5.0f, 0.0f, limits }; + + for (int i = 0; i <= samples; i += 50) + { + const auto forward{ SampleAt(profile, i) }; + const auto mirrored{ SampleAt(backwards, i) }; + + EXPECT_NEAR(mirrored.position, 5.0f - forward.position, tolerance); + EXPECT_NEAR(mirrored.velocity, -forward.velocity, tolerance); + EXPECT_NEAR(mirrored.acceleration, -forward.acceleration, tolerance); + EXPECT_NEAR(mirrored.jerk, -forward.jerk, tolerance); + } +} + +TEST_F(TestSCurveProfile, sample_clamps_outside_domain) +{ + EXPECT_EQ(profile.Sample(-1.0f), profile.Sample(0.0f)); + EXPECT_EQ(profile.Sample(profile.Duration() + 1.0f), profile.Sample(profile.Duration())); +} diff --git a/roadmap/README.md b/roadmap/README.md index 1f2f8c3f..ab332c7c 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -85,8 +85,6 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). ### `controllers` `SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) -### `motion_profiles` -`SCurveProfile` (60) ### `robust_control` `SlidingModeControl` (34) · `DisturbanceObserver` (35) · `ActiveDisturbanceRejection` (36) · `HInfinityStateFeedback` (46) diff --git a/roadmap/motion_profiles/SCurveProfile/explanation.md b/roadmap/motion_profiles/SCurveProfile/explanation.md deleted file mode 100644 index 2d177ff6..00000000 --- a/roadmap/motion_profiles/SCurveProfile/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Overview - -## What it is -A **seven-segment, jerk-bounded** motion profile (a "double-S" velocity curve). It extends the -trapezoid by ramping acceleration in and out with a bounded rate of change (jerk), so acceleration -is continuous rather than stepping instantaneously. - -## Why it matters (embedded) -Sudden acceleration jumps excite structural resonances, wear gearing, and spill liquids or wobble -payloads. Limiting **jerk** smooths those transitions, cutting vibration and mechanical stress at -the cost of a slightly longer move. It is the profile of choice for high-precision or -vibration-sensitive machines (pick-and-place, CNC, semiconductor handling). - -## How it works (intuition) -The velocity curve is built from seven phases: jerk up, hold acceleration, jerk down (to reach -cruise velocity), cruise, then the mirror-image deceleration triple. Depending on the distance and -limits, some phases shrink to zero — a short move may never reach `aMax` or `vMax`. For rest-to-rest -moves the phase durations follow from a three-step closed form: assume `vMax` is reached; if the move is -too short, drop the cruise and assume only `aMax` is reached; if still too short, neither is reached and -the acceleration ramps are pure triangles. Several axes finish together by stretching the faster ones in -time by a common factor `λ` (velocity `/λ`, acceleration `/λ²`, jerk `/λ³`). - -## Key parameters -- **`vMax`** — cruise-velocity ceiling. -- **`aMax`** — acceleration ceiling (flat top of the acceleration curve). -- **`jMax`** — jerk ceiling; the slope of the acceleration ramps and the vibration knob. - -## Reference -L. Biagiotti, C. Melchiorri, *Trajectory Planning for Automatic Machines and Robots* (2008), -§3.4 (double-S / jerk-limited profiles; the non-zero boundary-velocity case there needs an iterative -`aMax` reduction and is future work). - -## See also -`TrapezoidalProfile` (jerk-unbounded predecessor), `PolynomialProfile` (fixed-time quintic), -robotics-toolbox-cpp's `TimeOptimalPathParameterization` (limits along a geometric path). diff --git a/roadmap/motion_profiles/SCurveProfile/implementation.md b/roadmap/motion_profiles/SCurveProfile/implementation.md deleted file mode 100644 index a889a2ff..00000000 --- a/roadmap/motion_profiles/SCurveProfile/implementation.md +++ /dev/null @@ -1,122 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Implementation Pseudocode - -> Roadmap ref: #60 (Tier 2) · Target: `numerical/motion_profiles` · Namespace `motion_profiles` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -`TrajectoryState` and `MotionLimits` come from `numerical/motion_profiles/MotionProfileTypes.hpp` -(canonical definition: PolynomialProfile spec → Data structures); not redefined here. This profile -requires `vMax`, `aMax`, `jMax > 0`. - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class SCurveProfile: - T q0, direction, jMax - T Tj, Ta, Tv # jerk sub-phase, whole accel phase, cruise - array segT # duration of each of the 7 phases - array segStart # start time of each phase (+ tf) - array segPos, segVel, segAcc # state at each phase boundary - T tf - bool reachesMaxAccel, reachesMaxVel -``` - -## Interface - -```text -SCurveProfile(T q0, T qf, MotionLimits limits) -TrajectoryState Sample(T t) # hot path -T Duration() -bool ReachesMaxAccel() / ReachesMaxVel() -``` - -## Algorithm (pseudocode) - -Rest-to-rest closed form (Biagiotti & Melchiorri §3.4, `v0 = v1 = 0`), `h = |qf − q0|`: - -```cpp -# Seven phases: [+j][a=const][-j][v=const][-j][a=const][+j] -function plan(q0, qf, lim): - h = |qf - q0|; direction = sign(qf - q0); jMax = lim.jMax - if h == 0: all durations 0; return - # Step 1 — assume vMax is reached - if lim.vMax * lim.jMax >= lim.aMax^2: # aMax reached on the way to vMax - Tj = lim.aMax / lim.jMax - Ta = Tj + lim.vMax / lim.aMax - else: # triangular accel, peak jMax·Tj < aMax - Tj = sqrt(lim.vMax / lim.jMax) - Ta = 2*Tj - Tv = h / lim.vMax - Ta - if Tv < 0: - # Step 2 — vMax not reached (Tv = 0), assume aMax still reached - Tv = 0 - Tj = lim.aMax / lim.jMax - Δ = lim.aMax^4 / lim.jMax^2 + 4*lim.aMax*h - Ta = (lim.aMax^2 / lim.jMax + sqrt(Δ)) / (2*lim.aMax) - if Ta < 2*Tj: - # Step 3 — neither aMax nor vMax reached - Tj = cbrt(h / (2*lim.jMax)) - Ta = 2*Tj - aLim = jMax*Tj; vLim = aLim*(Ta - Tj) # peak |acc|, peak |vel| - reachesMaxAccel = (Ta > 2*Tj); reachesMaxVel = (Tv > 0) - segT = [Tj, Ta-2*Tj, Tj, Tv, Tj, Ta-2*Tj, Tj] # tf = 2·Ta + Tv - jerkOfPhase = [+jMax, 0, -jMax, 0, -jMax, 0, +jMax] - segPos[0] = segVel[0] = segAcc[0] = 0 - for i in 0..6: # exact constant-jerk integration - τ = segT[i]; j = jerkOfPhase[i] - segPos[i+1] = segPos[i] + segVel[i]*τ + segAcc[i]*τ^2/2 + j*τ^3/6 - segVel[i+1] = segVel[i] + segAcc[i]*τ + j*τ^2/2 - segAcc[i+1] = segAcc[i] + j*τ - segStart[i+1] = segStart[i] + τ - -function Sample(t): # OPTIMIZE_FOR_SPEED - t = clamp(t, 0, tf) - i = last phase with segStart[i] ≤ t (i ≤ 6); tau = t - segStart[i] - j = jerkOfPhase[i] - acc = segAcc[i] + j*tau - vel = segVel[i] + segAcc[i]*tau + 0.5*j*tau^2 - s = segPos[i] + segVel[i]*tau + 0.5*segAcc[i]*tau^2 + (1/6)*j*tau^3 - return { q0 + direction*s, direction*vel, direction*acc, direction*j } -``` - -Branch consistency (checked numerically on 20 000 random `h, vMax, aMax, jMax`): every branch ends at `h` -with zero velocity and acceleration, `|v| ≤ vMax`, `|a| ≤ aMax`, `|j| = jMax`; no phase duration is -negative (`Ta − 2Tj = vMax/aMax − aMax/jMax ≥ 0` in step 1a; the step-2 test guards it otherwise). - -## Complexity & memory - -- Planning: `O(1)` — at most three closed-form candidates (one `sqrt`, one `cbrt`), no iteration. -- `Sample`: `O(1)` — locate one of seven phases, evaluate a cubic-in-time (Horner). -- Memory: `O(1)` — four 8-entry tables plus seven durations; stack only, no heap. - -## Numerical / embedded notes - -- Precompute the seven durations and boundary states **once**; each tick is one cubic evaluation. -- Short moves collapse segments to zero length (`Tv = 0`, `Ta = 2·Tj`) — never a negative duration. -- Bounded jerk means acceleration is **continuous** ⇒ far less vibration than a trapezoid. -- Profile is symmetric about its midpoint for rest-to-rest moves (`Td = Ta`). -- **Synchronization:** multi-axis moves finish together by uniform time scaling of every non-slowest - axis by `λ = T_sync / T_axis ≥ 1`: velocity, acceleration and jerk become `v/λ, a/λ², j/λ³`, sampled at - `t/λ`. Replanning with limits `(vMax/λ, aMax/λ², jMax/λ³)` gives exactly `Duration() = λ·T_axis` in every - branch (branch tests and the step formulas are invariant under this scaling) — planning stays `O(1)`. -- **Non-rest boundary velocities** (`v0, v1 ≠ 0`, B&M §3.4 general case) need the iterative `aMax` - reduction (`aMax ← γ·aMax`, `0 < γ < 1`) plus a feasibility pre-check; not covered — future work. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/motion_profiles/SCurveProfile.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class SCurveProfile;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Uses `numerical/motion_profiles/MotionProfileTypes.hpp` (create it if this is the first `motion_profiles` item). -- Coverage: `numerical/motion_profiles/SCurveProfile.cpp` → `template class SCurveProfile;` -- Test: `numerical/motion_profiles/test/TestSCurveProfile.cpp` -- Doc: `doc/motion_profiles/SCurveProfile.md` (per `doc/TEMPLATE.md`) + row in `doc/motion_profiles/README.md`. -- CMake: `.hpp` → `target_sources(numerical.motion_profiles …)`; - `numerical_add_coverage_sources(numerical.motion_profiles SCurveProfile.cpp)`; - `TestSCurveProfile.cpp` → `numerical.motion_profiles_test`. -- New module (first `motion_profiles` item only): `numerical/motion_profiles/CMakeLists.txt` with - `numerical_add_header_library(numerical.motion_profiles)`, `MotionProfileTypes.hpp` in `target_sources`, a - `test/` subdir, `add_subdirectory(motion_profiles)` in `numerical/CMakeLists.txt`, and a `doc/motion_profiles/` folder. -- Depends on: #56 (`TrapezoidalProfile`, shares the synchronization scheme). -- Generic pattern: see `roadmap/DEPLOYMENT.md` (step 4, new module). diff --git a/roadmap/motion_profiles/SCurveProfile/tests.md b/roadmap/motion_profiles/SCurveProfile/tests.md deleted file mode 100644 index bd35c638..00000000 --- a/roadmap/motion_profiles/SCurveProfile/tests.md +++ /dev/null @@ -1,87 +0,0 @@ -# S-Curve (Jerk-Limited) Profile — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestSCurveProfile : public ::testing::Test: - MotionLimits limits{ .vMax = 1.0f, .aMax = 2.0f, .jMax = 10.0f } - SCurveProfile profile{ 0.0f, 5.0f, limits } # full 7-segment move (step 1, aMax reached) -# each case below is a TEST_F(TestSCurveProfile, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -endpoints_reached_exactly: - Assert: Sample(0).position ≈ 0 and Sample(tf).position ≈ 5 - -endpoints_at_rest_and_zero_accel: - Assert: Sample(0){vel,acc} ≈ 0 and Sample(tf){vel,acc} ≈ 0 - -velocity_never_exceeds_vmax: - Assert: max |velocity| ≤ vMax (1.0) across the profile - -acceleration_never_exceeds_amax: - Assert: max |acceleration| ≤ aMax (2.0) across the profile - -jerk_never_exceeds_jmax: - Assert: every sampled |jerk| ∈ {0, jMax}, ≤ jMax (10.0) - -acceleration_is_continuous: - Arrange: dense sampling across phase joins - Assert: no acceleration step between adjacent samples (bounded by jMax·dt) - -step1_full_profile_durations: # vMax·jMax ≥ aMax², vMax and aMax reached - Assert: Duration() ≈ 5.7; Sample(0.2).acceleration ≈ 2 (end of jerk phase, Tj = 0.2); - Sample(0.7).velocity ≈ 1 (end of accel phase, Ta = 0.7); - ReachesMaxAccel() and ReachesMaxVel() - -step1_triangular_acceleration_when_jerk_low: # vMax·jMax < aMax² - Arrange: limits { vMax=1, aMax=2, jMax=1 }, q0=0, qf=5 - Assert: Duration() ≈ 7; Sample(1).acceleration ≈ 1 (peak jMax·Tj < aMax); - Sample(3.5).velocity ≈ 1; !ReachesMaxAccel() and ReachesMaxVel() - -step2_short_move_reaches_amax_not_vmax: - Arrange: q0=0, qf=0.3 (fixture limits) - Assert: Duration() ≈ 1.0; Sample(0.5).velocity ≈ 0.6 (vLim, at tf/2); - max |acceleration| ≈ 2; ReachesMaxAccel() and !ReachesMaxVel(); Sample(tf).position ≈ 0.3 - -step3_very_short_move_reaches_neither: - Arrange: q0=0, qf=0.02 (fixture limits) - Assert: Duration() ≈ 0.4; Sample(0.1).acceleration ≈ 1 (aLim = jMax·Tj); - Sample(0.2).velocity ≈ 0.1 (vLim); !ReachesMaxAccel() and !ReachesMaxVel(); - Sample(tf).position ≈ 0.02 - -negative_direction_mirrors_profile: - Arrange: q0=5, qf=0 - Assert: Sample(t) = { 5 − p(t), −v(t), −a(t), −j(t) } of the fixture profile - -sample_clamps_outside_domain: - Assert: Sample(-1) == Sample(0) and Sample(tf+1) == Sample(tf) -``` - -## Reference vectors - -Computed by integrating the 7-segment constant-jerk profile (python3, exact): each ends at `h` with -zero velocity/acceleration and respects `vMax`, `aMax`, `jMax`. - -| Case | Limits `(vMax, aMax, jMax)` | `h` | Branch | `Tj` | `Ta` | `Tv` | `tf` | `aLim` | `vLim` | -|------------|-----------------------------|------|---------------|------|------|------|------|--------|--------| -| full | (1, 2, 10) | 5 | 1 (`vj ≥ a²`) | 0.2 | 0.7 | 4.3 | 5.7 | 2 | 1 | -| low jerk | (1, 2, 1) | 5 | 1 (`vj < a²`) | 1.0 | 2.0 | 3.0 | 7.0 | 1 | 1 | -| short | (1, 2, 10) | 0.3 | 2 | 0.2 | 0.5 | 0 | 1.0 | 2 | 0.6 | -| very short | (1, 2, 10) | 0.02 | 3 | 0.1 | 0.2 | 0 | 0.4 | 1 | 0.1 | - -- Step 2 for `h = 0.3`: `Δ = 2⁴/10² + 4·2·0.3 = 2.56`, `Ta = (0.4 + 1.6)/4 = 0.5 ≥ 2·Tj = 0.4`. -- Step 3 for `h = 0.02`: `Tj = (0.02/20)^{1/3} = 0.1`. -- Branch thresholds for `(1, 2, 10)`: step 1 iff `h ≥ vMax·Ta = 0.7`; step 2 iff `h ≥ 2·aMax³/jMax² = 0.16`. -- Synchronization: replanning with `(vMax/λ, aMax/λ², jMax/λ³)` scales `tf` by exactly `λ` in every branch - (e.g. full case, `λ = 2` ⇒ `11.4`). - -## Edge cases - -- `jMax → ∞` ⇒ jerk phases vanish, profile degenerates to a trapezoid (cross-check limit). -- `h` exactly at a branch threshold (0.7 or 0.16 above) ⇒ neighbouring branches give the same `tf`. -- `d == 0` ⇒ zero-length profile; `Sample(0)` is the start, fully at rest. From 696c9c64dc845cc8ed3409b3599b7250e8b9da87 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 16:26:04 +0000 Subject: [PATCH 4/5] docs: align markdown tables Apply markdown-table-formatter's layout to the tables this tier adds or edits, as suggested by MegaLinter. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 16 ++++----- doc/motion_profiles/PolynomialProfile.md | 42 +++++++++++----------- doc/motion_profiles/README.md | 10 +++--- doc/motion_profiles/SCurveProfile.md | 44 +++++++++++------------ doc/motion_profiles/TrapezoidalProfile.md | 32 ++++++++--------- 5 files changed, 72 insertions(+), 72 deletions(-) diff --git a/README.md b/README.md index 82e992f6..89f27dcf 100644 --- a/README.md +++ b/README.md @@ -21,14 +21,14 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer, Friction Compensation (Coulomb + Viscous + Stribeck) | | [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS | | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | -| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | -| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | -| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | -| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | -| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | -| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | -| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | -| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | +| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | +| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | +| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver | +| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | +| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | +| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | +| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | +| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/doc/motion_profiles/PolynomialProfile.md b/doc/motion_profiles/PolynomialProfile.md index 81caffa0..50b349bb 100644 --- a/doc/motion_profiles/PolynomialProfile.md +++ b/doc/motion_profiles/PolynomialProfile.md @@ -50,24 +50,24 @@ With zero end velocities (and accelerations, for the quintic) the cubic peaks at ## Complexity Analysis -| Operation | Time | Space | Notes | -|-----------|------|-------|-------| -| Coefficient solve | $O(1)$ | $O(1)$ | Closed-form expressions, once at construction or reset | -| Sample | $O(\text{degree})$ | $O(1)$ | Four Horner evaluations, at most 14 multiply-adds; no state change, so deterministic | -| Stored state | n/a | $O(1)$ | Six coefficients, the duration and the degree; stack only, no heap | +| Operation | Time | Space | Notes | +|-------------------|--------------------|--------|--------------------------------------------------------------------------------------| +| Coefficient solve | $O(1)$ | $O(1)$ | Closed-form expressions, once at construction or reset | +| Sample | $O(\text{degree})$ | $O(1)$ | Four Horner evaluations, at most 14 multiply-adds; no state change, so deterministic | +| Stored state | n/a | $O(1)$ | Six coefficients, the duration and the degree; stack only, no heap | ## Step-by-Step Walkthrough Rest-to-rest cubic with $q_0 = 0$, $q_f = 1$, $T = 2$, $v_0 = v_f = 0$, so $\Delta q = 1$ and $a_0 = a_1 = 0$. -| Step | Computation | Result | -|------|-------------|--------| -| $a_2$ | $(3\cdot 1 - 0)/2^2$ | $0.75$ | -| $a_3$ | $(-2\cdot 1 + 0)/2^3$ | $-0.25$ | -| Position | $0.75\,t^2 - 0.25\,t^3 = 3(t/2)^2 - 2(t/2)^3$ | $q(1) = 0.5$, $q(2) = 1$ | -| Velocity | $1.5\,t - 0.75\,t^2$ | $v(1) = 0.75 = 1.5\,\Delta q / T$ (the peak) | -| Acceleration | $1.5 - 1.5\,t$ | $+1.5$ at $t=0$, $-1.5$ at $t=2$ | -| Jerk | $6 a_3$ | $-1.5$, constant | +| Step | Computation | Result | +|--------------|-----------------------------------------------|----------------------------------------------| +| $a_2$ | $(3\cdot 1 - 0)/2^2$ | $0.75$ | +| $a_3$ | $(-2\cdot 1 + 0)/2^3$ | $-0.25$ | +| Position | $0.75\,t^2 - 0.25\,t^3 = 3(t/2)^2 - 2(t/2)^3$ | $q(1) = 0.5$, $q(2) = 1$ | +| Velocity | $1.5\,t - 0.75\,t^2$ | $v(1) = 0.75 = 1.5\,\Delta q / T$ (the peak) | +| Acceleration | $1.5 - 1.5\,t$ | $+1.5$ at $t=0$, $-1.5$ at $t=2$ | +| Jerk | $6 a_3$ | $-1.5$, constant | The position is the smoothstep of $t/2$, symmetric about the midpoint, where the velocity peaks. The acceleration jumps from $0$ at rest to $1.5$ at the first instant and from $-1.5$ back to $0$ at the end. @@ -81,10 +81,10 @@ The position is the smoothstep of $t/2$, symmetric about the midpoint, where the ## Variants & Generalizations -| Variant | Key difference | -|---------|----------------| -| **Cubic (this)** | Position and velocity matched; constant jerk; acceleration discontinuous at the ends | -| **Quintic (this)** | Adds acceleration matching; continuous acceleration at about $25\%$ higher peak velocity for rest-to-rest | +| Variant | Key difference | +|-----------------------------|---------------------------------------------------------------------------------------------------------------------------| +| **Cubic (this)** | Position and velocity matched; constant jerk; acceleration discontinuous at the ends | +| **Quintic (this)** | Adds acceleration matching; continuous acceleration at about $25\%$ higher peak velocity for rest-to-rest | | **Path-parameter time law** | A 0 to 1 instance maps time to a normalized path coordinate; with $v_0 = v_f = 1$, $T = 1$ the cubic collapses to $s = t$ | ## Applications @@ -95,11 +95,11 @@ The position is the smoothstep of $t/2$, symmetric about the midpoint, where the ## Connections to Other Algorithms -| Algorithm | Relationship | -|-----------|--------------| +| Algorithm | Relationship | +|------------------------------|-----------------------------------------------------------------------------------------------------------------------| | Trapezoidal velocity profile | Respects velocity and acceleration limits with a free duration; polynomials fix the duration and let the peaks follow | -| S-curve profile | Bounds jerk explicitly under limits; the quintic only smooths the endpoints | -| Cubic spline interpolation | Handles many via points with continuous acceleration; for two knots it reduces to this cubic | +| S-curve profile | Bounds jerk explicitly under limits; the quintic only smooths the endpoints | +| Cubic spline interpolation | Handles many via points with continuous acceleration; for two knots it reduces to this cubic | ## References & Further Reading diff --git a/doc/motion_profiles/README.md b/doc/motion_profiles/README.md index f1043b1d..82135134 100644 --- a/doc/motion_profiles/README.md +++ b/doc/motion_profiles/README.md @@ -4,8 +4,8 @@ Time laws for point-to-point motion of a single axis: position, velocity, accele ## Algorithms -| Algorithm | Description | -|-------------------------------------------------|---------------------------------------------------------------------------------------------| -| [Polynomial Profile](PolynomialProfile.md) | Cubic (position + velocity) or quintic (+ acceleration) boundary-matched time law over a fixed duration | -| [Trapezoidal Profile](TrapezoidalProfile.md) | Minimum-time trapezoid / triangle velocity profile (LSPB) and a fixed-duration plan for multi-axis synchronization | -| [S-Curve Profile](SCurveProfile.md) | Seven-phase jerk-limited rest-to-rest profile with a closed-form three-step planner — continuous acceleration | +| Algorithm | Description | +|----------------------------------------------|--------------------------------------------------------------------------------------------------------------------| +| [Polynomial Profile](PolynomialProfile.md) | Cubic (position + velocity) or quintic (+ acceleration) boundary-matched time law over a fixed duration | +| [Trapezoidal Profile](TrapezoidalProfile.md) | Minimum-time trapezoid / triangle velocity profile (LSPB) and a fixed-duration plan for multi-axis synchronization | +| [S-Curve Profile](SCurveProfile.md) | Seven-phase jerk-limited rest-to-rest profile with a closed-form three-step planner — continuous acceleration | diff --git a/doc/motion_profiles/SCurveProfile.md b/doc/motion_profiles/SCurveProfile.md index d36af22d..80a6a5f5 100644 --- a/doc/motion_profiles/SCurveProfile.md +++ b/doc/motion_profiles/SCurveProfile.md @@ -12,15 +12,15 @@ For a rest-to-rest move the phase durations have a closed form: planning is a ha A rest-to-rest move of length $h > 0$ is built from seven constant-jerk phases: -| Phase | Jerk | Acceleration | Duration | -|-------|------|--------------|----------| -| 1 | $+j_{\max}$ | ramps $0 \to a_{lim}$ | $T_j$ | -| 2 | $0$ | constant $a_{lim}$ | $T_a - 2T_j$ | -| 3 | $-j_{\max}$ | ramps $a_{lim} \to 0$ | $T_j$ | -| 4 | $0$ | zero, cruise at $v_{lim}$ | $T_v$ | -| 5 | $-j_{\max}$ | ramps $0 \to -a_{lim}$ | $T_j$ | -| 6 | $0$ | constant $-a_{lim}$ | $T_a - 2T_j$ | -| 7 | $+j_{\max}$ | ramps $-a_{lim} \to 0$ | $T_j$ | +| Phase | Jerk | Acceleration | Duration | +|-------|-------------|---------------------------|--------------| +| 1 | $+j_{\max}$ | ramps $0 \to a_{lim}$ | $T_j$ | +| 2 | $0$ | constant $a_{lim}$ | $T_a - 2T_j$ | +| 3 | $-j_{\max}$ | ramps $a_{lim} \to 0$ | $T_j$ | +| 4 | $0$ | zero, cruise at $v_{lim}$ | $T_v$ | +| 5 | $-j_{\max}$ | ramps $0 \to -a_{lim}$ | $T_j$ | +| 6 | $0$ | constant $-a_{lim}$ | $T_a - 2T_j$ | +| 7 | $+j_{\max}$ | ramps $-a_{lim} \to 0$ | $T_j$ | Phases 1–3 form the acceleration stage of length $T_a$; phases 5–7 mirror it, so the profile is symmetric about its midpoint and the total time is $T = 2T_a + T_v$. Integrating the stage gives the peaks and the distance: @@ -66,10 +66,10 @@ Replanning with $(v_{\max}/\lambda,\; a_{\max}/\lambda^2,\; j_{\max}/\lambda^3)$ ## Complexity Analysis -| Operation | Time | Space | Notes | -|-----------|------|-------|-------| -| Plan | $O(1)$ | $O(1)$ | At most one square root and one cube root, no iteration | -| Sample | $O(1)$ | $O(1)$ | Locate one of seven phases, evaluate one cubic | +| Operation | Time | Space | Notes | +|-----------|--------|--------|---------------------------------------------------------| +| Plan | $O(1)$ | $O(1)$ | At most one square root and one cube root, no iteration | +| Sample | $O(1)$ | $O(1)$ | Locate one of seven phases, evaluate one cubic | Stored state is a few fixed-size tables of phase boundaries on the stack; no heap is used. Cost is independent of $h$ and of the branch taken. @@ -82,12 +82,12 @@ Limits $(v_{\max}, a_{\max}, j_{\max}) = (1, 2, 10)$, $h = 0.3$. Boundary states $(a, v, s)$ of the acceleration stage: at $t = 0.2$, $(2,\, 0.2,\, 0.0133)$; at $t = 0.3$, $(2,\, 0.4,\, 0.0433)$; at $t = 0.5$, $(0,\, 0.6,\, 0.15)$. The mirrored stage ends at $s = 0.3$, at rest. A sample at $t = 0.4$ lies in phase 3 with $\tau = 0.1$: $a = 1$, $v = 0.55$, $s = 0.0917$. -| $(v_{\max}, a_{\max}, j_{\max})$ | $h$ | Branch | $T_j$ | $T_a$ | $T_v$ | $T$ | -|----------------------------------|-----|--------|-------|-------|-------|-----| -| (1, 2, 10) | 5 | 1, plateau | 0.2 | 0.7 | 4.3 | 5.7 | -| (1, 2, 1) | 5 | 1, triangle | 1 | 2 | 3 | 7 | -| (1, 2, 10) | 0.3 | 2 | 0.2 | 0.5 | 0 | 1.0 | -| (1, 2, 10) | 0.02 | 3 | 0.1 | 0.2 | 0 | 0.4 | +| $(v_{\max}, a_{\max}, j_{\max})$ | $h$ | Branch | $T_j$ | $T_a$ | $T_v$ | $T$ | +|----------------------------------|------|-------------|-------|-------|-------|-----| +| (1, 2, 10) | 5 | 1, plateau | 0.2 | 0.7 | 4.3 | 5.7 | +| (1, 2, 1) | 5 | 1, triangle | 1 | 2 | 3 | 7 | +| (1, 2, 10) | 0.3 | 2 | 0.2 | 0.5 | 0 | 1.0 | +| (1, 2, 10) | 0.02 | 3 | 0.1 | 0.2 | 0 | 0.4 | For $(1, 2, 10)$ the thresholds are $h \ge 0.7$ for step 1 and $h \ge 2a_{\max}^3/j_{\max}^2 = 0.16$ for step 2. The last row has $a_{lim} = 1$ and $v_{lim} = 0.1$. @@ -110,9 +110,9 @@ For $(1, 2, 10)$ the thresholds are $h \ge 0.7$ for step 1 and $h \ge 2a_{\max}^ ## Connections to Other Algorithms -| Algorithm | Relationship | -|-----------|--------------| -| Trapezoidal profile | Jerk-unbounded predecessor; shares the synchronization scheme | +| Algorithm | Relationship | +|----------------------------|-----------------------------------------------------------------| +| Trapezoidal profile | Jerk-unbounded predecessor; shares the synchronization scheme | | Quintic polynomial profile | Fixed duration and smooth, but limits are not enforced directly | ## References & Further Reading diff --git a/doc/motion_profiles/TrapezoidalProfile.md b/doc/motion_profiles/TrapezoidalProfile.md index c1c543a5..3df87851 100644 --- a/doc/motion_profiles/TrapezoidalProfile.md +++ b/doc/motion_profiles/TrapezoidalProfile.md @@ -29,11 +29,11 @@ The total time is $T = 2t_a + t_c$. The cases agree at $d = v_{max}^2/a_{max}$, With $v$, $t_a$, $t_c$, $T$ precomputed, the unsigned state is: -| Phase | Interval | Acceleration | Velocity | Position | -|-------|----------|--------------|----------|----------| -| Parabolic ramp-up | $0 \le t < t_a$ | $a_{max}$ | $a_{max}\,t$ | $\tfrac12 a_{max}\,t^2$ | -| Linear cruise | $t_a \le t < t_a + t_c$ | $0$ | $v$ | $v\,(t - t_a/2)$ | -| Parabolic ramp-down | $t_a + t_c \le t < T$ | $-a_{max}$ | $a_{max}\,(T-t)$ | $d - \tfrac12 a_{max}\,(T-t)^2$ | +| Phase | Interval | Acceleration | Velocity | Position | +|---------------------|-------------------------|--------------|------------------|---------------------------------| +| Parabolic ramp-up | $0 \le t < t_a$ | $a_{max}$ | $a_{max}\,t$ | $\tfrac12 a_{max}\,t^2$ | +| Linear cruise | $t_a \le t < t_a + t_c$ | $0$ | $v$ | $v\,(t - t_a/2)$ | +| Parabolic ramp-down | $t_a + t_c \le t < T$ | $-a_{max}$ | $a_{max}\,(T-t)$ | $d - \tfrac12 a_{max}\,(T-t)^2$ | Velocity, acceleration and the position offset are multiplied by $\sigma$. At and beyond $t = T$ the profile holds the goal at rest (velocity and acceleration zero); times before $0$ are treated as $t = 0$, holding the start with zero velocity. The reported jerk is zero in every phase. @@ -61,11 +61,11 @@ The fixed-duration plan is the alternative. It keeps $a_{max}$ and only lowers t ## Complexity Analysis -| Operation | Time | Space | Notes | -|-----------|------|-------|-------| -| Minimum-time plan | $O(1)$ | $O(1)$ | One comparison, one square root (triangle only) | -| Fixed-duration plan | $O(1)$ | $O(1)$ | One square root, one division | -| Sample | $O(1)$ | $O(1)$ | One phase branch, a few multiply-adds; no state change | +| Operation | Time | Space | Notes | +|---------------------|--------|--------|--------------------------------------------------------| +| Minimum-time plan | $O(1)$ | $O(1)$ | One comparison, one square root (triangle only) | +| Fixed-duration plan | $O(1)$ | $O(1)$ | One square root, one division | +| Sample | $O(1)$ | $O(1)$ | One phase branch, a few multiply-adds; no state change | Memory is a fixed handful of scalars: start, direction, distance, acceleration, peak speed and three phase times. @@ -73,12 +73,12 @@ Memory is a fixed handful of scalars: start, direction, distance, acceleration, **Trapezoid.** $d = 5$, $v_{max} = 1$, $a_{max} = 2$. The blend distance is $1/2 = 0.5 \le 5$, so a cruise exists: $t_a = 0.5$, $t_c = (5 - 0.5)/1 = 4.5$, $T = 2(0.5) + 4.5 = 5.5$. -| $t$ | Phase | Position | Velocity | Acceleration | -|-----|-------|----------|----------|--------------| -| $0.25$ | ramp-up | $\tfrac12\cdot 2\cdot 0.25^2 = 0.0625$ | $0.5$ | $2$ | -| $3$ | cruise | $1\cdot(3 - 0.25) = 2.75$ | $1$ | $0$ | -| $5.25$ | ramp-down | $5 - \tfrac12\cdot 2\cdot 0.25^2 = 4.9375$ | $0.5$ | $-2$ | -| $\ge 5.5$ | held | $5$ | $0$ | $0$ | +| $t$ | Phase | Position | Velocity | Acceleration | +|-----------|-----------|--------------------------------------------|----------|--------------| +| $0.25$ | ramp-up | $\tfrac12\cdot 2\cdot 0.25^2 = 0.0625$ | $0.5$ | $2$ | +| $3$ | cruise | $1\cdot(3 - 0.25) = 2.75$ | $1$ | $0$ | +| $5.25$ | ramp-down | $5 - \tfrac12\cdot 2\cdot 0.25^2 = 4.9375$ | $0.5$ | $-2$ | +| $\ge 5.5$ | held | $5$ | $0$ | $0$ | The ramp-up ends at position $0.25$, and the cruise formula gives the same value at $t = 0.5$. The triangle threshold for these limits is $d = 0.5$; for $d = 0.25$, $v = \sqrt{0.5} \approx 0.7071$ and $T \approx 0.7071$, with no cruise. From ad3b6a8cef9b53087c36dd6c27a1f7ea1d32eb08 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 16:28:28 +0000 Subject: [PATCH 5/5] fix(build): build motion_profiles tests in the QEMU presets The qemu-cortex-m4/m7 build presets list their test targets explicitly, so numerical.motion_profiles_test was registered with CTest but never built, and the QEMU jobs failed with "Could not load kernel". Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- CMakePresets.json | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakePresets.json b/CMakePresets.json index 953a9772..1e842b47 100644 --- a/CMakePresets.json +++ b/CMakePresets.json @@ -142,6 +142,7 @@ "numerical.filters.active_test", "numerical.filters.passive_test", "numerical.math_test", + "numerical.motion_profiles_test", "numerical.nonlinear_control_test", "numerical.optimization_test", "numerical.regularization_test", @@ -164,6 +165,7 @@ "numerical.filters.active_test", "numerical.filters.passive_test", "numerical.math_test", + "numerical.motion_profiles_test", "numerical.nonlinear_control_test", "numerical.optimization_test", "numerical.regularization_test",