From 11033e631a2150248b2fb4cd122c28c3f13ab902 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 6 Nov 2026 11:19:08 +0000 Subject: [PATCH] feat(trajectory): add TimeOptimalPathParameterization (M27) TOPP-RA: minimum-time timing law along a fixed joint path under torque and velocity limits. Path coefficients come from an inverse-dynamics adapter (three passes per gridpoint); a backward pass builds the controllable sets with exact Fourier-Motzkin projections of each stage polygon, and a greedy forward pass picks the largest admissible control inside them. Stage times are exact for constant path acceleration, and Sample streams the joint state by binary search over stage start times. The spec's JointLimits interface is named PathConstraints to stay distinct from dynamics::JointLimits. This was the last roadmap spec. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN --- README.md | 2 +- ROADMAP.md | 18 +- doc/trajectory/README.md | 9 +- .../TimeOptimalPathParameterization.md | 117 ++++++ roadmap/README.md | 12 +- .../explanation.md | 52 --- .../implementation.md | 196 ---------- .../TimeOptimalPathParameterization/tests.md | 103 ------ robotics/trajectory/CMakeLists.txt | 4 + .../TimeOptimalPathParameterization.cpp | 7 + .../TimeOptimalPathParameterization.hpp | 347 ++++++++++++++++++ robotics/trajectory/test/CMakeLists.txt | 3 + .../TestTimeOptimalPathParameterization.cpp | 236 ++++++++++++ .../trajectory/test_doubles/CMakeLists.txt | 12 + .../test_doubles/MockPathConstraints.hpp | 16 + .../test_doubles/MockPathGeometry.hpp | 19 + 16 files changed, 776 insertions(+), 377 deletions(-) create mode 100644 doc/trajectory/TimeOptimalPathParameterization.md delete mode 100644 roadmap/trajectory/TimeOptimalPathParameterization/explanation.md delete mode 100644 roadmap/trajectory/TimeOptimalPathParameterization/implementation.md delete mode 100644 roadmap/trajectory/TimeOptimalPathParameterization/tests.md create mode 100644 robotics/trajectory/TimeOptimalPathParameterization.cpp create mode 100644 robotics/trajectory/TimeOptimalPathParameterization.hpp create mode 100644 robotics/trajectory/test/TestTimeOptimalPathParameterization.cpp create mode 100644 robotics/trajectory/test_doubles/CMakeLists.txt create mode 100644 robotics/trajectory/test_doubles/MockPathConstraints.hpp create mode 100644 robotics/trajectory/test_doubles/MockPathGeometry.hpp diff --git a/README.md b/README.md index 6bb5cf2..ecd0c70 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ project already defines those targets. Includes are namespaced by domain, e.g. | [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (DLS), Pose Inverse Kinematics, Analytical IK (OPW), Continuum Kinematics, Parallel Manipulator Kinematics, Mobile-Manipulator Kinematics | | [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Dynamic Parameter Identification, Forward-Dynamics Integration, Generic Joint Link, Generalized-Momentum Observer | | [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking, Convex Collision (GJK and EPA), Grid Search (A* and Dijkstra), RRT-Connect, Dubins Shortest Paths, Dynamic Window Approach, Reeds–Shepp Shortest Paths | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation, Time-Optimal Path Parameterization (TOPP-RA) | | [Mobile Robots](doc/mobile/README.md) | Planar Odometry, Differential-Drive Kinematics, Car-Like Kinematics, Omnidirectional Kinematics, Skid-Steer Kinematics, Pure-Pursuit Path Tracking, Kanayama Trajectory Tracking, Stanley Path Tracking | | [Localization & Mapping](doc/localization/README.md) | Occupancy-Grid Mapping, Odometry Fusion, EKF Landmark Localization, 2-D Scan Matching (ICP), Monte Carlo Localization | | [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control, Impedance Control, Operational-Space Control, Hybrid Position/Force Control, Slotine–Li Adaptive Control, Cable Tension Distribution | diff --git a/ROADMAP.md b/ROADMAP.md index a45ed2a..852dc9a 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -96,7 +96,7 @@ The library already has the core of manipulator **modelling**: forward/inverse d ([ForwardDynamicsIntegrator](robotics/dynamics/ForwardDynamicsIntegrator.hpp)), the manipulator-equation adapter ([ChainDynamicsModel](robotics/dynamics/ChainDynamicsModel.hpp)), position forward kinematics with base and tool offsets, and damped-least-squares [IK](robotics/kinematics/InverseKinematics.hpp). Full-pose -kinematics and the geometric Jacobian have shipped (see the note below); what is missing is the +kinematics and the geometric Jacobian have shipped (see the note below), and so has the **planning and control layer** that turns those models into a usable manipulator stack. > **Full-pose kinematics shipped.** Links are generic ([JointLink](robotics/dynamics/JointLink.hpp): @@ -117,25 +117,13 @@ existing `dynamics/` convention. ### Manipulator list (by priority) -| # | Component | Target module | Depends on | Difficulty | -|-----|----------------------------------------------|--------------------|------------|------------| -| M27 | Time-optimal path parameterization (TOPP-RA) | `trajectory` (new) | M29 | ★★★★★ | +All manipulator items have shipped: see [Manipulator Control](doc/controllers/manipulator/README.md), +[Kinematics](doc/kinematics/README.md), [Dynamics](doc/dynamics/README.md) and [Trajectory](doc/trajectory/README.md). M2 (polynomial profile), M3 (trapezoidal profile), M4 (friction compensation), M6 (SE(3)) and M9 (S-curve profile) moved to numerical-toolbox as NT55, NT56, NT57, NT53 and NT60; their numbers are retired here. -### Tier 2 — Easy ★★☆☆☆ - -### Tier 3 — Moderate ★★★☆☆ - -### Tier 4 — Advanced ★★★★☆ - -### Tier 5 — Hard / research-grade ★★★★★ - -**M27. Time-optimal path parameterization (TOPP-RA).** Minimum-time traversal of a fixed path under joint velocity and torque limits via reachability analysis (two-variable LPs per grid stage, solved locally by Fourier–Motzkin elimination). -- *Algorithm / paper:* J. Bobrow, S. Dubowsky, J. Gibson, *IJRR*, 4(3), 1985; H. Pham, Q.-C. Pham, "A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis," *IEEE T-RO*, 34(3), 2018. - --- ## Shared foundation diff --git a/doc/trajectory/README.md b/doc/trajectory/README.md index dd1cc6c..98dada0 100644 --- a/doc/trajectory/README.md +++ b/doc/trajectory/README.md @@ -4,7 +4,8 @@ Time-parameterized joint and Cartesian references for manipulators. ## Algorithms -| Algorithm | Description | -|-------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------| -| [Cubic Spline Trajectory](CubicSplineTrajectory.md) | $C^2$ joint trajectory through timed via points, rest-to-rest by default — $O(nK)$ plan, $O(\log K + n)$ sample | -| [Cartesian Path with SLERP Orientation](CartesianSlerpInterpolation.md) | Straight-line position and shortest-arc quaternion SLERP on one time law, with twist feed-forward — $O(1)$ per sample | +| Algorithm | Description | +|------------------------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Cubic Spline Trajectory](CubicSplineTrajectory.md) | $C^2$ joint trajectory through timed via points, rest-to-rest by default — $O(nK)$ plan, $O(\log K + n)$ sample | +| [Cartesian Path with SLERP Orientation](CartesianSlerpInterpolation.md) | Straight-line position and shortest-arc quaternion SLERP on one time law, with twist feed-forward — $O(1)$ per sample | +| [Time-Optimal Path Parameterization (TOPP-RA)](TimeOptimalPathParameterization.md) | Minimum-time timing law along a fixed joint path under torque and velocity limits, by reachability analysis with exact Fourier–Motzkin stage projections — $O(Nn^2)$ plan, $O(\log N + n)$ sample | diff --git a/doc/trajectory/TimeOptimalPathParameterization.md b/doc/trajectory/TimeOptimalPathParameterization.md new file mode 100644 index 0000000..fa3678b --- /dev/null +++ b/doc/trajectory/TimeOptimalPathParameterization.md @@ -0,0 +1,117 @@ +# Time-Optimal Path Parameterization (TOPP-RA) + +## Overview & Motivation + +A path planner answers *where* the robot goes: a collision-free curve $q(s)$ in joint space, with $s$ running from 0 to 1. It does not say *how fast*. Running the curve at a constant, conservative speed wastes cycle time. Running it too fast asks the drives for torque they cannot deliver, and the arm leaves the path. + +**Time-optimal path parameterization** (TOPP) keeps the geometry fixed and finds the fastest timing law $s(t)$ that respects every joint's velocity and torque limits. The result accelerates as hard as the weakest drive allows, cruises where a speed limit binds, and brakes at the last possible moment. + +The classical phase-plane method (Bobrow et al., 1985) searches for switching points between maximum acceleration and maximum deceleration, which is fragile at singular points. **TOPP-RA** (Pham and Pham, 2018) instead propagates *sets of admissible speeds* along a grid. +It takes a fixed number of closed-form steps per grid stage, with bounded memory and deterministic run time, which suits an embedded planner. + +## Mathematical Theory + +### Reduction to the Path Coordinate + +Along the path, $\dot q = q'\,\dot s$ and $\ddot q = q'\,\ddot s + q''\,\dot s^2$. With $x = \dot s^2$ and $u = \ddot s$, the manipulator equation $\tau = M(q)\ddot q + C(q, \dot q)\dot q + g(q)$ becomes affine in $(x, u)$: + +$$\tau(s) = a(s)\,u + b(s)\,x + c(s), \qquad a = M q', \quad b = M q'' + C(q, q')\,q', \quad c = g(q)$$ + +Three inverse-dynamics passes give the coefficients: $c = \mathrm{ID}(q, 0, 0)$, $a = \mathrm{ID}(q, 0, q') - c$ and $b = \mathrm{ID}(q, q', q'') - c$. The term $C(q, q'\dot s)\,q'\dot s = \dot s^2\,C(q, q')\,q'$ is quadratic in $\dot s$, so it lands in $b$. + +A joint speed limit $|q'_j \dot s| \le \dot q_{\max,j}$ becomes an upper bound on $x$: + +$$x \le x_{\max}(s) = \min_{j:\,q'_j \ne 0} \left(\frac{\dot q_{\max,j}}{q'_j}\right)^2$$ + +### Discretization + +The grid is $s_i = i/N$, $i = 0, \ldots, N$, with step $\Delta = 1/N$. Each stage holds $u_i$ constant, so $x$ is linear in $s$: + +$$x_{i+1} = x_i + 2\Delta\,u_i$$ + +The constraints at gridpoint $i$ are the $2n$ torque rows $\tau_{\min} \le a_i u + b_i x + c_i \le \tau_{\max}$ and the speed rows $0 \le x \le x_{\max,i}$. All of them have the form $\alpha x + \beta u \le \gamma$. + +### Controllable Sets (Backward Pass) + +The controllable set $K_i = [l_i, h_i]$ holds the speeds $x$ at $s_i$ from which the robot can still finish the path and stop at $s = 1$. It starts at $K_N = [0, 0]$ and runs backwards: + +$$K_i = \{\, x \;:\; \exists u,\; (x, u) \text{ satisfies stage } i,\; x + 2\Delta u \in K_{i+1} \,\}$$ + +The feasible region of stage $i$ is a polygon in the $(x, u)$ plane, and $K_i$ is its shadow on the $x$ axis. Every row with $\beta > 0$ gives an upper bound on $u$, and every row with $\beta < 0$ a lower bound. Fourier–Motzkin elimination combines each pair $(p, n)$ into a constraint on $x$ alone: + +$$(\beta_p \alpha_n - \beta_n \alpha_p)\,x \le \beta_p \gamma_n - \beta_n \gamma_p$$ + +Together with the rows that do not involve $u$, these give the exact minimum and maximum of $x$ over the polygon, without an iterative LP solver. The path is infeasible if some $K_i$ is empty or if $0 \notin K_0$, since the robot must start from rest. + +### Greedy Forward Pass + +Starting from $x_0 = 0$, each stage takes the largest admissible control, a one-dimensional LP: + +$$u_i = \min_{\beta > 0} \frac{\gamma - \alpha x_i}{\beta}, \qquad x_{i+1} = \operatorname{clamp}(x_i + 2\Delta u_i,\; l_{i+1},\; h_{i+1})$$ + +The control $u_i$ is then recomputed from the clamped $x_{i+1}$, so each stage ends exactly at $s_{i+1}$. Staying inside $K_{i+1}$ guarantees that the next stage is feasible too, so the pass never gets stuck. + +### Timing and Sampling + +With constant $u_i$, the stage takes exactly + +$$\Delta t_i = \frac{2\Delta}{\dot s_i + \dot s_{i+1}}, \qquad \dot s_i = \sqrt{x_i}$$ + +which stays finite at the rest endpoints, unlike $\int ds / \sqrt{x}$. Inside stage $i$, at time $\tau$ after its start, $s = s_i + \dot s_i \tau + u_i \tau^2 / 2$, $\dot s = \dot s_i + u_i \tau$ and $\ddot s = u_i$. The joint state follows from the path derivatives. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-------------------|-----------------|----------------|---------------------------------------------------------------| +| Path coefficients | $O(N n)$ | $O(N n)$ | Three RNEA passes per gridpoint through the adapter | +| Backward pass | $O(N n^2)$ | $O(n)$ | All pairs of the $2n + 4$ rows of each stage | +| Forward pass | $O(N n)$ | $O(1)$ | One 1-D LP per stage | +| Sample | $O(\log N + n)$ | $O(1)$ | Binary search over stage start times, three path queries | +| Profile store | — | $O(N(3n + 6))$ | About 3 KB for 2 joints and 64 gridpoints in single precision | + +For $N$ stages and $n$ joints. Planning is deterministic: no iteration count depends on the data. + +## Step-by-Step Walkthrough + +A single joint with unit inertia and $|\tau| \le 1$ moves a distance $L$ rest to rest. The continuous optimum is bang-bang, with time $2\sqrt{L}$. The planner uses 64 gridpoints, so 63 stages: + +1. **Bang-bang**: for $L = 1$ the profile accelerates at $+1$ to the midpoint and brakes at $-1$. The total time is $2.000064$ against $2$ exact. The odd stage count leaves one coast stage at the midpoint, so the peak speed is $0.992$ instead of $1$. +2. **Longer path**: $L = 4$ takes $4.000128$, with the acceleration at exactly $\pm 1$ in the first and last 45 % of the motion. +3. **Speed limit**: with $\dot q_{\max} = 0.5$ and $|\tau| \le 100$, one stage ramps up, the speed holds at $0.5$, and one stage ramps down. The total is $2 + 4/63 = 2.063492$. +4. **Coupled two-joint path**: on $q(s) = (s,\; 0.4s - 0.4s^2)$ with position-dependent coefficients, the profile takes $2.256496$. Joint speeds peak at 58 % and 18 % of their limits. Torques hold exactly at gridpoints and exceed their bounds by at most $0.018$ between them. +5. **Infeasible holding torque**: a gravity torque of $2$ against a bound of $1$ forces $u \le -1$ on every stage. Then $0 \notin K_0$, and the planner reports failure. + +## Pitfalls & Edge Cases + +- **Collocation**: constraints hold exactly at gridpoints only, with an $O(\Delta)$ excursion between them. Refine the grid or shrink the bounds by a margin. +- **Viscous friction**: it is linear in $\dot s$, not quadratic, so it breaks the affine form. Use a friction-free model plus a torque margin. +- **Real-time use**: planning calls the path geometry and the dynamics through virtual interfaces. Plan offline, then resample the result into a [Cubic Spline Trajectory](CubicSplineTrajectory.md) for the servo loop. +- **Zero-length path**: if $q' \equiv 0$, the planner succeeds with a total time of $0$. +- **Stationary joints and zero inertia**: $q'_j = 0$ drops the joint from $x_{\max}$, and $a_{ij} = 0$ turns its torque rows into pure $x$ bounds. No division by zero occurs. +- **Stalls**: a zero speed limit pins $K_1$ to $\{0\}$, and the profile cannot leave rest. The planner reports failure instead of dividing by zero. + +## Variants & Generalizations + +- **Phase-plane TOPP**: integrates maximum-acceleration and maximum-deceleration curves between switching points (Bobrow; Pham 2014). +- **Convex-optimization TOPP**: solves the whole profile as one second-order cone program, at a higher cost. +- **Jerk-limited TOPP**: adds third-order limits, at the price of a nonconvex problem. +- **Second-order constraints**: contact stability, friction cones and Cartesian speed limits fit the same $\alpha x + \beta u \le \gamma$ rows. + +## Applications + +- Cycle-time optimization of industrial manipulators along fixed welding, gluing or machining paths. +- Speed scheduling of mobile manipulators and vehicles along planned curves. +- Retiming the output of sampling-based planners such as [RRT-Connect](../planning/RrtConnect.md) before execution. + +## Connections to Other Algorithms + +- [Chain Dynamics Model](../dynamics/ChainDynamicsModel.md): the inverse-dynamics model behind the path coefficients. +- [Cubic Spline Trajectory](CubicSplineTrajectory.md): resamples the optimal profile for the servo loop. +- [Cartesian Path with SLERP Orientation](CartesianSlerpInterpolation.md): a task-space path that inverse kinematics maps to $q(s)$. +- The S-curve and trapezoidal profiles in [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp) are the heuristic alternative when no dynamics model is available. + +## References & Further Reading + +- Pham, H. and Pham, Q.-C. (2018). "A New Approach to Time-Optimal Path Parameterization Based on Reachability Analysis." *IEEE Transactions on Robotics*, 34(3), 645–659. +- Bobrow, J.E., Dubowsky, S. and Gibson, J.S. (1985). "Time-Optimal Control of Robotic Manipulators Along Specified Paths." *International Journal of Robotics Research*, 4(3), 3–17. +- Pham, Q.-C. (2014). "A General, Fast, and Robust Implementation of the Time-Optimal Path Parameterization Algorithm." *IEEE Transactions on Robotics*, 30(6), 1533–1540. diff --git a/roadmap/README.md b/roadmap/README.md index 17d63d2..65fcd5c 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -3,8 +3,8 @@ Design-level pseudocode for proposed components in [../ROADMAP.md](../ROADMAP.md). These are **specifications, not compilable code** — they describe *what* to build and *how* the algorithm works, so an implementer can produce the real templated C++ afterwards. Every manipulator -(M), wheeled-robot (W), localization (E) and planning (P) item has a spec, as does F1; the other families -(F, A, L, S, G, C, H) are ROADMAP entries only, and their specs are added here family by family. +(M), wheeled-robot (W), localization (E) and planning (P) item, and F1, has shipped from a spec here; the +other families (F, A, L, S, G, C, H) are ROADMAP entries only, and their specs are added here family by family. ## Convention @@ -22,8 +22,9 @@ All pseudocode respects the library constraints: **no heap**, bounded containers `template` interface that is validated and instantiated for **`float`** only (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. -The canonical worked example is -[trajectory/TimeOptimalPathParameterization](trajectory/TimeOptimalPathParameterization/implementation.md). +No spec is pending. The last one, M27, shipped as +[TimeOptimalPathParameterization](../doc/trajectory/TimeOptimalPathParameterization.md); its spec files stay in the +git history as the canonical worked example. Conventions shared by every spec: 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches `(f; n)` — as fixed by upstream [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md) (NT53), and planar @@ -88,5 +89,4 @@ prerequisites. ## Index -### `trajectory` -`TimeOptimalPathParameterization` (M27) +No spec is pending. diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md b/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md deleted file mode 100644 index 4b11602..0000000 --- a/roadmap/trajectory/TimeOptimalPathParameterization/explanation.md +++ /dev/null @@ -1,52 +0,0 @@ -# Time-Optimal Path Parameterization (TOPP) — Overview - -## What it is -Given a **fixed geometric path** (the shape the end-effector or joints must follow) and the robot's -**actuator limits**, TOPP computes the fastest possible timing law `s(t)` to traverse that path -without violating any velocity or torque bound. It answers "how fast can I run *this* curve?" — the -geometry is frozen, only the speed along it is optimized. - -## Why it matters (embedded) -Separating *where* to go (path planning) from *how fast* to go (parameterization) is a powerful -decomposition. Once a collision-free path exists, TOPP squeezes maximum throughput out of the -hardware — critical for cycle-time-bound industrial robots — while provably respecting drive -saturation, so the plan never asks for torque the motors cannot deliver. The reachability variant -used here is a fixed number of closed-form steps per grid stage: bounded memory, no iterative solver, -deterministic planning time. - -## How it works (intuition) -Reparameterize the dynamics in terms of the scalar path coordinate `s`: with `x = ṡ²` and `u = s̈`, -every joint torque becomes affine, `τ = a(s)·u + b(s)·x + c(s)`, and a joint speed limit becomes an upper -bound on `x`. On a grid of `s`, holding `u` constant per stage makes `x` evolve linearly -(`x_{i+1} = x_i + 2Δ·u_i`). TOPP-RA then works on **sets of admissible speeds**: -- a **backward pass** computes, from the end at rest, the *controllable set* `K_i = [lo_i, hi_i]` — the - speeds at `s_i` from which the rest of the path can still be completed. Each `K_i` is the range of `x` - over a small polygon in the `(x, u)` plane (stage constraints plus "land inside `K_{i+1}`"), i.e. two - tiny **2-variable** linear programs, solved exactly by eliminating `u`; -- a **forward pass** starts at rest and greedily picks the largest acceleration that keeps the next - speed inside `K_{i+1}` — a 1-D linear program per stage. - -The result is bang-bang-like (maximum acceleration, then maximum deceleration, with speed-limited -plateaus), and the stage times follow exactly from the average of the speeds at both ends. It avoids -the fragile switch-point search of the classical phase-plane (Bobrow) and numerical-integration methods. - -## Key parameters -- **Path geometry** — `q(s)` and its first/second derivatives along `s ∈ [0, 1]`. -- **Velocity & torque limits** — per-joint bounds; the path coefficients `a, b, c` come from inverse - dynamics (three RNEA passes per grid point). -- **Grid resolution** — number of `s` samples; constraints hold exactly at grid points, with an - `O(Δ)` excursion between them. - -## Reference -H. Pham, Q.-C. Pham, "A New Approach to Time-Optimal Path Parameterization Based on Reachability -Analysis," *IEEE Trans. Robotics* 34(3), 2018 (TOPP-RA — the algorithm specified here). -Background: J. Bobrow, S. Dubowsky, J. Gibson, "Time-Optimal Control of Robotic Manipulators Along -Specified Paths," *IJRR* 4(3), 1985 (phase-plane method); Q.-C. Pham, "A General, Fast, and Robust -Implementation of the Time-Optimal Path Parameterization Algorithm," *IEEE Trans. Robotics* 30(6), 2014 -(numerical-integration TOPP, not TOPP-RA). - -## See also -`CartesianSlerpInterpolation` (task-space path, mapped to `q(s)` through IK), -`ChainDynamicsModel` / `InverseDynamicsModel` (M29, torque coefficients), -`CubicSplineTrajectory` (resample the result for the servo loop), upstream `SCurveProfile` (NT60, heuristic -limit-respecting alternative). diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md b/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md deleted file mode 100644 index c91eac3..0000000 --- a/roadmap/trajectory/TimeOptimalPathParameterization/implementation.md +++ /dev/null @@ -1,196 +0,0 @@ -# Time-Optimal Path Parameterization (TOPP-RA) — Implementation Pseudocode - -> Roadmap ref: #M27 (Tier 5) · Target: `robotics/trajectory` · Namespace `trajectory` · Type: `float` (templated on `T`, instantiated for `float` only) - -Reachability-analysis TOPP (H. Pham, Q.-C. Pham, IEEE T-RO 34(3), 2018). Planning-time only; the result -is streamed with `Sample`. - -## Data structures - -`JointTrajectoryState` comes from the shipped `robotics/trajectory/TrajectoryTypes.hpp`. - -```cpp -template using JointVector = math::Vector - -template # static_assert(std::is_floating_point_v); instantiated for float -class PathGeometry: # DI, protected ~PathGeometry() = default — fixed path q(s), s ∈ [0, 1] - virtual JointVector Position(T s) const = 0 # q(s) - virtual JointVector FirstDerivative(T s) const = 0 # q'(s) - virtual JointVector SecondDerivative(T s) const = 0 # q''(s) - -template -struct PathCoefficients: # joint torque along the path: τ = a·s̈ + b·ṡ² + c - JointVector a, b, c - -template -struct JointBounds: - JointVector torqueMin, torqueMax # τmin < τmax - JointVector velocityMax # q̇max > 0 - -template -class JointLimits: # DI, protected ~JointLimits() = default - virtual PathCoefficients Coefficients(T s) const = 0 - virtual const JointBounds& Bounds() const = 0 - -template # production JointLimits, built on M29 -class InverseDynamicsJointLimits : public JointLimits: - const PathGeometry& path - const dynamics::InverseDynamicsModel& dynamics # e.g. ChainDynamicsModel - JointBounds bounds - -template -struct StageRow: # one linear constraint α·x + β·u ≤ γ (x = ṡ², u = s̈) - T alpha, beta, gamma - -template # static_assert(Grid >= 3); gridpoints s_0..s_N, N = Grid − 1 stages -class TimeOptimalPathParameterization: - const PathGeometry& path - const JointLimits& limits - array, Grid> coefficients # a_i, b_i, c_i at s_i - array xMax # velocity bound on x at s_i - array lo, hi # controllable sets K_i = [lo_i, hi_i] - array sDot # sqrt(x_i) of the chosen profile - array u # constant s̈ on stage i (entry N unused) - array stageStartTime # time at s_i; entry N = TotalTime - bool parameterized -``` - -## Interface - -```cpp -TimeOptimalPathParameterization(const PathGeometry& path, const JointLimits& limits) -bool Parameterize() # false if infeasible -std::optional> Sample(T t) const # nullopt until Parameterize() succeeds -T TotalTime() const - -InverseDynamicsJointLimits(const PathGeometry&, const dynamics::InverseDynamicsModel&, - const JointBounds&) -``` - -## Algorithm (pseudocode) - -```cpp -# Along the path q̇ = q'·ṡ, q̈ = q'·u + q''·x, so every joint torque is affine in (x, u): -# τ(s) = a(s)·u + b(s)·x + c(s), a = M q', b = M q'' + C(q,q') q', c = g(q) -# Grid s_i = i/N, Δ_i = s_{i+1} − s_i (uniform 1/N). Stage i holds u_i constant on [s_i, s_{i+1}]: -# x_{i+1} = x_i + 2Δ_i·u_i - -function InverseDynamicsJointLimits::Coefficients(s): # three O(n) RNEA passes (M29 includes gravity) - q = path.Position(s); q1 = path.FirstDerivative(s); q2 = path.SecondDerivative(s) - c = dynamics.ComputeInverseDynamics(q, 0, 0) # RNEA(q, 0, 0, g) = g(q) - a = dynamics.ComputeInverseDynamics(q, 0, q1) − c # RNEA(q, 0, q', 0) = M q' - b = dynamics.ComputeInverseDynamics(q, q1, q2) − c # RNEA(q, q', q'', 0) = M q'' + C(q,q') q' - return { a, b, c } # C(q, q'ṡ) q'ṡ = ṡ²·C(q,q') q' - -function stageRows(i, targetLo, targetHi): # m = 2·Dof + 4 rows, bounded array - for j in 0..Dof−1: - { b_ij, a_ij, τmax_j − c_ij } # τ_j ≤ τmax_j - { −b_ij, −a_ij, c_ij − τmin_j } # τ_j ≥ τmin_j - { 1, 0, xMax_i }, { −1, 0, 0 } # 0 ≤ x ≤ xMax_i - { 1, 2Δ_i, targetHi }, { −1, −2Δ_i, −targetLo } # x + 2Δ_i·u ∈ [targetLo, targetHi] - -function tighten(K, α, γ): # apply α·x ≤ γ to K = [lo, hi] - if α > ε: K.hi = min(K.hi, γ/α) - else if α < −ε: K.lo = max(K.lo, γ/α) - else if γ < −ε: K.empty = true # row violated for every x - -function projectOntoX(rows): # min / max x of the 2-D polygon {(x,u) : rows} - K = [0, xCeiling] - for r in rows with |r.β| ≤ ε: tighten(K, r.α, r.γ) - for p in rows with p.β > ε: # u ≤ (γ_p − α_p·x)/β_p - for n in rows with n.β < −ε: # u ≥ (γ_n − α_n·x)/β_n - tighten(K, p.β·n.α − n.β·p.α, p.β·n.γ − n.β·p.γ) # eliminate u (Fourier–Motzkin) - return (K.empty or K.lo > K.hi + ε) ? empty : K - -function maxControl(rows, x): # 1-D LP: largest u satisfying every row at this x - return min over rows with r.β > ε of (r.γ − r.α·x)/r.β # finite: transition row has β = 2Δ_i - -function Parameterize(): - parameterized = false - for i in 0..N: - q1 = path.FirstDerivative(s_i); coefficients[i] = limits.Coefficients(s_i) - xMax[i] = min over j with |q1_j| > ε of (q̇max_j / q1_j)² # |q'_j·ṡ| ≤ q̇max_j - (xCeiling when every q1_j ≈ 0) - if every q1_j(s_i) ≈ 0: # zero-length path - stageStartTime, sDot, u = 0; parameterized = true; return true - lo[N] = hi[N] = 0 # K_N = [0, 0]: rest at the end - for i = N−1 down to 0: # backward pass: controllable sets - K = projectOntoX(stageRows(i, lo[i+1], hi[i+1])) - if K empty: return false - lo[i], hi[i] = K.lo, K.hi - if lo[0] > ε: return false # 0 ∉ K_0: cannot start from rest - x = 0; sDot[0] = 0; stageStartTime[0] = 0 - for i = 0 .. N−1: # forward pass: greedy, stays inside K_{i+1} - xNext = clamp(x + 2Δ_i·maxControl(stageRows(i, lo[i+1], hi[i+1]), x), lo[i+1], hi[i+1]) - u[i] = (xNext − x) / (2Δ_i) # re-derived after the clamp ⇒ stage ends exactly at s_{i+1} - sDot[i+1] = sqrt(xNext) - if sDot[i] + sDot[i+1] ≤ ε: return false # stalled at rest - stageStartTime[i+1] = stageStartTime[i] + 2Δ_i / (sDot[i] + sDot[i+1]) - x = xNext - parameterized = true; return true - -function Sample(t): # OPTIMIZE_FOR_SPEED - if !parameterized: return nullopt - t = clamp(t, 0, stageStartTime[N]) - i = binary search: last stage i ∈ [0, N−1] with stageStartTime[i] ≤ t - τ = t − stageStartTime[i] - s = s_i + sDot[i]·τ + u[i]·τ²/2; sd = sDot[i] + u[i]·τ; sdd = u[i] - q1 = path.FirstDerivative(s) - return { path.Position(s), q1·sd, q1·sdd + path.SecondDerivative(s)·sd² } -``` - -`projectOntoX` is the exact projection of the stage polygon onto `x`: every `(p, n)` pair is a candidate -vertex (intersection of an upper and a lower `u`-bound line), so `lo`/`hi` are the two 2-variable LP -optima `min x` / `max x` without an iterative solver. - -## Complexity & memory - -- `Parameterize`: `O(Grid·Dof²)` — per stage one projection over `m = 2·Dof + 4` rows (`O(m²)` pairs) and - one `O(m)` 1-D LP, plus one `Coefficients` call (three `O(Dof)` RNEA passes). No iteration, deterministic. -- `Sample`: `O(log Grid)` binary search + three path evaluations. -- Memory: `(3·Dof + 6)·Grid` scalars in bounded `std::array`s (≈ 3 KB for ``); the rows of one - stage live on the stack. No heap — place the planner in static storage. - -## Numerical / embedded notes - -- **Planning-time only**, never in an ISR. For a hard real-time loop, pre-sample the result into a - joint-space `CubicSplineTrajectory` (M33) instead of calling the virtual path geometry per tick. -- The two-variable stage LPs stay local on purpose: they are linear (upstream `ActiveSetQp`, NT65, needs a - strictly convex cost) and the Fourier–Motzkin projection solves them exactly in `O(m²)` with no - iteration. -- Stage time `Δt_i = 2Δ_i / (sqrt(x_i) + sqrt(x_{i+1}))` is exact for constant `u_i` and finite at the rest - endpoints; never integrate `ds / sqrt(x)` (singular at `x = 0`). -- Zero-inertia points (`a_ij = 0`) and `q'_j = 0` need no special case: the row becomes a pure `x` bound or - the joint drops out of `xMax` — no division by zero. `xCeiling` (e.g. `1 / math::Tolerance()`) keeps - every stage polygon bounded when no joint limits the speed. -- Constraints are collocated at gridpoints: between them `τ` may exceed a bound by `O(Δ)` — refine `Grid` - or shrink the bounds by a margin. -- `ε`: relative tolerance on the row scale (`math::Tolerance()`·max|coefficient|). -- The affine form holds for `M q̈ + C q̇ + g` (+ armature, Coulomb friction with fixed sign along the path); - **viscous friction is linear in `ṡ`, not `ṡ²`** — use a friction-free model or a torque margin. -- Infeasible iff some `K_i` is empty or `0 ∉ K_0`; both surface as `Parameterize() == false`. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `robotics/trajectory/TimeOptimalPathParameterization.hpp` (interfaces `PathGeometry`, - `JointLimits`, the `InverseDynamicsJointLimits` adapter and the planner) — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Sample`, and - `extern template class TimeOptimalPathParameterization;` / - `extern template class InverseDynamicsJointLimits;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Uses `robotics/trajectory/TrajectoryTypes.hpp` and `robotics/dynamics/InverseDynamicsModel.hpp` (M29); - `robotics.trajectory` links `robotics.dynamics`. -- Coverage: `robotics/trajectory/TimeOptimalPathParameterization.cpp` → - `template class TimeOptimalPathParameterization;` and - `template class InverseDynamicsJointLimits;` -- Test: `robotics/trajectory/test/TestTimeOptimalPathParameterization.cpp` -- Doc: `doc/trajectory/TimeOptimalPathParameterization.md` (per `doc/TEMPLATE.md`) + row in `doc/trajectory/README.md`. -- CMake: `.hpp` → `target_sources(robotics.trajectory …)`; - `robotics_add_coverage_sources(robotics.trajectory TimeOptimalPathParameterization.cpp)`; - `TestTimeOptimalPathParameterization.cpp` → `robotics.trajectory_test`. -- New module (first trajectory item only): `robotics/trajectory/CMakeLists.txt` with - `robotics_add_header_library(robotics.trajectory)`, `TrajectoryTypes.hpp` in `target_sources`, a - `test/` subdir, `add_subdirectory(trajectory)` in `robotics/CMakeLists.txt`, and a `doc/trajectory/` folder. -- Depends on: M29 (`InverseDynamicsModel`, `ChainDynamicsModel`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/trajectory/TimeOptimalPathParameterization/tests.md b/roadmap/trajectory/TimeOptimalPathParameterization/tests.md deleted file mode 100644 index 12f4ca3..0000000 --- a/roadmap/trajectory/TimeOptimalPathParameterization/tests.md +++ /dev/null @@ -1,103 +0,0 @@ -# Time-Optimal Path Parameterization (TOPP-RA) — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class PathGeometryMock : public PathGeometry # MOCK_METHOD Position / FirstDerivative / SecondDerivative -class JointLimitsMock : public JointLimits # MOCK_METHOD Coefficients / Bounds -class InverseDynamicsModelMock : public dynamics::InverseDynamicsModel # MOCK_METHOD ComputeInverseDynamics - -class TestTopp : public ::testing::Test: - StrictMock path - StrictMock limits - TimeOptimalPathParameterization topp{ path, limits } # N = 63 stages, Δ = 1/63 - JointBounds unitTorque{ .torqueMin = (−1, −1), .torqueMax = (1, 1), .velocityMax = (100, 100) } - - void GivenStraightSingleJointPath(float L, JointBounds bounds): - # q(s) = (L·s, 0), unit inertia, no gravity: a = (L, 0), b = 0, c = 0 - EXPECT_CALL(path, Position(_)).WillRepeatedly(Return((L·s, 0))) - EXPECT_CALL(path, FirstDerivative(_)).WillRepeatedly(Return((L, 0))) - EXPECT_CALL(path, SecondDerivative(_)).WillRepeatedly(Return((0, 0))) - EXPECT_CALL(limits, Coefficients(_)).WillRepeatedly(Return({ (L, 0), (0, 0), (0, 0) })) - EXPECT_CALL(limits, Bounds()).WillRepeatedly(ReturnRef(bounds)) -# each case below is a TEST_F(TestTopp, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -bang_bang_total_time_matches_closed_form: - Arrange: GivenStraightSingleJointPath(L = 1, unitTorque) - Act: Parameterize() - Assert: true; TotalTime() ≈ 2·sqrt(L) = 2 (tol 1e-3; exact value 2.000064 for Grid 64) - -bang_bang_acceleration_saturates_torque: - Arrange: as above, L = 4 - Assert: |Sample(t).acceleration_0| ≈ 1 (= |τ|max / inertia) for t in the first and last 45 % of - TotalTime(); positive before TotalTime()/2, negative after - -velocity_limit_caps_path_speed: - Arrange: GivenStraightSingleJointPath(L = 1, { torque ±100, velocityMax = (0.5, 0.5) }) - Assert: every sampled |velocity_0| ≤ 0.5·(1 + tol); Sample(TotalTime()/2).velocity_0 ≈ 0.5; - TotalTime() ≈ 2 + 4Δ = 2.063492 - -profile_respects_torque_and_velocity_bounds: - Arrange: q(s) = (s, 0.4s − 0.4s²) ⇒ q' = (1, 0.4 − 0.8s), q'' = (0, −0.8); - coupled mock coefficients a = (1 + 0.5s, 0.4 − 0.8s), b = (0.3·cos 3s, −0.2 + 0.1s), - c = (0.1, −0.05s); τ ∈ [(−1, −0.6), (1, 0.6)], q̇max = (1.5, 0.8) - Act: Parameterize(); dense Sample(t) (s = position_0, ṡ = velocity_0, s̈ = acceleration_0) - Assert: true; TotalTime() ≈ 2.256496; |q̇_j| ≤ q̇max_j; a(s)·s̈ + b(s)·ṡ² + c(s) ∈ [τmin − 0.02, τmax + 0.02] - (collocation: exact at gridpoints, O(Δ) between them — 0.018 measured) - -starts_and_ends_at_rest_on_path_endpoints: - Arrange: GivenStraightSingleJointPath(L = 1, unitTorque) - Assert: Sample(0) = { q(0), 0, · } and Sample(TotalTime()) = { q(1), ≈0, · } - -sample_is_continuous_across_stages: - Arrange: GivenStraightSingleJointPath(L = 1, unitTorque) - Assert: for every interior stage start time t_i: Sample(t_i − δ) ≈ Sample(t_i + δ) in position and - velocity (δ = 1e-4·TotalTime()) - -infeasible_static_torque_reports_failure: - Arrange: straight path with c = (2, 0) > τmax = 1 (gravity cannot be held) - Assert: Parameterize() == false and Sample(0) == nullopt - -sample_before_parameterize_returns_nullopt: - Assert: Sample(0) == nullopt - -zero_length_path_takes_zero_time: - Arrange: q'(s) = q''(s) = (0, 0), q(s) = (0.3, −0.2) - Assert: Parameterize() == true, TotalTime() == 0, Sample(0) = { (0.3, −0.2), 0, 0 } - -inverse_dynamics_adapter_builds_coefficients: - Arrange: StrictMock dyn; path mock q = (0.1, 0.2), q' = (1, 2), q'' = (3, 4) - EXPECT_CALL(dyn, ComputeInverseDynamics(q, 0, 0)) → g = (0.5, 0.25) - EXPECT_CALL(dyn, ComputeInverseDynamics(q, 0, q')) → g + Mq' = (1.5, 2.25) - EXPECT_CALL(dyn, ComputeInverseDynamics(q, q', q'')) → g + Mq'' + Cq' = (4.5, 5.25) - Act: InverseDynamicsJointLimits{ path, dyn, unitTorque }.Coefficients(0.5) - Assert: a ≈ (1, 2), b ≈ (4, 5), c ≈ (0.5, 0.25) -``` - -## Reference vectors - -Computed with a python3 implementation of exactly this pseudocode. - -- Single joint, unit inertia, `|τ| ≤ 1`, path length `L`, rest-to-rest ⇒ bang-bang time `2·sqrt(L)`. - Grid 64 (63 stages, odd ⇒ one coast stage at the midpoint): `L = 1 → 2.000064`, `L = 4 → 4.000128` - (relative error `3.2e-5`); Grid 65 (even stage count): exact to `1e-15`; Grid 128: `7.8e-6`. -- Velocity-limited straight segment, `q̇max = 0.5`, `|τ| ≤ 100`, `L = 1`, Grid 64 ⇒ `ṡ = 0.5` on the - interior, one full ramp stage at each end: `TotalTime = (N − 2)·Δ/0.5 + 2·(2Δ/0.5) = 2 + 4/63 = 2.063492`. -- Coupled case above ⇒ feasible, `TotalTime = 2.256496`; torque bound violation `1e-12` at gridpoints, - `0.0178` between them (Grid 64); velocity bounds never violated. -- `c = 2 > τmax = 1` ⇒ every stage forces `u ≤ −1`, so `0 ∉ K_0` ⇒ infeasible. -- Adapter on a 2R planar arm: `τ(q'ṡ, q'u + q''ṡ²) − (a·u + b·ṡ² + c)` ≤ `2e-15` for arbitrary `(ṡ, u)`. - -## Edge cases - -- Zero-length path (`q'` ≡ 0) ⇒ `TotalTime() == 0`, trivial success. -- `q'_j(s) = 0` or `a_ij(s) = 0` at a gridpoint ⇒ no division, row becomes a pure `x` bound. -- Velocity-only vs torque-only limiting ⇒ the binding row changes per gridpoint; both cases above. -- `Grid = 2` (one stage) cannot leave and return to rest with one constant `u` ⇒ stalls; hence - `static_assert(Grid >= 3)`. `Grid = 3` on the bang-bang case gives exactly `2·sqrt(L)`. diff --git a/robotics/trajectory/CMakeLists.txt b/robotics/trajectory/CMakeLists.txt index c5174fc..5df7077 100644 --- a/robotics/trajectory/CMakeLists.txt +++ b/robotics/trajectory/CMakeLists.txt @@ -9,17 +9,21 @@ target_link_libraries(robotics.trajectory ${ROBOTICS_VISIBILITY} infra.util numerical.math numerical.motion_profiles + robotics.dynamics ) target_sources(robotics.trajectory PRIVATE CartesianSlerpInterpolation.hpp CubicSplineTrajectory.hpp + TimeOptimalPathParameterization.hpp TrajectoryTypes.hpp ) robotics_add_coverage_sources(robotics.trajectory CartesianSlerpInterpolation.cpp CubicSplineTrajectory.cpp + TimeOptimalPathParameterization.cpp ) add_subdirectory(test) +add_subdirectory(test_doubles) diff --git a/robotics/trajectory/TimeOptimalPathParameterization.cpp b/robotics/trajectory/TimeOptimalPathParameterization.cpp new file mode 100644 index 0000000..35f1aed --- /dev/null +++ b/robotics/trajectory/TimeOptimalPathParameterization.cpp @@ -0,0 +1,7 @@ +#include "robotics/trajectory/TimeOptimalPathParameterization.hpp" + +namespace trajectory +{ + template class InverseDynamicsPathConstraints; + template class TimeOptimalPathParameterization; +} diff --git a/robotics/trajectory/TimeOptimalPathParameterization.hpp b/robotics/trajectory/TimeOptimalPathParameterization.hpp new file mode 100644 index 0000000..e8d3b20 --- /dev/null +++ b/robotics/trajectory/TimeOptimalPathParameterization.hpp @@ -0,0 +1,347 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/Tolerance.hpp" +#include "robotics/dynamics/InverseDynamicsModel.hpp" +#include "robotics/trajectory/TrajectoryTypes.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace trajectory +{ + template + class PathGeometry + { + static_assert(std::is_floating_point_v, "PathGeometry supports floating-point types"); + + public: + using JointVector = math::Vector; + + virtual JointVector Position(T s) const = 0; + virtual JointVector FirstDerivative(T s) const = 0; + virtual JointVector SecondDerivative(T s) const = 0; + + protected: + ~PathGeometry() = default; + }; + + template + struct PathCoefficients + { + math::Vector a{}; + math::Vector b{}; + math::Vector c{}; + }; + + template + struct JointBounds + { + math::Vector torqueMin{}; + math::Vector torqueMax{}; + math::Vector velocityMax{}; + }; + + template + class PathConstraints + { + static_assert(std::is_floating_point_v, "PathConstraints supports floating-point types"); + + public: + virtual PathCoefficients Coefficients(T s) const = 0; + virtual const JointBounds& Bounds() const = 0; + + protected: + ~PathConstraints() = default; + }; + + template + class InverseDynamicsPathConstraints + : public PathConstraints + { + public: + InverseDynamicsPathConstraints(const PathGeometry& path, const dynamics::InverseDynamicsModel& model, const JointBounds& bounds); + + PathCoefficients Coefficients(T s) const override; + const JointBounds& Bounds() const override; + + private: + const PathGeometry& path; + const dynamics::InverseDynamicsModel& model; + JointBounds bounds; + }; + + template + class TimeOptimalPathParameterization + { + static_assert(std::is_floating_point_v, "TimeOptimalPathParameterization supports floating-point types"); + static_assert(Grid >= 3, "TimeOptimalPathParameterization needs at least two stages"); + + public: + using JointVector = math::Vector; + using State = JointTrajectoryState; + + TimeOptimalPathParameterization(const PathGeometry& path, const PathConstraints& constraints); + + bool Parameterize(); + OPTIMIZE_FOR_SPEED std::optional Sample(T t) const; + T TotalTime() const; + + private: + struct StageRow + { + T alpha; + T beta; + T gamma; + }; + + struct Interval + { + T lo; + T hi; + bool empty; + }; + + static constexpr std::size_t stages{ Grid - 1 }; + static constexpr std::size_t rowCount{ 2 * Dof + 4 }; + static constexpr T step{ T{ 1 } / static_cast(stages) }; + static constexpr T epsilon{ std::numeric_limits::epsilon() * T{ 64 } }; + + using StageRows = std::array; + + static T GridPoint(std::size_t i); + static void Tighten(Interval& interval, T alpha, T gamma); + static std::optional ProjectOntoX(const StageRows& rows); + static T MaximumControl(const StageRows& rows, T x); + static T SpeedLimit(const JointVector& firstDerivative, const JointBounds& bounds); + static T Ceiling(); + + StageRows Rows(std::size_t i, const JointBounds& bounds) const; + bool Integrate(); + + const PathGeometry& path; + const PathConstraints& constraints; + std::array, Grid> coefficients{}; + std::array xMax{}; + std::array lo{}; + std::array hi{}; + std::array sDot{}; + std::array u{}; + std::array stageStartTime{}; + bool parameterized{ false }; + }; + + template + InverseDynamicsPathConstraints::InverseDynamicsPathConstraints(const PathGeometry& path, const dynamics::InverseDynamicsModel& model, const JointBounds& bounds) + : path(path) + , model(model) + , bounds(bounds) + {} + + template + PathCoefficients InverseDynamicsPathConstraints::Coefficients(T s) const + { + const auto q{ path.Position(s) }; + const auto firstDerivative{ path.FirstDerivative(s) }; + const auto secondDerivative{ path.SecondDerivative(s) }; + const math::Vector zero{}; + + const auto c{ model.ComputeInverseDynamics(q, zero, zero) }; + const auto a{ model.ComputeInverseDynamics(q, zero, firstDerivative) - c }; + const auto b{ model.ComputeInverseDynamics(q, firstDerivative, secondDerivative) - c }; + return PathCoefficients{ a, b, c }; + } + + template + const JointBounds& InverseDynamicsPathConstraints::Bounds() const + { + return bounds; + } + + template + TimeOptimalPathParameterization::TimeOptimalPathParameterization(const PathGeometry& path, const PathConstraints& constraints) + : path(path) + , constraints(constraints) + {} + + template + bool TimeOptimalPathParameterization::Parameterize() + { + parameterized = false; + const auto& bounds{ constraints.Bounds() }; + bool moving{ false }; + for (std::size_t i = 0; i < Grid; ++i) + { + const auto firstDerivative{ path.FirstDerivative(GridPoint(i)) }; + coefficients[i] = constraints.Coefficients(GridPoint(i)); + xMax[i] = SpeedLimit(firstDerivative, bounds); + for (std::size_t j = 0; j < Dof; ++j) + moving = moving || std::abs(firstDerivative[j]) > epsilon; + } + + if (!moving) + { + sDot.fill(T{}); + u.fill(T{}); + stageStartTime.fill(T{}); + parameterized = true; + return true; + } + + lo[stages] = T{}; + hi[stages] = T{}; + for (std::size_t k = stages; k > 0; --k) + { + const auto interval{ ProjectOntoX(Rows(k - 1, bounds)) }; + if (!interval) + return false; + lo[k - 1] = interval->lo; + hi[k - 1] = interval->hi; + } + + if (lo[0] > epsilon) + return false; + + parameterized = Integrate(); + return parameterized; + } + + template + bool TimeOptimalPathParameterization::Integrate() + { + const auto& bounds{ constraints.Bounds() }; + T x{}; + sDot[0] = T{}; + stageStartTime[0] = T{}; + for (std::size_t i = 0; i < stages; ++i) + { + const T reach{ x + T{ 2 } * step * MaximumControl(Rows(i, bounds), x) }; + const T xNext{ std::max(std::min(reach, hi[i + 1]), lo[i + 1]) }; + u[i] = (xNext - x) / (T{ 2 } * step); + sDot[i + 1] = math::Sqrt(xNext); + if (sDot[i] + sDot[i + 1] <= epsilon) + return false; + stageStartTime[i + 1] = stageStartTime[i] + T{ 2 } * step / (sDot[i] + sDot[i + 1]); + x = xNext; + } + return true; + } + + template + OPTIMIZE_FOR_SPEED auto TimeOptimalPathParameterization::Sample(T t) const -> std::optional + { + if (!parameterized) + return std::nullopt; + + const T time{ std::clamp(t, T{}, stageStartTime[stages]) }; + const auto first{ stageStartTime.begin() }; + const auto next{ std::upper_bound(first, first + stages, time) }; + const auto i{ static_cast(next - first) - 1 }; + + const T tau{ time - stageStartTime[i] }; + const T s{ GridPoint(i) + sDot[i] * tau + u[i] * tau * tau / T{ 2 } }; + const T speed{ sDot[i] + u[i] * tau }; + const auto firstDerivative{ path.FirstDerivative(s) }; + return State{ path.Position(s), firstDerivative * speed, firstDerivative * u[i] + path.SecondDerivative(s) * (speed * speed) }; + } + + template + T TimeOptimalPathParameterization::TotalTime() const + { + return stageStartTime[stages]; + } + + template + T TimeOptimalPathParameterization::GridPoint(std::size_t i) + { + return static_cast(i) / static_cast(stages); + } + + template + T TimeOptimalPathParameterization::Ceiling() + { + return T{ 1 } / static_cast(math::Tolerance()); + } + + template + T TimeOptimalPathParameterization::SpeedLimit(const JointVector& firstDerivative, const JointBounds& bounds) + { + T limit{ Ceiling() }; + for (std::size_t j = 0; j < Dof; ++j) + if (std::abs(firstDerivative[j]) > epsilon) + { + const T ratio{ bounds.velocityMax[j] / firstDerivative[j] }; + limit = std::min(limit, ratio * ratio); + } + return limit; + } + + template + auto TimeOptimalPathParameterization::Rows(std::size_t i, const JointBounds& bounds) const -> StageRows + { + const auto& coefficient{ coefficients[i] }; + StageRows rows{}; + for (std::size_t j = 0; j < Dof; ++j) + { + rows[2 * j] = StageRow{ coefficient.b[j], coefficient.a[j], bounds.torqueMax[j] - coefficient.c[j] }; + rows[2 * j + 1] = StageRow{ -coefficient.b[j], -coefficient.a[j], coefficient.c[j] - bounds.torqueMin[j] }; + } + + rows[2 * Dof] = StageRow{ T{ 1 }, T{}, xMax[i] }; + rows[2 * Dof + 1] = StageRow{ T{ -1 }, T{}, T{} }; + rows[2 * Dof + 2] = StageRow{ T{ 1 }, T{ 2 } * step, hi[i + 1] }; + rows[2 * Dof + 3] = StageRow{ T{ -1 }, T{ -2 } * step, -lo[i + 1] }; + return rows; + } + + template + void TimeOptimalPathParameterization::Tighten(Interval& interval, T alpha, T gamma) + { + if (alpha > epsilon) + interval.hi = std::min(interval.hi, gamma / alpha); + else if (alpha < -epsilon) + interval.lo = std::max(interval.lo, gamma / alpha); + else if (gamma < -epsilon) + interval.empty = true; + } + + template + auto TimeOptimalPathParameterization::ProjectOntoX(const StageRows& rows) -> std::optional + { + Interval interval{ T{}, Ceiling(), false }; + for (const auto& row : rows) + if (std::abs(row.beta) <= epsilon) + Tighten(interval, row.alpha, row.gamma); + + for (const auto& upper : rows) + if (upper.beta > epsilon) + for (const auto& lower : rows) + if (lower.beta < -epsilon) + Tighten(interval, upper.beta * lower.alpha - lower.beta * upper.alpha, upper.beta * lower.gamma - lower.beta * upper.gamma); + + if (interval.empty || interval.lo > interval.hi + epsilon) + return std::nullopt; + return interval; + } + + template + T TimeOptimalPathParameterization::MaximumControl(const StageRows& rows, T x) + { + T control{ std::numeric_limits::max() }; + for (const auto& row : rows) + if (row.beta > epsilon) + control = std::min(control, (row.gamma - row.alpha * x) / row.beta); + return control; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class InverseDynamicsPathConstraints; + extern template class TimeOptimalPathParameterization; +#endif +} diff --git a/robotics/trajectory/test/CMakeLists.txt b/robotics/trajectory/test/CMakeLists.txt index 2ae0194..e747a40 100644 --- a/robotics/trajectory/test/CMakeLists.txt +++ b/robotics/trajectory/test/CMakeLists.txt @@ -5,10 +5,13 @@ emil_add_test(robotics.trajectory_test) target_link_libraries(robotics.trajectory_test PUBLIC gmock_main numerical.motion_profiles + robotics.dynamics_test_doubles robotics.trajectory + robotics.trajectory_test_doubles ) target_sources(robotics.trajectory_test PRIVATE TestCartesianSlerpInterpolation.cpp TestCubicSplineTrajectory.cpp + TestTimeOptimalPathParameterization.cpp ) diff --git a/robotics/trajectory/test/TestTimeOptimalPathParameterization.cpp b/robotics/trajectory/test/TestTimeOptimalPathParameterization.cpp new file mode 100644 index 0000000..c05c281 --- /dev/null +++ b/robotics/trajectory/test/TestTimeOptimalPathParameterization.cpp @@ -0,0 +1,236 @@ +#include "robotics/dynamics/test_doubles/MockInverseDynamicsModel.hpp" +#include "robotics/trajectory/TimeOptimalPathParameterization.hpp" +#include "robotics/trajectory/test_doubles/MockPathConstraints.hpp" +#include "robotics/trajectory/test_doubles/MockPathGeometry.hpp" +#include +#include +#include + +namespace +{ + using Vector2 = math::Vector; + using Coefficients = trajectory::PathCoefficients; + using Bounds = trajectory::JointBounds; + using Topp = trajectory::TimeOptimalPathParameterization; + + MATCHER_P(VectorNear, expected, "") + { + return std::abs(arg[0] - expected[0]) < 1e-6f && std::abs(arg[1] - expected[1]) < 1e-6f; + } + + Vector2 CurvedPosition(float s) + { + return Vector2{ s, 0.4f * s - 0.4f * s * s }; + } + + Vector2 CurvedFirstDerivative(float s) + { + return Vector2{ 1.0f, 0.4f - 0.8f * s }; + } + + Coefficients CoupledCoefficients(float s) + { + return Coefficients{ Vector2{ 1.0f + 0.5f * s, 0.4f - 0.8f * s }, Vector2{ 0.3f * std::cos(3.0f * s), -0.2f + 0.1f * s }, Vector2{ 0.1f, -0.05f * s } }; + } + + class TestTimeOptimalPathParameterization + : public ::testing::Test + { + protected: + void GivenStraightSingleJointPath(float length, const Bounds& jointBounds, const Vector2& holdingTorque = Vector2{}) + { + bounds = jointBounds; + EXPECT_CALL(path, Position(testing::_)).WillRepeatedly(testing::Invoke([length](float s) + { + return Vector2{ length * s, 0.0f }; + })); + EXPECT_CALL(path, FirstDerivative(testing::_)).WillRepeatedly(testing::Return(Vector2{ length, 0.0f })); + EXPECT_CALL(path, SecondDerivative(testing::_)).WillRepeatedly(testing::Return(Vector2{})); + EXPECT_CALL(constraints, Coefficients(testing::_)).WillRepeatedly(testing::Return(Coefficients{ Vector2{ length, 0.0f }, Vector2{}, holdingTorque })); + EXPECT_CALL(constraints, Bounds()).WillRepeatedly(testing::ReturnRef(bounds)); + } + + float TimeAt(float s) const + { + float low{ 0.0f }; + float high{ topp.TotalTime() }; + for (std::size_t k = 0; k < 40; ++k) + { + const float middle{ (low + high) / 2.0f }; + if (topp.Sample(middle)->position[0] < s) + low = middle; + else + high = middle; + } + return (low + high) / 2.0f; + } + + static void ExpectNear(const Vector2& actual, const Vector2& expected, float tolerance) + { + EXPECT_NEAR(actual[0], expected[0], tolerance); + EXPECT_NEAR(actual[1], expected[1], tolerance); + } + + testing::StrictMock> path; + testing::StrictMock> constraints; + Topp topp{ path, constraints }; + Bounds unitTorque{ Vector2{ -1.0f, -1.0f }, Vector2{ 1.0f, 1.0f }, Vector2{ 100.0f, 100.0f } }; + Bounds bounds{}; + }; +} + +TEST_F(TestTimeOptimalPathParameterization, bang_bang_total_time_matches_closed_form) +{ + GivenStraightSingleJointPath(1.0f, unitTorque); + + ASSERT_TRUE(topp.Parameterize()); + EXPECT_NEAR(topp.TotalTime(), 2.0f, 1e-3f); +} + +TEST_F(TestTimeOptimalPathParameterization, bang_bang_acceleration_saturates_torque) +{ + GivenStraightSingleJointPath(4.0f, unitTorque); + + ASSERT_TRUE(topp.Parameterize()); + const float total{ topp.TotalTime() }; + for (std::size_t k = 0; k <= 45; ++k) + { + const float t{ total * 0.01f * static_cast(k) }; + EXPECT_NEAR(topp.Sample(t)->acceleration[0], 1.0f, 1e-3f); + EXPECT_NEAR(topp.Sample(total - t)->acceleration[0], -1.0f, 1e-3f); + } +} + +TEST_F(TestTimeOptimalPathParameterization, velocity_limit_caps_path_speed) +{ + GivenStraightSingleJointPath(1.0f, Bounds{ Vector2{ -100.0f, -100.0f }, Vector2{ 100.0f, 100.0f }, Vector2{ 0.5f, 0.5f } }); + + ASSERT_TRUE(topp.Parameterize()); + const float total{ topp.TotalTime() }; + for (std::size_t k = 0; k <= 200; ++k) + EXPECT_LE(std::abs(topp.Sample(total * 0.005f * static_cast(k))->velocity[0]), 0.5f * (1.0f + 1e-4f)); + EXPECT_NEAR(topp.Sample(total / 2.0f)->velocity[0], 0.5f, 1e-4f); + EXPECT_NEAR(total, 2.063492f, 1e-4f); +} + +TEST_F(TestTimeOptimalPathParameterization, profile_respects_torque_and_velocity_bounds) +{ + bounds = Bounds{ Vector2{ -1.0f, -0.6f }, Vector2{ 1.0f, 0.6f }, Vector2{ 1.5f, 0.8f } }; + EXPECT_CALL(path, Position(testing::_)).WillRepeatedly(testing::Invoke(CurvedPosition)); + EXPECT_CALL(path, FirstDerivative(testing::_)).WillRepeatedly(testing::Invoke(CurvedFirstDerivative)); + EXPECT_CALL(path, SecondDerivative(testing::_)).WillRepeatedly(testing::Return(Vector2{ 0.0f, -0.8f })); + EXPECT_CALL(constraints, Coefficients(testing::_)).WillRepeatedly(testing::Invoke(CoupledCoefficients)); + EXPECT_CALL(constraints, Bounds()).WillRepeatedly(testing::ReturnRef(bounds)); + + ASSERT_TRUE(topp.Parameterize()); + EXPECT_NEAR(topp.TotalTime(), 2.256496f, 1e-4f); + for (std::size_t k = 0; k <= 1000; ++k) + { + const auto state{ *topp.Sample(topp.TotalTime() * 0.001f * static_cast(k)) }; + const float speed{ state.velocity[0] }; + const auto coefficients{ CoupledCoefficients(state.position[0]) }; + for (std::size_t j = 0; j < 2; ++j) + { + const float torque{ coefficients.a[j] * state.acceleration[0] + coefficients.b[j] * speed * speed + coefficients.c[j] }; + EXPECT_GE(torque, bounds.torqueMin[j] - 0.02f); + EXPECT_LE(torque, bounds.torqueMax[j] + 0.02f); + EXPECT_LE(std::abs(state.velocity[j]), bounds.velocityMax[j] + 1e-4f); + } + } +} + +TEST_F(TestTimeOptimalPathParameterization, starts_and_ends_at_rest_on_path_endpoints) +{ + GivenStraightSingleJointPath(1.0f, unitTorque); + + ASSERT_TRUE(topp.Parameterize()); + const auto start{ *topp.Sample(0.0f) }; + const auto end{ *topp.Sample(topp.TotalTime()) }; + ExpectNear(start.position, Vector2{}, 1e-6f); + ExpectNear(start.velocity, Vector2{}, 1e-6f); + ExpectNear(end.position, Vector2{ 1.0f, 0.0f }, 1e-5f); + ExpectNear(end.velocity, Vector2{}, 1e-4f); +} + +TEST_F(TestTimeOptimalPathParameterization, sample_is_continuous_across_stages) +{ + GivenStraightSingleJointPath(1.0f, unitTorque); + + ASSERT_TRUE(topp.Parameterize()); + const float delta{ 1e-4f * topp.TotalTime() }; + for (std::size_t i = 1; i < 63; ++i) + { + const float t{ TimeAt(static_cast(i) / 63.0f) }; + const auto before{ *topp.Sample(t - delta) }; + const auto after{ *topp.Sample(t + delta) }; + ExpectNear(before.position, after.position, 1e-3f); + ExpectNear(before.velocity, after.velocity, 1e-3f); + } +} + +TEST_F(TestTimeOptimalPathParameterization, infeasible_static_torque_reports_failure) +{ + GivenStraightSingleJointPath(1.0f, unitTorque, Vector2{ 2.0f, 0.0f }); + + EXPECT_FALSE(topp.Parameterize()); + EXPECT_FALSE(topp.Sample(0.0f).has_value()); +} + +TEST_F(TestTimeOptimalPathParameterization, stationary_joint_overload_empties_controllable_set) +{ + GivenStraightSingleJointPath(1.0f, unitTorque, Vector2{ 0.0f, 2.0f }); + + EXPECT_FALSE(topp.Parameterize()); + EXPECT_FALSE(topp.Sample(0.0f).has_value()); +} + +TEST_F(TestTimeOptimalPathParameterization, zero_velocity_bound_stalls_at_rest) +{ + GivenStraightSingleJointPath(1.0f, Bounds{ unitTorque.torqueMin, unitTorque.torqueMax, Vector2{ 0.0f, 100.0f } }); + + EXPECT_FALSE(topp.Parameterize()); + EXPECT_FALSE(topp.Sample(0.0f).has_value()); +} + +TEST_F(TestTimeOptimalPathParameterization, sample_before_parameterize_returns_nullopt) +{ + EXPECT_FALSE(topp.Sample(0.0f).has_value()); +} + +TEST_F(TestTimeOptimalPathParameterization, zero_length_path_takes_zero_time) +{ + bounds = unitTorque; + EXPECT_CALL(path, Position(testing::_)).WillRepeatedly(testing::Return(Vector2{ 0.3f, -0.2f })); + EXPECT_CALL(path, FirstDerivative(testing::_)).WillRepeatedly(testing::Return(Vector2{})); + EXPECT_CALL(path, SecondDerivative(testing::_)).WillRepeatedly(testing::Return(Vector2{})); + EXPECT_CALL(constraints, Coefficients(testing::_)).WillRepeatedly(testing::Return(Coefficients{})); + EXPECT_CALL(constraints, Bounds()).WillRepeatedly(testing::ReturnRef(bounds)); + + ASSERT_TRUE(topp.Parameterize()); + EXPECT_EQ(topp.TotalTime(), 0.0f); + const auto state{ *topp.Sample(0.0f) }; + ExpectNear(state.position, Vector2{ 0.3f, -0.2f }, 1e-6f); + ExpectNear(state.velocity, Vector2{}, 1e-6f); + ExpectNear(state.acceleration, Vector2{}, 1e-6f); +} + +TEST_F(TestTimeOptimalPathParameterization, inverse_dynamics_adapter_builds_coefficients) +{ + testing::StrictMock> model; + const Vector2 q{ 0.1f, 0.2f }; + const Vector2 firstDerivative{ 1.0f, 2.0f }; + const Vector2 secondDerivative{ 3.0f, 4.0f }; + EXPECT_CALL(path, Position(0.5f)).WillOnce(testing::Return(q)); + EXPECT_CALL(path, FirstDerivative(0.5f)).WillOnce(testing::Return(firstDerivative)); + EXPECT_CALL(path, SecondDerivative(0.5f)).WillOnce(testing::Return(secondDerivative)); + EXPECT_CALL(model, ComputeInverseDynamics(VectorNear(q), VectorNear(Vector2{}), VectorNear(Vector2{}))).WillOnce(testing::Return(Vector2{ 0.5f, 0.25f })); + EXPECT_CALL(model, ComputeInverseDynamics(VectorNear(q), VectorNear(Vector2{}), VectorNear(firstDerivative))).WillOnce(testing::Return(Vector2{ 1.5f, 2.25f })); + EXPECT_CALL(model, ComputeInverseDynamics(VectorNear(q), VectorNear(firstDerivative), VectorNear(secondDerivative))).WillOnce(testing::Return(Vector2{ 4.5f, 5.25f })); + + const trajectory::InverseDynamicsPathConstraints adapter{ path, model, unitTorque }; + const auto coefficients{ adapter.Coefficients(0.5f) }; + ExpectNear(coefficients.a, Vector2{ 1.0f, 2.0f }, 1e-6f); + ExpectNear(coefficients.b, Vector2{ 4.0f, 5.0f }, 1e-6f); + ExpectNear(coefficients.c, Vector2{ 0.5f, 0.25f }, 1e-6f); + ExpectNear(adapter.Bounds().torqueMax, unitTorque.torqueMax, 0.0f); +} diff --git a/robotics/trajectory/test_doubles/CMakeLists.txt b/robotics/trajectory/test_doubles/CMakeLists.txt new file mode 100644 index 0000000..c6e345f --- /dev/null +++ b/robotics/trajectory/test_doubles/CMakeLists.txt @@ -0,0 +1,12 @@ +add_library(robotics.trajectory_test_doubles INTERFACE) +emil_build_for(robotics.trajectory_test_doubles BOOL ROBOTICS_TOOLBOX_BUILD_TESTS) + +target_link_libraries(robotics.trajectory_test_doubles INTERFACE + gmock + robotics.trajectory +) + +target_sources(robotics.trajectory_test_doubles PRIVATE + MockPathConstraints.hpp + MockPathGeometry.hpp +) diff --git a/robotics/trajectory/test_doubles/MockPathConstraints.hpp b/robotics/trajectory/test_doubles/MockPathConstraints.hpp new file mode 100644 index 0000000..b8e6190 --- /dev/null +++ b/robotics/trajectory/test_doubles/MockPathConstraints.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include "robotics/trajectory/TimeOptimalPathParameterization.hpp" +#include + +namespace trajectory +{ + template + class MockPathConstraints + : public PathConstraints + { + public: + MOCK_METHOD((PathCoefficients), Coefficients, (T s), (const, override)); + MOCK_METHOD((const JointBounds&), Bounds, (), (const, override)); + }; +} diff --git a/robotics/trajectory/test_doubles/MockPathGeometry.hpp b/robotics/trajectory/test_doubles/MockPathGeometry.hpp new file mode 100644 index 0000000..3969ce4 --- /dev/null +++ b/robotics/trajectory/test_doubles/MockPathGeometry.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include "robotics/trajectory/TimeOptimalPathParameterization.hpp" +#include + +namespace trajectory +{ + template + class MockPathGeometry + : public PathGeometry + { + public: + using typename PathGeometry::JointVector; + + MOCK_METHOD(JointVector, Position, (T s), (const, override)); + MOCK_METHOD(JointVector, FirstDerivative, (T s), (const, override)); + MOCK_METHOD(JointVector, SecondDerivative, (T s), (const, override)); + }; +}