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)); + }; +}