diff --git a/ROADMAP.md b/ROADMAP.md index 57814d7..c5ef9d1 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -1,7 +1,8 @@ # Robotics Toolbox — Roadmap -Prioritized backlog by robot family. Manipulators (**M**) are the most developed; the other families are -entries only so far — their specs (`roadmap///`) come family by family: +Prioritized backlog by robot family. Manipulators (**M**) and wheeled robots (**W**, with **F1**) have specs +(`roadmap///`); the other families are entries only so far, and their specs come family by +family: | Prefix | Family | Section | |--------|---------------------------------------------------------------|---------------------------------------------------------------------------| @@ -39,12 +40,13 @@ this roadmap needs that upstream does not ship yet are specified and built upstr Generic algorithms live in numerical-toolbox-cpp and are referenced here as **NT** plus its master-list item number (NT53 is upstream item 53). A robotics item that depends on one needs a -numerical-toolbox pin that ships it. +numerical-toolbox pin that ships it. NT53–NT65 have shipped upstream, and their links point to the upstream +docs; using them needs a pin at or after `1f6d717` (the current pin, `adfc10d`, predates them). -| # | Component | Upstream module | Spec | Needed by | +| # | Component | Upstream module | Spec / doc | Needed by | |------|-------------------------------------------------------|----------------------|-----------------------------------|----------------------------------------------------------------------------| | NT53 | SE(3) transform, twists, wrenches, exp/log, PoseError | `math` | [SE3Transform][nt53] | M7, M8, M10, M13, M15, M21, M23, M25, M26, M30, F3, A1, A6, L3, S2, G1, C1 | -| NT54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | [AngleUtilities][nt54] | M21, S1 | +| NT54 | Angle utilities (wrap, shortest difference, unwrap) | `math` | [AngleUtilities][nt54] | M21, W6, W9, W10, S1 | | NT55 | Polynomial point-to-point profile (+ profile types) | `motion_profiles` | [PolynomialProfile][nt55] | M10, F4, L2 | | NT56 | Trapezoidal (LSPB) profile + duration sync | `motion_profiles` | [TrapezoidalProfile][nt56] | M10 (time law, optional), F4 | | NT57 | Friction compensation (Coulomb + viscous + Stribeck) | `controllers` | [FrictionCompensation][nt57] | M5, M12, M29 (optional feedforward) | @@ -56,7 +58,7 @@ numerical-toolbox pin that ships it. | NT63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | [IncrementalQrLeastSquares][nt63] | M22 | | NT64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | [LevenbergMarquardt][nt64] | M13, M23, E5, C2 | | NT65 | Dense convex QP (Goldfarb–Idnani) | `solvers` | [ActiveSetQp][nt65] | M25 (exact path), L6, G2, G3 | -| NT66 | SE(2) transform, twists, exp/log, pose error | `math` | [master list][ntrm], no spec yet | F1, W1, W2, W3, W5, W7, W8, E1, E2, E3, E5 | +| NT66 | SE(2) transform, twists, exp/log, pose error | `math` | [SE2Transform][nt66] (spec) | F1, W1–W10, M24, E1, E2, E3, E5 | | NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [master list][ntrm], no spec yet | E5, C1 | | NT68 | Lie-group Jacobians and ⊕/⊖ (SO(3), SE(3), SE(2)) | `math` | [master list][ntrm], no spec yet | C1 (and NT71) | | NT69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | [master list][ntrm], no spec yet | A2, S3 | @@ -66,19 +68,20 @@ numerical-toolbox pin that ships it. | NT73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` | [master list][ntrm], no spec yet | F2 | [ntrm]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/ROADMAP.md#master-list-by-priority -[nt53]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE3Transform/implementation.md -[nt54]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/AngleUtilities/implementation.md -[nt55]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/motion_profiles/PolynomialProfile/implementation.md -[nt56]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/motion_profiles/TrapezoidalProfile/implementation.md -[nt57]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/controllers/FrictionCompensation/implementation.md -[nt58]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/SymplecticIntegrators/implementation.md -[nt59]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/TridiagonalSolver/implementation.md -[nt60]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/motion_profiles/SCurveProfile/implementation.md -[nt61]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/CubicSpline/implementation.md -[nt62]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/DampedPseudoInverse/implementation.md -[nt63]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md -[nt64]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/optimization/LevenbergMarquardt/implementation.md -[nt65]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/ActiveSetQp/implementation.md +[nt53]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE3Transform.md +[nt54]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/AngleUtilities.md +[nt55]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/motion_profiles/PolynomialProfile.md +[nt56]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/motion_profiles/TrapezoidalProfile.md +[nt57]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/controllers/FrictionCompensation.md +[nt58]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/solvers/SymplecticIntegrators.md +[nt59]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/TridiagonalSolver.md +[nt60]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/motion_profiles/SCurveProfile.md +[nt61]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/CubicSpline.md +[nt62]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/solvers/DampedPseudoInverse.md +[nt63]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/estimators/IncrementalQrLeastSquares.md +[nt64]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/optimization/LevenbergMarquardt.md +[nt65]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/solvers/ActiveSetQp.md +[nt66]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE2Transform/implementation.md ## Robot manipulators & other manipulator types @@ -286,17 +289,18 @@ Cross-family building blocks that several of the families below depend on. ## Wheeled mobile robots -| # | Component | Target module | Depends on | Difficulty | -|----|------------------------------------------------------|---------------|------------|------------| -| W1 | Differential-drive kinematics | `mobile` | NT66 | ★☆☆☆☆ | -| W2 | Car-like (Ackermann / kinematic bicycle) kinematics | `mobile` | NT66 | ★☆☆☆☆ | -| W3 | Omnidirectional (mecanum / Swedish-wheel) kinematics | `mobile` | NT66 | ★☆☆☆☆ | -| W4 | Skid-steer kinematics with slip (ICR model) | `mobile` | W1 | ★★☆☆☆ | -| W5 | Pure-pursuit path tracking | `mobile` | W1, NT66 | ★★☆☆☆ | -| W6 | Stanley path tracking | `mobile` | W2 | ★★☆☆☆ | -| W7 | Kanayama Lyapunov trajectory tracking | `mobile` | W1, NT66 | ★★☆☆☆ | -| W8 | Dubins and Reeds–Shepp shortest paths | `planning` | NT66 | ★★★☆☆ | -| W9 | Dynamic window approach (local obstacle avoidance) | `planning` | W1, P1 | ★★★☆☆ | +| # | Component | Target module | Depends on | Difficulty | +|-----|------------------------------------------------------|---------------|------------|------------| +| W1 | Differential-drive kinematics | `mobile` | NT66 | ★☆☆☆☆ | +| W2 | Car-like (Ackermann / kinematic bicycle) kinematics | `mobile` | NT66 | ★☆☆☆☆ | +| W3 | Omnidirectional (mecanum / Swedish-wheel) kinematics | `mobile` | NT66 | ★☆☆☆☆ | +| W4 | Skid-steer kinematics with slip (ICR model) | `mobile` | W1 | ★★☆☆☆ | +| W5 | Pure-pursuit path tracking (+ planar polyline path) | `mobile` | W1, NT66 | ★★☆☆☆ | +| W6 | Stanley path tracking | `mobile` | W2, W5 | ★★☆☆☆ | +| W7 | Kanayama Lyapunov trajectory tracking | `mobile` | W1, NT66 | ★★☆☆☆ | +| W8 | Dubins shortest paths (+ car path type) | `planning` | NT66 | ★★☆☆☆ | +| W9 | Dynamic window approach (local obstacle avoidance) | `planning` | W1, NT66 | ★★★☆☆ | +| W10 | Reeds–Shepp shortest paths (forward and reverse) | `planning` | W8, NT54 | ★★★☆☆ | **W1. Differential-drive kinematics.** Body twist from wheel speeds and back, wheel-speed limits mapped to `(v, ω)` limits; the base model of M24. - *Algorithm / paper:* Siegwart et al. (2011), Ch. 3. @@ -311,28 +315,32 @@ Cross-family building blocks that several of the families below depend on. - *Reuses / builds on:* NT66. **W4. Skid-steer kinematics.** Instantaneous-centre-of-rotation model with identified ICR offsets for tracked and skid-steered vehicles. -- *Algorithm / paper:* J. Martínez, J. Morales, A. Mandow, A. García-Cerezo, *IJRR*, 24(10), 2005. +- *Algorithm / paper:* J. L. Martínez, A. Mandow, J. Morales, S. Pedraza, A. García-Cerezo, *IJRR*, 24(10), 2005. - *Reuses / builds on:* W1. -**W5. Pure pursuit.** Curvature command towards a look-ahead point on the path, with speed-scheduled look-ahead distance. +**W5. Pure pursuit.** Curvature command towards a look-ahead point on the path, with speed-scheduled look-ahead distance and monotone progress; defines the bounded polyline `PlanarPath` that W6 and the planners share. - *Algorithm / paper:* R. C. Coulter, "Implementation of the pure pursuit path tracking algorithm," CMU-RI-TR-92-01, 1992. - *Reuses / builds on:* W1 (or W2), NT66. **W6. Stanley controller.** Front-axle cross-track and heading-error steering law for car-like vehicles. - *Algorithm / paper:* S. Thrun et al., "Stanley: The robot that won the DARPA Grand Challenge," *J. Field Robotics*, 23(9), 2006. -- *Reuses / builds on:* W2. +- *Reuses / builds on:* W2, W5 (path). **W7. Kanayama tracking controller.** Lyapunov-stable tracking of a reference pose and twist for unicycle-type bases, error expressed in the robot frame. - *Algorithm / paper:* Y. Kanayama, Y. Kimura, F. Miyazaki, T. Noguchi, *ICRA* 1990. - *Reuses / builds on:* W1, NT66. -**W8. Dubins and Reeds–Shepp paths.** Closed-form shortest paths for a bounded-curvature car, forward only (Dubins, 6 words) or with reversing (Reeds–Shepp, 48 words). -- *Algorithm / paper:* L. E. Dubins, *Am. J. Math.*, 79(3), 1957; J. A. Reeds, L. A. Shepp, *Pacific J. Math.*, 145(2), 1990. +**W8. Dubins paths.** Closed-form shortest forward-only paths for a bounded-curvature car (6 words), with float-safe existence tests; defines the `CarPath` type W10 reuses. +- *Algorithm / paper:* L. E. Dubins, *Am. J. Math.*, 79(3), 1957; A. Shkel, V. Lumelsky, *Robotics and Autonomous Systems*, 34(4), 2001. - *Reuses / builds on:* NT66. -**W9. Dynamic window approach.** Samples the reachable `(v, ω)` window per cycle and scores heading, clearance and speed against obstacle distances. +**W9. Dynamic window approach.** Samples the reachable `(v, ω)` window per cycle and scores heading, clearance and speed; clearance is the closed-form arc distance to the points of a laser scan, so no collision library (P1) is needed. - *Algorithm / paper:* D. Fox, W. Burgard, S. Thrun, *IEEE Robotics & Automation Magazine*, 4(1), 1997. -- *Reuses / builds on:* W1, P1. +- *Reuses / builds on:* W1, NT66. + +**W10. Reeds–Shepp paths.** Closed-form shortest paths for a bounded-curvature car that may reverse (48 words from 8 base formulas and their symmetries); continuous in the goal, the metric for parking and car-like planners. +- *Algorithm / paper:* J. A. Reeds, L. A. Shepp, *Pacific J. Math.*, 145(2), 1990; corrected formulas as in OMPL. +- *Reuses / builds on:* W8 (`CarPath`), NT54. ## Aerial robots diff --git a/roadmap/DEPLOYMENT.md b/roadmap/DEPLOYMENT.md index 940ce4f..876b62c 100644 --- a/roadmap/DEPLOYMENT.md +++ b/roadmap/DEPLOYMENT.md @@ -21,7 +21,7 @@ Read the spec's three files first (`implementation.md`, `tests.md`, `explanation 4. **CMake** - Add `.hpp` to `target_sources(...)`, `.cpp` to `robotics_add_coverage_sources(...)`, `Test.cpp` to the `_test` target's `target_sources`. - - New module (`trajectory`, `controllers/manipulator`): + - New module (`trajectory`, `controllers/manipulator`, `mobile`, `planning`): create `robotics//CMakeLists.txt` via `robotics_add_header_library(...)`, add a `test/` subdir, register it in the parent `CMakeLists.txt`, and add a `doc//` folder. diff --git a/roadmap/README.md b/roadmap/README.md index 111a391..93fd4b9 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) item has a spec; the other families (F, W, A, L, S, G, P, E, C, H) are ROADMAP entries only, and their -specs are added here family by family. +(M) and wheeled-robot (W) item has a spec, as does F1; the other families (F, A, L, S, G, P, E, C, H) are +ROADMAP entries only, and their specs are added here family by family. ## Convention @@ -26,7 +26,8 @@ The canonical worked example is [controllers/manipulator/PdGravityCompensation](controllers/manipulator/PdGravityCompensation/implementation.md). Conventions shared by every spec: 6-vectors are ordered linear part first — twists `(v; ω)`, wrenches -`(f; n)` — as fixed by upstream [math/SE3Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE3Transform/implementation.md) (NT53); dynamics +`(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 +3-vectors `(vx, vy, ω)` by [math/SE2Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/math/SE2Transform/implementation.md) (NT66); dynamics and Jacobians reach controllers through interfaces implemented by [dynamics/ChainDynamicsModel](dynamics/ChainDynamicsModel/implementation.md) and [kinematics/GeometricJacobian](kinematics/GeometricJacobian/implementation.md). @@ -96,5 +97,11 @@ prerequisites. ### `trajectory` `CartesianSlerpInterpolation` (M10) · `TimeOptimalPathParameterization` (M27) · `CubicSplineTrajectory` (M33) +### `mobile` +`PlanarOdometry` (F1) · `DifferentialDrive` (W1) · `CarLikeKinematics` (W2) · `OmnidirectionalKinematics` (W3) · `SkidSteerKinematics` (W4) · `PurePursuit` (W5) · `StanleyController` (W6) · `KanayamaController` (W7) + +### `planning` +`DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) + ### `controllers/manipulator` `PdGravityCompensation` (M5) · `ComputedTorqueControl` (M12) · `ImpedanceControl` (M17) · `OperationalSpaceControl` (M18) · `HybridPositionForceControl` (M19) · `SlotineLiAdaptiveControl` (M20) · `CableTensionDistribution` (M25) diff --git a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md b/roadmap/kinematics/MobileManipulatorKinematics/explanation.md index 2687d08..9eee4ec 100644 --- a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md +++ b/roadmap/kinematics/MobileManipulatorKinematics/explanation.md @@ -26,7 +26,6 @@ resolution decides how much each contributes. - **base pose `(x, y, φ)`** and **controls `(v, ω)`** — axle-midpoint pose, drive speed and turn rate. - **arm joints `q`** and the arm's Jacobian provider. - **mount transform** — arm base in the mobile-base frame. -- **wheel base** — converts `(v, ω)` to wheel speeds. ## Reference Y. Yamamoto, X. Yun, "Coordinating Locomotion and Manipulation of a Mobile Manipulator," *IEEE Trans. @@ -34,4 +33,4 @@ Automatic Control*, 39(6), 1994. ## See also `JacobianProvider` (M8, the arm block), `math::SE3Transform` (NT53, pose composition), `RedundancyResolution` -(M14, splitting base vs arm). +(M14, splitting base vs arm), `DifferentialDrive` (W1, wheel speeds from `(v, ω)`). diff --git a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md b/roadmap/kinematics/MobileManipulatorKinematics/implementation.md index ad0bd6f..a6f2993 100644 --- a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md +++ b/roadmap/kinematics/MobileManipulatorKinematics/implementation.md @@ -5,41 +5,37 @@ ## Data structures ```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct BaseState: # planar pose of the axle midpoint in the world frame - T x, y, phi - -template +template # static_assert(std::is_floating_point_v); instantiated for float class MobileManipulatorKinematics: const JacobianProvider& arm # M8: J and ToolPose in the arm-base frame SE3Transform mount # arm base in the mobile-base frame (NT53) - T wheelBase # L, axle length +# base pose: math::SE2Transform of the axle midpoint in the world frame (NT66) ``` ## Interface ```cpp -MobileManipulatorKinematics(const JacobianProvider& arm, const SE3Transform& mount, T wheelBase) -Matrix Compute(const BaseState& base, const JointVector& q) const # hot path -SE3Transform ToolPose(const BaseState& base, const JointVector& q) const +MobileManipulatorKinematics(const JacobianProvider& arm, const SE3Transform& mount) +Matrix Compute(const SE2Transform& base, const JointVector& q) const # hot path +SE3Transform ToolPose(const SE2Transform& base, const JointVector& q) const static Matrix BaseConstraint(T phi) # (ẋ, ẏ, φ̇) = S(φ)·(v, ω) -std::array WheelSpeeds(T v, T omega) const # (left, right) rim speeds ``` Columns of `Compute` act on `u = (v, ω, q̇)`: base forward speed, base yaw rate, arm joint rates. -Rows are the tool-point twist `(v; ω)` in the world frame (NT53/M8 ordering). +Rows are the tool-point twist `(v; ω)` in the world frame (NT53/M8 ordering). Wheel speeds for the +`(v, ω)` part come from W1's `DifferentialDrive::Inverse`. ## Algorithm (pseudocode) ```cpp function ToolPose(base, q): - T_wb = { Rz(base.phi), (base.x, base.y, 0) } + T_wb = base.ToSE3() # NT66: Rz(φ), (x, y, 0) return T_wb * mount * arm.ToolPose(q) function Compute(base, q): # OPTIMIZE_FOR_SPEED - R_wa = Rz(base.phi) · mount.R # arm-base orientation in the world + R_wa = base.ToSE3().R · mount.R # arm-base orientation in the world J_arm = arm.Jacobian(q) # 6×ArmDof, arm-base frame - r = ToolPose(base, q).p − (base.x, base.y, 0) # lever arm, world frame + r = ToolPose(base, q).p − (base.translation, 0) # lever arm, world frame column 0 (v) = ( (cos φ, sin φ, 0) ; 0 ) column 1 (ω) = ( CrossProduct(ẑ, r) ; ẑ ) columns 2.. = ( R_wa · J_arm.linear ; R_wa · J_arm.angular ) # blockdiag(R_wa, R_wa)·J_arm @@ -47,9 +43,6 @@ function Compute(base, q): # OPTIMIZE_FOR_SPEED function BaseConstraint(φ): # no side-slip: [−sin φ, cos φ, 0]·(ẋ, ẏ, φ̇) = 0 return [[cos φ, 0], [sin φ, 0], [0, 1]] - -function WheelSpeeds(v, ω): - return (v − ω·L/2, v + ω·L/2) ``` ## Complexity & memory @@ -80,5 +73,6 @@ function WheelSpeeds(v, ω): - Doc: `doc/kinematics/MobileManipulatorKinematics.md` (per `doc/TEMPLATE.md`) - CMake: `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`; `TestMobileManipulatorKinematics.cpp` → the `_test` target. -- Depends on: NT53 (`math::SE3Transform`), M8 (`JacobianProvider`). +- Depends on: NT53 (`math::SE3Transform`), NT66 (`math::SE2Transform`), M8 (`JacobianProvider`); W1 supplies + the wheel speeds. - Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/kinematics/MobileManipulatorKinematics/tests.md b/roadmap/kinematics/MobileManipulatorKinematics/tests.md index 4466135..3d2bde7 100644 --- a/roadmap/kinematics/MobileManipulatorKinematics/tests.md +++ b/roadmap/kinematics/MobileManipulatorKinematics/tests.md @@ -9,8 +9,8 @@ class TestMobileManipulatorKinematics : public ::testing::Test: # 3-link arm (M30 link model): z-axis joint at (0,0,0.1); y-axis joint +(0,0,0.2); y-axis joint +(0.3,0,0) std::array, 3> links{ ... } ChainTaskJacobian arm{ links, SE3Transform{ I, (0.25, 0, 0) } } - MobileManipulatorKinematics mm{ arm, SE3Transform{ Rz(π/2), (0.1, 0, 0.3) }, 0.3 } - BaseState base{ 1, 2, π/6 }; JointVector q{ 0.4, −0.6, 0.9 } + MobileManipulatorKinematics mm{ arm, SE3Transform{ Rz(π/2), (0.1, 0, 0.3) } } + SE2Transform base{ FromPose(1, 2, π/6) }; JointVector q{ 0.4, −0.6, 0.9 } u = (v, ω, q̇) = (0.3, −0.5, 0.2, −0.1, 0.4) # each case below is a TEST_F(TestMobileManipulatorKinematics, ) ``` @@ -35,8 +35,6 @@ yaw_moves_tool_by_lever_arm: Assert: linear = ẑ × r, ‖linear‖ = ‖r_xy‖ = 0.456874; angular = ẑ base_constraint_blocks_sideslip: Assert: (−sin φ, cos φ, 0)·BaseConstraint(φ)·(v, ω) = 0 for φ ∈ {0, π/6, π} -wheel_speeds_from_unicycle_command: - Assert: WheelSpeeds(1, 2) = (0.7, 1.3) (L = 0.3) ``` ## Reference vectors diff --git a/roadmap/mobile/CarLikeKinematics/explanation.md b/roadmap/mobile/CarLikeKinematics/explanation.md new file mode 100644 index 0000000..40c7e93 --- /dev/null +++ b/roadmap/mobile/CarLikeKinematics/explanation.md @@ -0,0 +1,34 @@ +# Car-Like Kinematics — Overview + +## What it is +The kinematic model of a front-steered vehicle, from cars and delivery robots to tractors. The two +front wheels are replaced by one virtual wheel in the middle (the "bicycle"), and the model gives how +fast the vehicle turns for a speed and steering angle. It also returns the individual left and right +wheel angles real steering geometry needs. + +## Why it matters (embedded) +Path trackers for car-like vehicles (pure pursuit, Stanley) produce a steering angle every cycle, which +must respect the steering limit and the actuator's slew rate. At low speeds the bicycle model is exact +enough for control and costs a single tangent; the Ackermann angles drive steer-by-wire systems with +separate actuators per wheel. + +## How it works (intuition) +A car cannot slide sideways, so every wheel rolls perpendicular to a line through the turning centre, +which lies on the extension of the rear axle. The steering angle sets how far away that centre is: the +turn radius is the wheelbase divided by the tangent of the steering angle. On a real axle the inner wheel +must steer more than the outer one so that both point around the same centre; that is the Ackermann +condition. Steering commands are clipped twice, once to the maximum angle and once to how far the +actuator can move in one control period. + +## Key parameters +- **wheelbase `L`** and **front track width `W`**. +- **max steering angle** (sets the minimum turning radius) and **max steering rate**. + +## Reference +R. Rajamani, *Vehicle Dynamics and Control*, 2nd ed. (2012), Ch. 2; B. Paden, M. Čáp, S. Z. Yong, +D. Yershov, E. Frazzoli, "A survey of motion planning and control techniques for self-driving urban +vehicles," *IEEE T-IV*, 1(1), 2016. + +## See also +`PurePursuit` (W5), `StanleyController` (W6), `DubinsPlanner` and `ReedsSheppPlanner` (W8, W10: paths +at the minimum turning radius), `math::SE2Transform` (NT66). diff --git a/roadmap/mobile/CarLikeKinematics/implementation.md b/roadmap/mobile/CarLikeKinematics/implementation.md new file mode 100644 index 0000000..bc29e60 --- /dev/null +++ b/roadmap/mobile/CarLikeKinematics/implementation.md @@ -0,0 +1,92 @@ +# Car-Like (Kinematic Bicycle / Ackermann) Kinematics — Implementation Pseudocode + +> Roadmap ref: #W2 (★☆☆☆☆) · Target: `robotics/mobile` · Namespace `mobile` · Type: `float` (templated on `T`, instantiated for `float` only) + +Front-steered vehicles (cars, tractors, forklifts driven from the rear) reduced to the kinematic bicycle: +one virtual front wheel at steering angle `δ`, reference point at the rear-axle midpoint, no lateral +slip. Valid at low lateral acceleration (below roughly 0.4 g); beyond that tyre slip needs a dynamic +model. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct CarLikeParameters: + T wheelBase # L, rear to front axle + T trackWidth # W, between the two front wheels (Ackermann only) + T maxSteer # δ_max > 0, rad; really_assert δ_max < atan(2L/W) + T maxSteerRate # δ̇_max > 0, rad/s + +template +struct AckermannAngles: + T left + T right + +template +class CarLikeKinematics: + CarLikeParameters parameters +``` + +## Interface + +```cpp +explicit CarLikeKinematics(const CarLikeParameters& parameters) +math::Vector3 Forward(T v, T steer) const # rear-axle twist (v, 0, v·tan δ / L); hot path +math::Vector3 TwistAt(T v, T steer, T offset) const # twist of a point `offset` ahead of the rear axle +T Curvature(T steer) const # tan δ / L +T SteerForCurvature(T curvature) const # atan(L·κ), clamped to ±δ_max +T LimitSteer(T requested, T previous, T dt) const # angle limit, then rate limit +AckermannAngles Ackermann(T steer) const +T MinTurningRadius() const # L / tan δ_max +const CarLikeParameters& Parameters() const +``` + +`v` is the rear-axle speed (signed; negative reverses). + +## Algorithm (pseudocode) + +```text +function Forward(v, δ): # OPTIMIZE_FOR_SPEED + return (v, 0, v·tan δ / L) + +function TwistAt(v, δ, a): # = FromPose(−a, 0, 0).TransformTwist(Forward(v, δ)) (NT66 adjoint) + ω = v·tan δ / L + return (v, a·ω, ω) + +function SteerForCurvature(κ): return clamp(atan(L·κ), −δ_max, δ_max) + +function LimitSteer(δ_req, δ_prev, dt): + δ = clamp(δ_req, −δ_max, δ_max) + return clamp(δ, δ_prev − δ̇_max·dt, δ_prev + δ̇_max·dt) + +function Ackermann(δ): # both front-wheel normals meet the rear-axle line at R = L / tan δ + t = tan δ + return { atan(L·t / (L − (W/2)·t)), atan(L·t / (L + (W/2)·t)) } # finite at δ = 0, no division by tan δ +``` + +## Complexity & memory + +- `O(1)`: one `tan` or `atan` per call. Four scalars of state. No heap. + +## Numerical / embedded notes + +- **Reference point.** Pure pursuit (W5) uses the rear axle, the point whose velocity has no lateral + part. Stanley (W6) uses the front axle (`TwistAt(v, δ, L)`), and Rajamani's centre-of-gravity model + uses `TwistAt(v, δ, l_r)`. `TwistAt` is the NT66 adjoint, so it agrees with SE(2) composition. +- `LimitSteer` assumes `δ_prev` is already within `±δ_max`, so the result is too. The steering actuator's + rate limit is what makes Stanley and pure pursuit overshoot at speed, so trackers apply it every cycle. +- `Ackermann` needs the inner wheel below 90°, i.e. `|δ| < atan(2L/W)`; the constructor asserts it for + `δ_max`. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/mobile/CarLikeKinematics.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Forward`, + and `extern template class CarLikeKinematics;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/mobile/CarLikeKinematics.cpp` → `template class CarLikeKinematics;` +- Test: `robotics/mobile/test/TestCarLikeKinematics.cpp` +- Doc: `doc/mobile/CarLikeKinematics.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.mobile ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.mobile_test`. +- Depends on: NT66 (twist convention; the test uses its adjoint). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/mobile/CarLikeKinematics/tests.md b/roadmap/mobile/CarLikeKinematics/tests.md new file mode 100644 index 0000000..0ce8b6d --- /dev/null +++ b/roadmap/mobile/CarLikeKinematics/tests.md @@ -0,0 +1,44 @@ +# Car-Like Kinematics — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestCarLikeKinematics : public ::testing::Test: + CarLikeKinematics car{ { 2.5, 1.5, 0.6, 0.5 } } # L, W, δ_max, δ̇_max +# each case below is a TEST_F(TestCarLikeKinematics, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +forward_gives_the_rear_axle_twist: + Assert: Forward(10, 0.1) ≈ (10, 0, 0.401339) +twist_at_offset_matches_the_se2_adjoint: + Assert: TwistAt(10, 0.1, 1.2) ≈ (10, 0.481606, 0.401339) + ≈ FromPose(−1.2, 0, 0).TransformTwist(Forward(10, 0.1)) +steer_for_curvature_inverts_curvature_and_saturates: + Assert: SteerForCurvature(0.1) ≈ 0.244979; Curvature(0.244979) ≈ 0.1 + SteerForCurvature(0.5) == 0.6 # unclamped atan(1.25) = 0.896055 +limit_steer_applies_angle_then_rate: + Assert: LimitSteer(0.5, 0.1, 0.2) ≈ 0.2 # rate: 0.1 per 0.2 s + LimitSteer(0.9, 0.55, 1) ≈ 0.6 # angle limit binds + LimitSteer(−0.9, 0.55, 1) ≈ 0.05 # angle limit, then rate +ackermann_wheels_share_one_turning_centre: + Assert: Ackermann(0.3) ≈ { 0.328616, 0.275851 } + tan(left)·(R − 0.75) ≈ tan(right)·(R + 0.75) ≈ 2.5 with R = 2.5 / tan 0.3 = 8.081820 + Ackermann(−0.3) ≈ { −0.275851, −0.328616 }; Ackermann(0) == { 0, 0 } +min_turning_radius_uses_max_steer: + Assert: MinTurningRadius() ≈ 3.654240 +``` + +## Reference vectors + +- `L = 2.5`: `tan 0.1 = 0.100335` ⇒ `ω = 10·0.100335/2.5 = 0.401339`. +- Ackermann at `δ = 0.3`: inner (left) `0.328616`, outer (right) `0.275851`. + +## Edge cases + +- `v < 0` reverses the yaw-rate sign for the same steer (`Forward(−10, 0.1)` ⇒ `ω = −0.401339`). +- `SteerForCurvature(0)` = 0; `Curvature(±δ_max)` = `±1/MinTurningRadius()`. diff --git a/roadmap/mobile/DifferentialDrive/explanation.md b/roadmap/mobile/DifferentialDrive/explanation.md new file mode 100644 index 0000000..39a01c9 --- /dev/null +++ b/roadmap/mobile/DifferentialDrive/explanation.md @@ -0,0 +1,32 @@ +# Differential-Drive Kinematics — Overview + +## What it is +The kinematic model of a robot with two independently driven wheels on a common axle, such as robot +vacuums, warehouse carts or educational robots. It converts the two wheel speeds into the robot's +forward speed and turn rate, and back. + +## Why it matters (embedded) +The motor controllers speak wheel speeds, while planners and trackers speak forward speed and turn rate. +This conversion runs every control cycle in both directions: commands go down to the wheels, and +measured wheel speeds come up to odometry. Saturating wheel commands in the right way keeps the robot on +its planned curve when it asks for more than the motors can give. + +## How it works (intuition) +Both wheels rolling at the same speed drive straight; a speed difference turns the robot about a point on +the axle line, and opposite speeds spin it in place. The average of the two rim speeds is the forward +speed, and their difference divided by the track width is the turn rate. When a command would push a +wheel past its limit, both wheel speeds are scaled down by the same factor. The ratio of turn rate to +speed, and so the curve, stays the same, and the robot simply drives it more slowly. + +## Key parameters +- **wheel radius `r`** and **track width `B`** — set the conversion; calibrate them for good odometry. +- **max wheel speed** — the motor limit used for saturation. + +## Reference +R. Siegwart, I. R. Nourbakhsh, D. Scaramuzza, *Introduction to Autonomous Mobile Robots*, 2nd ed. (2011), +Ch. 3; J. Borenstein, L. Feng, "Measurement and correction of systematic odometry errors in mobile +robots," *IEEE T-RA*, 12(6), 1996. + +## See also +`PlanarOdometry` (F1), `SkidSteerKinematics` (W4, the slipping generalisation), `PurePursuit` (W5) and +`KanayamaController` (W7), `MobileManipulatorKinematics` (M24). diff --git a/roadmap/mobile/DifferentialDrive/implementation.md b/roadmap/mobile/DifferentialDrive/implementation.md new file mode 100644 index 0000000..70aba71 --- /dev/null +++ b/roadmap/mobile/DifferentialDrive/implementation.md @@ -0,0 +1,85 @@ +# Differential-Drive Kinematics — Implementation Pseudocode + +> Roadmap ref: #W1 (★☆☆☆☆) · Target: `robotics/mobile` · Namespace `mobile` · Type: `float` (templated on `T`, instantiated for `float` only) + +Two independently driven wheels on one axle (plus casters): the base of most indoor robots and the base +model of M24. The body twist is `(v, 0, ω)` at the axle midpoint; the wheels cannot slide sideways. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct WheelSpeeds: # wheel angular speeds in rad/s; positive drives the robot forward + T left + T right + +template +class DifferentialDrive: + T wheelRadius # r + T trackWidth # B, distance between the wheel contact points + T maxWheelSpeed # ω_max, rad/s +``` + +## Interface + +```cpp +DifferentialDrive(T wheelRadius, T trackWidth, T maxWheelSpeed) # really_assert all > 0 +math::Vector3 Forward(const WheelSpeeds& wheels) const # body twist (v, 0, ω); hot path +WheelSpeeds Inverse(T v, T omega) const # hot path +WheelSpeeds Limit(const WheelSpeeds& wheels) const +math::Vector3 Achievable(T v, T omega) const # Forward(Limit(Inverse(v, ω))) +T MaxLinearSpeed() const # r·ω_max +T MaxAngularSpeed() const # 2·r·ω_max / B + +template +WheelSpeeds ScaleToLimit(const WheelSpeeds& wheels, T maxWheelSpeed) # free function, shared with W4 +``` + +## Algorithm (pseudocode) + +```text +function Forward(w): # OPTIMIZE_FOR_SPEED + return ( r·(w.left + w.right)/2, 0, r·(w.right − w.left)/B ) + +function Inverse(v, ω): # OPTIMIZE_FOR_SPEED + return { (v − ω·B/2)/r, (v + ω·B/2)/r } + +function ScaleToLimit(w, ω_max): # one factor for both wheels keeps the curvature ω/v + m = max(|w.left|, |w.right|) + if m ≤ ω_max: return w + k = ω_max / m + return { w.left·k, w.right·k } + +function Limit(w): return ScaleToLimit(w, maxWheelSpeed) +``` + +## Complexity & memory + +- `O(1)`: a handful of multiplies, no trigonometry. Three scalars of state. No heap. + +## Numerical / embedded notes + +- **Limit keeps the path.** Scaling both wheels by one factor keeps `ω/v`, so a saturated command still + follows the planned curve, only more slowly. Clipping each wheel on its own changes the curvature and + makes trackers (W5, W7) overshoot. +- The feasible commands form the diamond `|v|/v_max + |ω|/ω_body,max ≤ 1`; `Achievable` projects onto it + along the ray through the origin. +- `Forward` is linear, so applied to wheel-angle increments it gives the body displacement for F1's + `Integrate(Δ, 1)`. +- Odometry accuracy is set by `r` (left and right separately, if needed) and `B`; calibrate them, for + example with UMBmark (Borenstein & Feng, 1996). +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/mobile/DifferentialDrive.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on + `Forward`/`Inverse`, and `extern template class DifferentialDrive;` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/mobile/DifferentialDrive.cpp` → `template class DifferentialDrive;` and + `template WheelSpeeds ScaleToLimit(const WheelSpeeds&, float);` +- Test: `robotics/mobile/test/TestDifferentialDrive.cpp` +- Doc: `doc/mobile/DifferentialDrive.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.mobile ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.mobile_test`. +- Depends on: NT66 (`math::Vector3` twist convention). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/mobile/DifferentialDrive/tests.md b/roadmap/mobile/DifferentialDrive/tests.md new file mode 100644 index 0000000..4958c15 --- /dev/null +++ b/roadmap/mobile/DifferentialDrive/tests.md @@ -0,0 +1,40 @@ +# Differential-Drive Kinematics — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestDifferentialDrive : public ::testing::Test: + DifferentialDrive drive{ 0.05, 0.3, 20 } # r = 5 cm, B = 30 cm, 20 rad/s +# each case below is a TEST_F(TestDifferentialDrive, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +forward_maps_wheel_speeds_to_a_unicycle_twist: + Assert: Forward({ 10, 14 }) ≈ (0.6, 0, 0.666667) +inverse_inverts_forward: + Assert: Inverse(0.6, 0.666667) ≈ { 10, 14 } +spin_in_place_counter_rotates_the_wheels: + Assert: Inverse(0, 2) ≈ { −6, 6 }; Forward({ −6, 6 }) ≈ (0, 0, 2) +limit_preserves_curvature: + Assert: Inverse(1.2, 2) ≈ { 18, 30 }; Limit({ 18, 30 }) ≈ { 12, 20 } + Achievable(1.2, 2) ≈ (0.8, 0, 1.333333) # ω/v = 1.666667 before and after +limit_leaves_feasible_speeds_unchanged: + Assert: Limit({ 10, −14 }) == { 10, −14 } +speed_envelope_corners: + Assert: MaxLinearSpeed() ≈ 1; MaxAngularSpeed() ≈ 6.666667 + Achievable(1, 0) ≈ (1, 0, 0); Achievable(0, 6.666667) ≈ (0, 0, 6.666667) +``` + +## Reference vectors + +- `r = 0.05, B = 0.3`: wheels `(10, 14)` rad/s ⇔ `(v, ω) = (0.6, 0.666667)`. +- `(1.2, 2)` needs `(18, 30)` rad/s; scaled by `20/30` it becomes `(12, 20)` ⇒ `(0.8, 1.333333)`. + +## Edge cases + +- Negative `v` (reversing) maps symmetrically; `Limit` uses `|·|`. +- `ScaleToLimit` with both wheels at zero returns zeros (no division). diff --git a/roadmap/mobile/KanayamaController/explanation.md b/roadmap/mobile/KanayamaController/explanation.md new file mode 100644 index 0000000..e0681a5 --- /dev/null +++ b/roadmap/mobile/KanayamaController/explanation.md @@ -0,0 +1,33 @@ +# Kanayama Trajectory Tracking — Overview + +## What it is +A controller that makes a two-wheeled (or tracked) robot follow a moving reference, a "ghost" robot +driving the planned trajectory with known speed and turn rate. It outputs the speed and turn rate that +bring the real robot onto the ghost and keep it there. + +## Why it matters (embedded) +Time-parameterised tracking is what coordinated fleets, docking and trajectory-optimised motion need: +the robot must be at the right place at the right time, not just on the right path. The law is a few +trigonometric operations per cycle and comes with a stability proof, so its behaviour is predictable +on any trajectory that keeps moving. + +## How it works (intuition) +The error is measured from the robot's own point of view: how far the ghost is ahead, how far to the +side, and how much its heading differs. Being behind raises the speed; the ghost being off to one side +or pointing differently adds turning, in proportion to how fast the ghost moves. The robot cannot slide +sideways, so it closes a sideways gap by steering towards it while moving. A carefully chosen energy-like +quantity can be shown never to increase, which guarantees convergence as long as the reference keeps +moving. + +## Key parameters +- **along-track gain** `kx`, **cross-track gain** `ky`, **heading gain** `kθ` (critically damped when + `kθ = 2√ky`). +- **reference speed and turn rate** — the feedforward from the trajectory. + +## Reference +Y. Kanayama, Y. Kimura, F. Miyazaki, T. Noguchi, "A stable tracking control method for an autonomous +mobile robot," *ICRA* 1990. + +## See also +`DifferentialDrive` (W1, saturation), `PurePursuit` (W5, path rather than trajectory tracking), +`math::SE2Transform` (NT66). diff --git a/roadmap/mobile/KanayamaController/implementation.md b/roadmap/mobile/KanayamaController/implementation.md new file mode 100644 index 0000000..ae32f41 --- /dev/null +++ b/roadmap/mobile/KanayamaController/implementation.md @@ -0,0 +1,82 @@ +# Kanayama Lyapunov Trajectory Tracking — Implementation Pseudocode + +> Roadmap ref: #W7 (★★☆☆☆) · Target: `robotics/mobile` · Namespace `mobile` · Type: `float` (templated on `T`, instantiated for `float` only) + +Time-parameterised trajectory tracking for unicycle-type bases (differential drive, skid steer). The +reference supplies a pose and its feedforward `(v_r, ω_r)` at every tick. The error is expressed in the +robot frame, and a Lyapunov-derived law returns `(v, ω)` that converge to the reference for any initial +error while `v_r > 0`. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct KanayamaGains: + T kx # 1/s, along-track + T ky # 1/m², cross-track + T ktheta # 1/m, heading + +template +struct UnicycleCommand: + T v + T omega + +template +class KanayamaController: + KanayamaGains gains # really_assert all > 0 +``` + +## Interface + +```cpp +explicit KanayamaController(const KanayamaGains& gains) +UnicycleCommand Compute(const math::SE2Transform& pose, const math::SE2Transform& reference, + T referenceSpeed, T referenceTurnRate) const # hot path +static math::Vector3 TrackingError(const math::SE2Transform& pose, const math::SE2Transform& reference) +static KanayamaGains CriticallyDamped(T kx, T ky) # ktheta = 2·√ky +``` + +## Algorithm (pseudocode) + +```text +function TrackingError(pose, ref): # reference seen from the robot + e = pose.Inverse() * ref + return (e.translation.x, e.translation.y, e.Heading()) # (ex, ey, eθ), eθ ∈ (−π, π] + +function Compute(pose, ref, v_r, ω_r): # OPTIMIZE_FOR_SPEED + (ex, ey, eθ) = TrackingError(pose, ref) + v = v_r·cos eθ + kx·ex + ω = ω_r + v_r·(ky·ey + kθ·sin eθ) + return { v, ω } +``` + +## Complexity & memory + +- `O(1)`: one composition, one `atan2` (heading) and one `sin`/`cos` pair. Three scalars of state. No heap. + +## Numerical / embedded notes + +- **Stability.** `V = ex² + ey² + 2(1 − cos eθ)/ky` gives `V̇ = −kx·ex² − v_r·kθ·sin²eθ/ky ≤ 0` for + `v_r > 0` and positive gains (Kanayama et al., 1990). The test checks that `V` never increases along a + simulated run. +- **Tuning.** Linearised about a straight reference, the lateral loop has natural frequency + `v_r·√ky` and damping `kθ/(2√ky)`. `CriticallyDamped` sets `kθ = 2√ky`. +- **`v_r = 0`.** The lateral error is uncontrollable in place (the nonholonomic constraint), so a + stationary reference only regulates `ex` and `eθ`. For point stabilisation use a time-varying or + discontinuous law instead. +- **Saturation.** Pass the output through W1's `Achievable`, which scales `v` and `ω` together. The + Lyapunov argument assumes the command is applied as computed, so keep the reference within about 70 % + of the actuator limits. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/mobile/KanayamaController.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Compute`, and + `extern template class KanayamaController;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/mobile/KanayamaController.cpp` → `template class KanayamaController;` +- Test: `robotics/mobile/test/TestKanayamaController.cpp` +- Doc: `doc/mobile/KanayamaController.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.mobile ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.mobile_test`. +- Depends on: NT66 (`math::SE2Transform`); W1 for saturation (caller side). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/mobile/KanayamaController/tests.md b/roadmap/mobile/KanayamaController/tests.md new file mode 100644 index 0000000..b3adbe6 --- /dev/null +++ b/roadmap/mobile/KanayamaController/tests.md @@ -0,0 +1,44 @@ +# Kanayama Controller — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestKanayamaController : public ::testing::Test: + KanayamaController controller{ { 1, 4, 4 } } # kx, ky, kθ (critically damped) + float vr{ 0.5 } +# each case below is a TEST_F(TestKanayamaController, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +zero_error_returns_the_feedforward: + Assert: Compute(Identity, Identity, 0.5, 0.1) ≈ { 0.5, 0.1 } +longitudinal_error_changes_speed: + Assert: Compute(Identity, FromPose(0.2, 0, 0), 0.5, 0) ≈ { 0.7, 0 } +lateral_error_turns_towards_the_reference: + Assert: Compute(Identity, FromPose(0, 0.1, 0), 0.5, 0) ≈ { 0.5, 0.2 } +heading_error_enters_through_sine_and_cosine: + Assert: Compute(Identity, FromPose(0, 0, 0.3), 0.5, 0) ≈ { 0.477668, 0.591040 } +error_is_expressed_in_the_robot_frame: + Assert: TrackingError(FromPose(1, 1, π/2), FromPose(1, 2, π/2)) ≈ (1, 0, 0) + Compute(FromPose(1, 1, π/2), FromPose(1, 2, π/2), 0.5, 0) ≈ { 1.5, 0 } +critically_damped_gains: + Assert: CriticallyDamped(1, 4) == { 1, 4, 4 } +closed_loop_converges_with_a_non_increasing_lyapunov_function: + Arrange: pose = FromPose(−0.3, 0.5, −0.4), reference starts at Identity moving at (0.5, 0, 0), dt = 0.01 + Act: 2000 steps: u = Compute(pose, ref, 0.5, 0); pose = pose * Exp((u.v, 0, u.ω), dt); ref = ref * Exp((0.5, 0, 0), dt) + Assert: ‖TrackingError‖ < 1e-4 at t = 20 s; V(k+1) ≤ V(k) + 1e-6 at every step # V(0) = 0.379470 +``` + +## Reference vectors + +- Gains `(1, 4, 4)`, `v_r = 0.5`: `eθ = 0.3` ⇒ `v = 0.5 cos 0.3 = 0.477668`, `ω = 0.5·4·sin 0.3 = 0.591040`. +- Closed loop from `(−0.3, 0.5, −0.4)`: error `≈ (6e-6, −1.4e-4, 2.5e-4)` at 10 s and `‖e‖ ≈ 3e-8` at 20 s (double). + +## Edge cases + +- `eθ = π` (reference behind and reversed): `v = −v_r + kx·ex`, bounded output. +- `v_r = 0`: `ey` has no effect on `ω` (documented limitation). diff --git a/roadmap/mobile/OmnidirectionalKinematics/explanation.md b/roadmap/mobile/OmnidirectionalKinematics/explanation.md new file mode 100644 index 0000000..05dec8d --- /dev/null +++ b/roadmap/mobile/OmnidirectionalKinematics/explanation.md @@ -0,0 +1,31 @@ +# Omnidirectional Kinematics — Overview + +## What it is +The kinematic model of holonomic wheeled bases — robots on mecanum or omni wheels that can drive +sideways and diagonally and rotate while translating. It converts a desired body motion into +individual wheel speeds and measured wheel speeds back into body motion. + +## Why it matters (embedded) +Omnidirectional bases are common in warehouse logistics, research platforms and robot competitions +because they manoeuvre in tight spaces without parking manoeuvres. The conversion runs every control +cycle; with four or more wheels the redundancy also lets the controller notice a slipping wheel or bad +encoder, at almost no extra cost. + +## How it works (intuition) +Each wheel can push the robot only along one direction — its drive direction tilted by the angle of the +rollers on its rim — and slides freely in the perpendicular direction. A wheel's speed is therefore the +component of the robot's motion at that wheel along its push direction, including the part caused by +rotation about the centre. Stacking these relations for all wheels gives a small matrix. Going from body +motion to wheel speeds is a matrix multiply; going back uses the best-fitting body motion when there are +more wheels than degrees of freedom, and the mismatch measures how inconsistent the wheels are. + +## Key parameters +- **wheel positions**, **drive directions** and **roller angles** (0° for omni wheels, ±45° for mecanum). +- **wheel radius** and **max wheel speed**. + +## Reference +P. F. Muir, C. P. Neuman, "Kinematic modeling of wheeled mobile robots," *J. Robotic Systems*, 4(2), 1987; +K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 13.2. + +## See also +`DifferentialDrive` (W1), `PlanarOdometry` (F1), `math::SE2Transform` (NT66). diff --git a/roadmap/mobile/OmnidirectionalKinematics/implementation.md b/roadmap/mobile/OmnidirectionalKinematics/implementation.md new file mode 100644 index 0000000..230136b --- /dev/null +++ b/roadmap/mobile/OmnidirectionalKinematics/implementation.md @@ -0,0 +1,87 @@ +# Omnidirectional (Mecanum / Swedish-Wheel) Kinematics — Implementation Pseudocode + +> Roadmap ref: #W3 (★☆☆☆☆) · Target: `robotics/mobile` · Namespace `mobile` · Type: `float` (templated on `T`, instantiated for `float` only) + +Holonomic bases built from `N ≥ 3` Swedish wheels: three-wheel omni ("kiwi") platforms, four-wheel +mecanum carts, and any other layout. Each wheel has free rollers on its rim, so it drives in one +direction and slides freely in another. The body can then follow any twist `(vx, vy, ω)`. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct SwedishWheel: + math::Vector2 position # contact point in the body frame + T driveAngle # β: direction the hub moves for a positive wheel speed + T rollerAngle # γ: 0 for omniwheels, ±π/4 for mecanum (sign = roller handedness) + T radius # r + +template # static_assert(NumWheels >= 3) +class OmnidirectionalKinematics: + math::Matrix jacobian # H: wheel speeds = H · (vx, vy, ω) + math::Matrix pseudoInverse # H⁺ = (HᵀH)⁻¹Hᵀ, fixed at construction + T maxWheelSpeed +``` + +## Interface + +```cpp +OmnidirectionalKinematics(const std::array, NumWheels>& wheels, T maxWheelSpeed) +math::Vector Inverse(const math::Vector3& twist) const # hot path +math::Vector3 Forward(const math::Vector& speeds) const # least squares; hot path +math::Vector Limit(const math::Vector& speeds) const # uniform scaling +T SlipResidual(const math::Vector& speeds) const +const math::Matrix& WheelJacobian() const +``` + +## Algorithm (pseudocode) + +```text +function Construct(wheels, ω_max): + for each wheel i: # really_assert |cos γᵢ| > 1e-3 (rollers along the drive transmit nothing) + a = βᵢ + γᵢ + H[i] = ( cos a, sin a, xᵢ·sin a − yᵢ·cos a ) / (rᵢ·cos γᵢ) + G = Hᵀ·H # 3×3, SPD for a rank-3 layout + L = CholeskyDecomposition::TryFactor(G) # really_assert has_value: the layout must span all 3 DOF + H⁺ = solve G·X = Hᵀ column by column with the factor (N triangular solve pairs, construction only) + +function Inverse(V): return H·V # OPTIMIZE_FOR_SPEED +function Forward(u): return H⁺·u # OPTIMIZE_FOR_SPEED; exact for N = 3, least squares for N > 3 + +function Limit(u): + m = maxᵢ |uᵢ| + return m ≤ ω_max ? u : u·(ω_max / m) # same body direction, lower speed + +function SlipResidual(u): return ‖u − H·Forward(u)‖ +``` + +## Complexity & memory + +- `Inverse` / `Forward`: `3N` multiply-adds each; construction `O(N)` plus one 3×3 Cholesky. +- Memory: `6N + 1` scalars (`H` and `H⁺`). No heap. + +## Numerical / embedded notes + +- **Row formula.** `uᵢ = (1/(rᵢ cos γᵢ))·[cos(βᵢ+γᵢ), sin(βᵢ+γᵢ), xᵢ sin(βᵢ+γᵢ) − yᵢ cos(βᵢ+γᵢ)]·(vx, vy, ω)`, + from Lynch & Park eq. (13.5) with the twist reordered to linear-first. For a standard mecanum cart + (wheels at `(±l_x, ±l_y)`, `β = 0`, `γ = −π/4, π/4, π/4, −π/4` for FL, FR, RL, RR, `k = l_x + l_y`) it + reduces to `r·u = (vx − vy − kω, vx + vy + kω, vx + vy − kω, vx − vy + kω)`. +- **Slip detection.** With `N > 3` the wheel speeds overdetermine the twist. A non-zero `SlipResidual` on + *measured* speeds flags a slipping or lifted wheel or a faulty encoder, which odometry (F1) should then + down-weight. +- `Limit` scales every wheel by one factor, so the body moves in the commanded direction, more slowly. +- Roller handedness is easy to get wrong in the field: confirm each `γᵢ` sign by commanding pure `vy`. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/mobile/OmnidirectionalKinematics.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on + `Inverse`/`Forward`, and `extern template class OmnidirectionalKinematics;` / `` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/mobile/OmnidirectionalKinematics.cpp` → the same instantiations. +- Test: `robotics/mobile/test/TestOmnidirectionalKinematics.cpp` +- Doc: `doc/mobile/OmnidirectionalKinematics.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.mobile ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.mobile_test`. +- Depends on: NT66 (twist convention); upstream `math::CholeskyDecomposition`. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/mobile/OmnidirectionalKinematics/tests.md b/roadmap/mobile/OmnidirectionalKinematics/tests.md new file mode 100644 index 0000000..7494fb5 --- /dev/null +++ b/roadmap/mobile/OmnidirectionalKinematics/tests.md @@ -0,0 +1,45 @@ +# Omnidirectional Kinematics — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestOmnidirectionalKinematics : public ::testing::Test: + # mecanum: r = 0.05, l_x = 0.2, l_y = 0.15, order FL, FR, RL, RR, β = 0 + OmnidirectionalKinematics mecanum{ { { (0.2, 0.15), 0, −π/4, 0.05 }, { (0.2, −0.15), 0, π/4, 0.05 }, + { (−0.2, 0.15), 0, π/4, 0.05 }, { (−0.2, −0.15), 0, −π/4, 0.05 } }, 20 } + # kiwi: omniwheels (γ = 0) at radius 0.2, angles a = 90°, 210°, 330°, β = a + 90°, r = 0.05 + OmnidirectionalKinematics kiwi{ ..., 20 } +# each case below is a TEST_F(TestOmnidirectionalKinematics, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +mecanum_jacobian_matches_the_closed_form: + Assert: mecanum.WheelJacobian() ≈ [[20, −20, −7], [20, 20, 7], [20, 20, −7], [20, −20, 7]] +inverse_then_forward_recovers_the_twist: + Assert: mecanum.Inverse((0.3, −0.2, 0.5)) ≈ (6.5, 5.5, −1.5, 13.5) + mecanum.Forward((6.5, 5.5, −1.5, 13.5)) ≈ (0.3, −0.2, 0.5); SlipResidual ≈ 0 +forward_is_least_squares_and_flags_slip: + Arrange: u = (10, 0, 0, 0) # one wheel spinning, three stalled + Assert: mecanum.Forward(u) ≈ (0.125, −0.125, −0.357143); mecanum.SlipResidual(u) ≈ 5 +kiwi_is_exactly_determined: + Assert: kiwi.WheelJacobian() ≈ [[−20, 0, 4], [10, −17.320508, 4], [10, 17.320508, 4]] + kiwi.Forward((5, 5, 5)) ≈ (0, 0, 1.25); kiwi.SlipResidual((3, −1, 7)) ≈ 0 +limit_scales_all_wheels_uniformly: + Arrange: u = mecanum.Inverse((1, 0.5, 1)) = (3, 37, 23, 17) + Assert: mecanum.Limit(u) ≈ u·20/37; mecanum.Forward(mecanum.Limit(u)) ≈ (0.540541, 0.270270, 0.540541) +``` + +## Reference vectors + +- Mecanum `H⁺ = [[0.0125]·4; [−0.0125, 0.0125, 0.0125, −0.0125]; [−0.035714, 0.035714, −0.035714, 0.035714]]`. +- `u = (10, 0, 0, 0)` ⇒ residual `(2.5, 2.5, −2.5, −2.5)`, norm 5. + +## Edge cases + +- A degenerate layout (all wheels driving parallel, e.g. every `β = 0, γ = 0`) fails the construction + assert (rank-deficient `HᵀH`). +- `Limit` of an all-zero vector returns zeros. diff --git a/roadmap/mobile/PlanarOdometry/explanation.md b/roadmap/mobile/PlanarOdometry/explanation.md new file mode 100644 index 0000000..be97779 --- /dev/null +++ b/roadmap/mobile/PlanarOdometry/explanation.md @@ -0,0 +1,30 @@ +# Planar Odometry on SE(2) — Overview + +## What it is +Dead reckoning for ground robots: it accumulates the robot's pose from its measured velocity (forward +speed, sideways speed, turn rate), sample by sample, without external references. + +## Why it matters (embedded) +Every wheeled robot runs odometry at its control rate: it is the pose estimate between GPS fixes or scan +matches, and the motion model inside the localization filters. It has to be cheap, deterministic and +free of systematic integration bias, because its errors only grow. + +## How it works (intuition) +Over one sample period the robot moves with a nearly constant velocity, which traces a circular arc (a +line if it is not turning). Simple integration (move along the current heading, then turn) cuts every +arc short and slowly bends the estimated path. Here each step jumps to the exact end of the arc with the +planar exponential map, so only sensor errors remain. The integrator also reports the motion between +any two poses in the robot's own frame, which is what the localization filters consume. + +## Key parameters +- **twist `(vx, vy, ω)`** — body velocity from a wheel model (or a displacement from encoder increments). +- **sample period `dt`**. + +## Reference +R. Siegwart, I. R. Nourbakhsh, D. Scaramuzza, *Introduction to Autonomous Mobile Robots*, 2nd ed. (2011), +Ch. 5; J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics" (2018). + +## See also +`math::SE2Transform` (NT66, the exponential, from +[numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp)); `DifferentialDrive` (W1), +`OmnidirectionalKinematics` (W3) and `SkidSteerKinematics` (W4) as twist sources. diff --git a/roadmap/mobile/PlanarOdometry/implementation.md b/roadmap/mobile/PlanarOdometry/implementation.md new file mode 100644 index 0000000..d3989dd --- /dev/null +++ b/roadmap/mobile/PlanarOdometry/implementation.md @@ -0,0 +1,68 @@ +# Planar Odometry on SE(2) — Implementation Pseudocode + +> Roadmap ref: #F1 (★★☆☆☆) · Target: `robotics/mobile` · Namespace `mobile` · Type: `float` (templated on `T`, instantiated for `float` only) + +Dead-reckoning integrator shared by every wheeled model (W1–W4) and the localization filters (E1, E4). +A body twist held over one sample period moves the robot along a circular arc. Integrating it with the +SE(2) exponential (NT66) lands on that arc exactly, so heading changes inside the step do not bias the +position, as they do with Euler or Runge-Kutta integration of `(ẋ, ẏ, θ̇)`. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +class PlanarOdometry: + math::SE2Transform pose # body frame in the odometry frame (NT66) +``` + +## Interface + +```cpp +PlanarOdometry() # starts at Identity +explicit PlanarOdometry(const math::SE2Transform& initial) +void Integrate(const math::Vector3& twist, T dt) # body twist (vx, vy, ω); hot path +void Reset(const math::SE2Transform& pose) +const math::SE2Transform& Pose() const +static math::Vector3 Increment(const math::SE2Transform& from, const math::SE2Transform& to) +``` + +## Algorithm (pseudocode) + +```text +function Integrate(ξ, dt): # OPTIMIZE_FOR_SPEED + pose = (pose * SE2Transform::Exp(ξ, dt)).Normalized() + +function Increment(from, to): # body-frame motion between two odometry readings + return (from.Inverse() * to).Log() # (Δx, Δy, Δθ) as a twist held for unit time +``` + +## Complexity & memory + +- `Integrate`: one `Exp` (one `sin`/`cos` pair), one composition and one `Hypot`. +- Memory: one `SE2Transform` (4 scalars). No heap. + +## Numerical / embedded notes + +- **Exact for a constant twist.** For a twist that changes during the period, pass the mean of the twists + at both ends of the step (second order), not the newest sample. +- **Encoder increments.** The wheel models' `Forward` maps are linear, so applying them to wheel-angle + increments gives the body displacement `Δ = ξ·dt` directly. Integrate it with `Integrate(Δ, 1)`; no + division by a jittery `dt` is needed. +- `Normalized` every step keeps `cos² + sin² = 1` for arbitrarily long runs (one `Hypot` per step). +- `Increment` is what odometry motion models consume (E1, E4). It goes through `Log`, so it needs the + heading change between the two readings to stay below `π`; sample faster than half a turn. +- Odometry error grows without bound. Wheel-radius and track-width errors dominate it, which is the + wheel models' job to calibrate (UMBmark), not this integrator's. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/mobile/PlanarOdometry.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Integrate`, and + `extern template class PlanarOdometry;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/mobile/PlanarOdometry.cpp` → `template class PlanarOdometry;` +- Test: `robotics/mobile/test/TestPlanarOdometry.cpp` +- Doc: `doc/mobile/PlanarOdometry.md` (per `doc/TEMPLATE.md`) +- CMake: new module `robotics/mobile` (`robotics.mobile`, test target `robotics.mobile_test`), created by + the first `mobile` item deployed; `.hpp` → `target_sources`; `.cpp` → `robotics_add_coverage_sources`. +- Depends on: NT66 (`math::SE2Transform`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/mobile/PlanarOdometry/tests.md b/roadmap/mobile/PlanarOdometry/tests.md new file mode 100644 index 0000000..4629f4b --- /dev/null +++ b/roadmap/mobile/PlanarOdometry/tests.md @@ -0,0 +1,47 @@ +# Planar Odometry — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestPlanarOdometry : public ::testing::Test: + PlanarOdometry odometry # starts at Identity + Vector3 arc{ 0.5, 0, 0.25 } # radius 2 m circle, period 8π s +# each case below is a TEST_F(TestPlanarOdometry, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +straight_line_accumulates_distance: + Act: 4 × Integrate((1, 0, 0), 0.25) + Assert: Pose().Pose() ≈ (1, 0, 0) +quarter_arc_is_exact_in_one_step: + Act: Integrate(arc, 2π) + Assert: Pose().Pose() ≈ (2, 2, π/2) +full_circle_closes_independent_of_step_count: + Act: 7 steps of 8π/7, and separately 1000 steps of 8π/1000 + Assert: both end at (0, 0, 0) (tol 1e-4 for 1000 steps) +increment_recovers_the_integrated_twist: + Arrange: odometry starts at FromPose(1, 2, 0.3) + Act: Integrate((0.4, 0.1, −0.6), 0.5) + Assert: Increment(FromPose(1, 2, 0.3), Pose()) ≈ (0.2, 0.05, −0.3) +reset_replaces_the_pose: + Act: Integrate(arc, 1); Reset(FromPose(−1, 3, 2)) + Assert: Pose().Pose() ≈ (−1, 3, 2) +long_runs_keep_a_unit_rotation: + Act: 10⁵ × Integrate(arc, 1e-3) + Assert: cosine² + sine² ≈ 1 (tol 1e-6) +``` + +## Reference vectors + +- Twist `(0.5, 0, 0.25)`: radius `v/ω = 2`; after `2π` s the robot is at `(2, 2, π/2)`; after `8π` s it is + back at the origin. +- `FromPose(1, 2, 0.3)` integrated with `(0.4, 0.1, −0.6)` for 0.5 s ⇒ `Increment = (0.2, 0.05, −0.3)`. + +## Edge cases + +- `dt = 0` leaves the pose unchanged. +- `ω = 0` reduces to straight-line motion along the body heading. diff --git a/roadmap/mobile/PurePursuit/explanation.md b/roadmap/mobile/PurePursuit/explanation.md new file mode 100644 index 0000000..c3a5629 --- /dev/null +++ b/roadmap/mobile/PurePursuit/explanation.md @@ -0,0 +1,35 @@ +# Pure Pursuit — Overview + +## What it is +A path-following controller for wheeled robots and cars. It picks a point a fixed distance ahead on the +path, the "carrot", and steers along the circular arc that would take the vehicle there. The output is +a curvature, which a differential-drive robot turns into a turn rate and a car into a steering angle. +The same module provides the bounded polyline path type the other trackers and planners use. + +## Why it matters (embedded) +Pure pursuit is the workhorse tracker of warehouse robots, agricultural vehicles and autonomous carts: +it needs no model beyond geometry, has one intuitive tuning knob (how far ahead to look), and its cost +per cycle is a short segment search and a few multiplications. The path itself lives in fixed-size +storage, so long missions run without allocation. + +## How it works (intuition) +Draw a circle around the robot with radius equal to the look-ahead distance; where the path leaves that +circle is the target. Exactly one arc starts at the robot, tangent to its heading, and passes through +the target, and its curvature is twice the target's sideways offset divided by the squared distance. +Looking farther ahead smooths the motion and cuts corners; looking closer tracks tightly but can +oscillate, so the distance grows with speed. The tracker remembers how far along the path it has come +and only searches forward, so a path that doubles back cannot pull it onto the wrong leg. + +## Key parameters +- **look-ahead** — minimum, maximum, and the speed gain between them. +- **search window** — how many segments ahead the closest-point search may look. +- **goal tolerance** — distance to the last waypoint that counts as arrived. + +## Reference +R. C. Coulter, "Implementation of the pure pursuit path tracking algorithm," CMU-RI-TR-92-01, 1992; +S. Macenski, S. Singh, F. Martín, J. Ginés, "Regulated pure pursuit for robot path tracking," +*Autonomous Robots*, 47, 2023. + +## See also +`DifferentialDrive` (W1), `CarLikeKinematics` (W2), `StanleyController` (W6, shares the path type), +`math::SE2Transform` (NT66). diff --git a/roadmap/mobile/PurePursuit/implementation.md b/roadmap/mobile/PurePursuit/implementation.md new file mode 100644 index 0000000..cc536a1 --- /dev/null +++ b/roadmap/mobile/PurePursuit/implementation.md @@ -0,0 +1,136 @@ +# Pure-Pursuit Path Tracking (+ Planar Polyline Path) — Implementation Pseudocode + +> Roadmap ref: #W5 (★★☆☆☆) · Target: `robotics/mobile` · Namespace `mobile` · Type: `float` (templated on `T`, instantiated for `float` only) + +Geometric path tracker: aim at the point one look-ahead distance ahead on the path and drive the +circular arc that reaches it. It outputs a curvature, which fits both a unicycle (W1: `ω = v·κ`) and a +car (W2: `δ = SteerForCurvature(κ)`). It also defines `PlanarPath`, the bounded polyline that W6 and the +planners share. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct PathProjection: + T arcLength # s of the closest point + math::Vector2 point + T heading # direction of the segment there + T crossTrack # signed distance, positive when the query point is left of the path + std::size_t segment + +template # static_assert(MaxPoints >= 2) +class PlanarPath: + std::array, MaxPoints> points + std::array arcLength # cumulative, arcLength[0] = 0 + std::size_t count{ 0 } + +template +struct PurePursuitParameters: + T minLookahead, maxLookahead # L_min ≤ L_max, metres + T lookaheadGain # seconds: L = clamp(L_min + gain·|v|, L_min, L_max) + T goalTolerance + std::size_t searchWindow # segments scanned from the current one + +template +struct PursuitCommand: + T curvature + math::Vector2 goal + bool reached + +template +class PurePursuit: + const PlanarPath& path + PurePursuitParameters parameters + T progress{ 0 } # arc length reached so far; never decreases + std::size_t segment{ 0 } +``` + +## Interface + +```cpp +# PlanarPath +bool Assign(std::span> waypoints) # false, path unchanged, if size ∉ [2, MaxPoints] + # or a segment is shorter than 1e-6 +std::size_t Size() const; std::size_t Segments() const; T Length() const +const math::Vector2& Point(std::size_t i) const; T ArcLength(std::size_t i) const +PathProjection Project(const math::Vector2& p, std::size_t firstSegment, std::size_t segmentWindow) const +math::Vector2 PointAt(T s) const # s clamped to [0, Length] +T HeadingAt(T s) const + +# PurePursuit +PurePursuit(const PlanarPath& path, const PurePursuitParameters& parameters) +PursuitCommand Compute(const math::SE2Transform& pose, T speed) # hot path +void Restart() # progress and segment back to 0 +T Progress() const +``` + +## Algorithm (pseudocode) + +```text +function PlanarPath::Project(p, first, window): # nearest point over a bounded run of segments + best = none + for k in [first, min(first + window, Segments())): + a = points[k]; d = points[k+1] − a; ℓ = |d|; u = d/ℓ + t = clamp((p − a)·u, 0, ℓ); c = a + t·u + if best is none or |p − c| < best.distance: # ties keep the earlier segment + best = { arcLength[k] + t, c, atan2(u.y, u.x), u.x·(p − a).y − u.y·(p − a).x, k } + return best + +function PlanarPath::PointAt(s): # binary search over arcLength, then interpolate + s = clamp(s, 0, Length()); k = last index with arcLength[k] ≤ s (k ≤ Segments() − 1) + return points[k] + (s − arcLength[k])·(points[k+1] − points[k]) / (arcLength[k+1] − arcLength[k]) + +function PurePursuit::Compute(pose, v): # OPTIMIZE_FOR_SPEED + L = clamp(L_min + gain·|v|, L_min, L_max) + q = pose.translation + proj = path.Project(q, segment, searchWindow) + if proj.arcLength ≥ progress: progress = proj.arcLength; segment = proj.segment + + goal = none + for k in [segment, path.Segments()): # first exit point of the circle |x − q| = L ahead + a = path.Point(k); d = path.Point(k+1) − a; f = a − q + A = d·d; B = 2·f·d; C = f·f − L²; Δ = B² − 4AC + if Δ < 0: continue + t = (−B + √Δ) / (2A) # larger root: where the path leaves the circle + if 0 ≤ t ≤ 1 and path.ArcLength(k) + t·√A ≥ progress: goal = a + t·d; break + if goal is none: goal = path.PointAt(min(progress + L, path.Length())) # off the path or near its end + + b = pose.Inverse().Apply(goal) # goal in the body frame + κ = (b·b < 1e-12) ? 0 : 2·b.y / (b·b) # arc tangent to the heading through the goal + reached = |q − path.Point(path.Size() − 1)| ≤ goalTolerance + return { κ, goal, reached } +``` + +## Complexity & memory + +- `Project`: `O(searchWindow)`; goal search `O(K)` worst case, normally one or two segments; `PointAt` + `O(log K)`. +- Memory: `PlanarPath` stores `3·MaxPoints` scalars; `PurePursuit` holds a reference and 7 scalars. No heap. + +## Numerical / embedded notes + +- **Monotone progress.** The closest point is searched only from the current segment over `searchWindow` + segments, and `progress` never decreases. A path that doubles back (U-turns, parking loops) therefore + cannot capture the robot onto its return leg; the test shows the difference. +- **Look-ahead.** A short look-ahead tracks tightly but oscillates at speed; a long one cuts corners. The + speed-scheduled `L_min + gain·|v|` is Coulter's recommendation. The regulated variant (Macenski et al., + 2023) also slows down on high curvature, which the caller can do from `PursuitCommand::curvature`. +- **Reversing.** List the waypoints in driving order and pass a negative speed. The arc formula is the + same, because the circle through the goal tangent to the heading does not depend on the direction of + travel. +- Off the path by more than `L`, the goal falls back to `PointAt(progress + L)`, which steers back + towards the path instead of stopping. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Headers: `robotics/mobile/PlanarPath.hpp` and `robotics/mobile/PurePursuit.hpp` — `#pragma once`, + `OPTIMIZE_FOR_SPEED` on `Compute`, and `extern template class PlanarPath;` / + `extern template class PurePursuit;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/mobile/PlanarPath.cpp`, `robotics/mobile/PurePursuit.cpp` → the same instantiations. +- Tests: `robotics/mobile/test/TestPlanarPath.cpp`, `robotics/mobile/test/TestPurePursuit.cpp` +- Doc: `doc/mobile/PurePursuit.md` (per `doc/TEMPLATE.md`; covers the path type) +- CMake: `.hpp` → `target_sources(robotics.mobile ...)`; `.cpp` → `robotics_add_coverage_sources`; + tests → `robotics.mobile_test`. +- Depends on: NT66 (`math::SE2Transform`); W1 or W2 to turn the curvature into a command (caller side). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/mobile/PurePursuit/tests.md b/roadmap/mobile/PurePursuit/tests.md new file mode 100644 index 0000000..cf9ffe2 --- /dev/null +++ b/roadmap/mobile/PurePursuit/tests.md @@ -0,0 +1,64 @@ +# Pure Pursuit and Planar Path — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixtures + +```cpp +class TestPlanarPath : public ::testing::Test: + PlanarPath path # Assign({ (0, 0), (2, 0), (4, 2), (4, 5) }) in the constructor +class TestPurePursuit : public ::testing::Test: + PlanarPath path # same waypoints + PlanarPath uTurn # { (0, 0), (4, 0), (4, 1), (0, 1) } + PurePursuitParameters parameters{ 1.0, 3.0, 0.5, 0.1, 2 } # L_min, L_max, gain, tolerance, window +# each case below is a TEST_F(, ) +``` + +## Test cases — `TestPlanarPath` + +```cpp +assign_computes_arc_lengths_and_rejects_bad_input: + Assert: Length() ≈ 7.828427; ArcLength(2) ≈ 4.828427 + Assign of 1 point, of 9 points, or with a repeated point ⇒ false and the path is unchanged +project_reports_the_signed_cross_track: + Assert: Project((1, 0.5), 0, 3) ≈ { s 1, point (1, 0), heading 0, cross +0.5, segment 0 } + Project((3.5, 0.5), 0, 3) ≈ { s 3.414214, point (3, 1), heading π/4, cross −0.707107, segment 1 } +point_at_clamps_to_the_path: + Assert: PointAt(5.5) ≈ (4, 2.671573); HeadingAt(5.5) ≈ π/2; PointAt(−1) == (0, 0); PointAt(99) == (4, 5) +``` + +## Test cases — `TestPurePursuit` + +```cpp +goal_on_the_current_segment: + Assert: Compute(FromPose(0.5, −0.2, 0), 1) ⇒ goal ≈ (1.986607, 0), κ ≈ 0.177778 # L = 1.5 +goal_on_the_next_segment: + Assert: Compute(FromPose(1.8, 0, 0), 1) ⇒ goal ≈ (2.955936, 0.955936), κ ≈ 0.849721 +lookahead_is_speed_scheduled_and_clamped: + Assert: |goal − p| ≈ 3 for speed 10, and ≈ 1 for speed 0, from FromPose(0.5, 0, 0) +falls_back_to_a_path_point_when_far_off_the_path: + Assert: Compute(FromPose(1, −3, 0), 0) ⇒ goal ≈ (2, 0), κ ≈ 0.6 +end_of_path_targets_the_last_point_and_reports_reached: + Assert: Compute(FromPose(4, 4.5, π/2), 1) ⇒ goal (4, 5), κ ≈ 0, reached false + Compute(FromPose(4, 4.95, π/2), 1) ⇒ reached true +search_window_keeps_progress_on_the_current_leg: + Arrange: PurePursuit on uTurn, window 2 + Assert: Compute(FromPose(1, 0.6, 0), 0) ⇒ goal ≈ (1.8, 0), κ ≈ −1.2 + # an unbounded window projects onto the return leg (distance 0.4 < 0.6): goal (0.083485, 1), κ +0.8 +closed_loop_unicycle_reaches_the_goal: + Arrange: parameters { 0.6, 2.0, 0.5, 0.1, 2 }, start FromPose(0, −0.5, 0.3), v = 0.8, dt = 0.02 + Act: pose = pose * Exp((v, 0, v·κ), dt) until reached, at most 600 steps + Assert: reached within 600 steps (double reference: 479), final position within 0.1 of (4, 5) +``` + +## Reference vectors + +- Path `(0,0) → (2,0) → (4,2) → (4,5)`: arc lengths `0, 2, 4.828427, 7.828427`. +- From `(0.5, −0.2, 0)`, `L = 1.5`: goal `x = 0.5 + √2.21 = 1.986607`, body `(1.486607, 0.2)`, + `κ = 2·0.2/1.5² = 0.177778`. +- From `(1.8, 0, 0)`: `2t² + 0.4t − 2.21 = 0` ⇒ `t = 0.955936` on segment 1. + +## Edge cases + +- Robot exactly on the last waypoint ⇒ `b = 0`, `κ = 0`, `reached`. +- `Restart()` after reaching the goal sets `Progress()` back to 0. diff --git a/roadmap/mobile/SkidSteerKinematics/explanation.md b/roadmap/mobile/SkidSteerKinematics/explanation.md new file mode 100644 index 0000000..2b67ca1 --- /dev/null +++ b/roadmap/mobile/SkidSteerKinematics/explanation.md @@ -0,0 +1,34 @@ +# Skid-Steer Kinematics — Overview + +## What it is +A kinematic model for vehicles that steer by driving their left and right sides at different speeds +while the wheels or tracks skid sideways — tracked robots, excavators, and four-wheel skid-steer +platforms. It also includes a small estimator that learns the model's slip parameters from driving data. + +## Why it matters (embedded) +Treating a skid-steer vehicle like an ideal two-wheeled robot overestimates how fast it turns, often by +a third or more, which ruins odometry and makes path trackers oscillate. The corrected model costs the +same few multiplications per cycle, and its three parameters can be identified on the vehicle itself +from a short calibration drive. + +## How it works (intuition) +When a vehicle turns, every point of it rotates about one point on the ground: the instantaneous centre +of rotation. Each track has its own such point where the track does not slide on the ground. For an +ideal vehicle these lie exactly under the tracks. For a skidding vehicle they sit farther out, and the +vehicle's own centre of rotation shifts forward or back, adding a small sideways slip. Measuring where +these points are, from turns with known motion, turns the ideal formulas into accurate ones. Each +logged sample adds to three running averages, which give the three positions directly. + +## Key parameters +- **ICR positions** — left and right tread ICR (lateral) and vehicle ICR (longitudinal), identified per + terrain. +- **sprocket radius** and **max sprocket speed**. +- **minimum excitation** — how much turning the estimator needs before it trusts its result. + +## Reference +J. L. Martínez, A. Mandow, J. Morales, S. Pedraza, A. García-Cerezo, "Approximating kinematics for +tracked mobile robots," *IJRR*, 24(10), 2005; A. Mandow, J. L. Martínez, J. Morales, J. L. Blanco, +A. García-Cerezo, J. González, "Experimental kinematics for wheeled skid-steer mobile robots," *IROS* 2007. + +## See also +`DifferentialDrive` (W1, the no-slip special case), `PlanarOdometry` (F1). diff --git a/roadmap/mobile/SkidSteerKinematics/implementation.md b/roadmap/mobile/SkidSteerKinematics/implementation.md new file mode 100644 index 0000000..ca26481 --- /dev/null +++ b/roadmap/mobile/SkidSteerKinematics/implementation.md @@ -0,0 +1,106 @@ +# Skid-Steer Kinematics with Slip (ICR Model) — Implementation Pseudocode + +> Roadmap ref: #W4 (★★☆☆☆) · Target: `robotics/mobile` · Namespace `mobile` · Type: `float` (templated on `T`, instantiated for `float` only) + +Tracked vehicles and four-wheel skid-steered robots turn by skidding, so the ideal differential-drive +model (W1) over-predicts their turn rate. The instantaneous-centre-of-rotation (ICR) model keeps the +linear form but replaces `±B/2` with the identified positions of the left and right tread ICRs, plus a +longitudinal offset of the vehicle ICR that produces the lateral slip velocity. A small least-squares +estimator identifies the three numbers from logged motion. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct IcrParameters: # body frame (FLU), metres + T yLeft # y of the left tread's ICR (ideal: B/2) + T yRight # y of the right tread's ICR (ideal: −B/2) + T xIcr # x of the vehicle's ICR (ideal: 0) + +template +class SkidSteerKinematics: + IcrParameters icr # really_assert yLeft > yRight + T sprocketRadius # r: tread speed = r · sprocket speed + T maxSprocketSpeed + +template +class IcrEstimator: # closed-form linear least squares over logged samples + T sumOmega2, sumLeft, sumRight, sumLateral + std::size_t samples +``` + +## Interface + +```cpp +static IcrParameters IdealIcr(T trackWidth) # (B/2, −B/2, 0) + +SkidSteerKinematics(const IcrParameters& icr, T sprocketRadius, T maxSprocketSpeed) +math::Vector3 Forward(const WheelSpeeds& sprockets) const # (vx, vy, ω); hot path +WheelSpeeds Inverse(T vx, T omega) const # hot path; vy follows, it is not commanded +WheelSpeeds Limit(const WheelSpeeds& sprockets) const # W1 ScaleToLimit + +explicit IcrEstimator(T sprocketRadius) +void Add(const WheelSpeeds& sprockets, const math::Vector3& measuredTwist) +std::optional> Estimate(T minExcitation) const # nullopt while Σω² < minExcitation +void Reset() +std::size_t Samples() const +``` + +`WheelSpeeds` is W1's (left, right) pair, here the sprocket speeds in rad/s. + +## Algorithm (pseudocode) + +```text +# A body point (x, y) moves at (vx − ω·y, vy + ω·x). A tread at speed v_t slides on the ground at +# vx − ω·y − v_t, which is zero at its ICR y_t = (vx − v_t)/ω. + +function Forward(s): # OPTIMIZE_FOR_SPEED + vl = r·s.left; vr = r·s.right; D = yLeft − yRight + ω = (vr − vl) / D + vx = (vr·yLeft − vl·yRight) / D + vy = −xIcr·ω + return (vx, vy, ω) + +function Inverse(vx, ω): # OPTIMIZE_FOR_SPEED + return { (vx − ω·yLeft)/r, (vx − ω·yRight)/r } + +function IcrEstimator::Add(s, (vx, vy, ω)): # each sample: yLeft·ω = vx − vl, yRight·ω = vx − vr, xIcr·ω = −vy + vl = r·s.left; vr = r·s.right + sumOmega2 += ω²; sumLeft += ω·(vx − vl); sumRight += ω·(vx − vr); sumLateral += ω·vy + samples += 1 + +function IcrEstimator::Estimate(minExcitation): + if sumOmega2 < minExcitation: return nullopt # straight driving cannot identify the ICR + return { sumLeft/sumOmega2, sumRight/sumOmega2, −sumLateral/sumOmega2 } +``` + +## Complexity & memory + +- `Forward` / `Inverse` / `Add`: `O(1)`. Kinematics: 5 scalars; estimator: 4 sums and a count. No heap. + +## Numerical / embedded notes + +- **Reduces to W1.** `IdealIcr(B)` gives `ω = r(s_r − s_l)/B`, `vx = r(s_l + s_r)/2`, `vy = 0`; the test + checks it against `DifferentialDrive`. +- **Identification data.** The measured twist comes from an external reference: GNSS heading plus speed, + a total station, motion capture, or scan matching (E5). Turning manoeuvres at both turn directions are + required; `Estimate` returns `nullopt` until `Σω²` is large enough. +- The ICR moves with terrain, load and speed (Martínez et al. found it nearly constant per terrain at + moderate speeds): identify per surface and `Reset` when the surface changes. +- `xIcr ≠ 0` makes `vy` non-zero in turns. Trackers that assume `vy = 0` (W5, W7) see it as a + disturbance, and odometry (F1) should integrate the full twist. +- Float sums: for runs of more than about 10⁶ samples, accumulate in blocks or `Reset` periodically. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/mobile/SkidSteerKinematics.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on + `Forward`/`Inverse`, and `extern template class SkidSteerKinematics;` and + `extern template class IcrEstimator;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/mobile/SkidSteerKinematics.cpp` → the same instantiations. +- Test: `robotics/mobile/test/TestSkidSteerKinematics.cpp` +- Doc: `doc/mobile/SkidSteerKinematics.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.mobile ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.mobile_test`. +- Depends on: W1 (`WheelSpeeds`, `ScaleToLimit`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/mobile/SkidSteerKinematics/tests.md b/roadmap/mobile/SkidSteerKinematics/tests.md new file mode 100644 index 0000000..7cc8f3d --- /dev/null +++ b/roadmap/mobile/SkidSteerKinematics/tests.md @@ -0,0 +1,43 @@ +# Skid-Steer Kinematics — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestSkidSteerKinematics : public ::testing::Test: + IcrParameters identified{ 0.30, −0.28, 0.05 } + SkidSteerKinematics vehicle{ identified, 0.1, 20 } # r = 0.1 + IcrEstimator estimator{ 0.1 } +# each case below is a TEST_F(TestSkidSteerKinematics, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +ideal_icr_reduces_to_differential_drive: + Arrange: SkidSteerKinematics ideal{ IdealIcr(0.4), 0.1, 20 }; DifferentialDrive drive{ 0.1, 0.4, 20 } + Assert: ideal.Forward({ 5, 8 }) ≈ drive.Forward({ 5, 8 }) ≈ (0.65, 0, 0.75) +forward_with_identified_icr_turns_slower_and_slips: + Assert: vehicle.Forward({ 5, 8 }) ≈ (0.655172, −0.025862, 0.517241) +inverse_inverts_forward: + Assert: vehicle.Inverse(0.655172, 0.517241) ≈ { 5, 8 } +estimator_recovers_the_icr_from_consistent_samples: + Arrange: for s in { (5, 8), (8, 5), (−3, 4), (6, 6), (2, −2) }: estimator.Add(s, vehicle.Forward(s)) + Assert: Samples() == 5; Estimate(0.01) ≈ { 0.30, −0.28, 0.05 } # Σω² = 2.467301 +estimator_rejects_straight_driving: + Arrange: estimator.Add({ 5, 5 }, vehicle.Forward({ 5, 5 })); estimator.Add({ 8, 8 }, vehicle.Forward({ 8, 8 })) + Assert: Estimate(0.01) == nullopt; after Reset(), Samples() == 0 +``` + +## Reference vectors + +- `(5, 8)` rad/s with `r = 0.1`: tread speeds `(0.5, 0.8)`, `D = 0.58` ⇒ `ω = 0.517241`, + `vx = (0.8·0.30 + 0.5·0.28)/0.58 = 0.655172`, `vy = −0.05·ω = −0.025862`. +- The ideal model (`B = 0.4`) would predict `ω = 0.75` for the same treads: identified skid lowers the + turn rate by 31 %. + +## Edge cases + +- Equal tread speeds ⇒ `ω = 0`, `vy = 0`, `vx = r·s` for any ICR. +- `Limit` is W1's `ScaleToLimit` (not re-tested here). diff --git a/roadmap/mobile/StanleyController/explanation.md b/roadmap/mobile/StanleyController/explanation.md new file mode 100644 index 0000000..00f978a --- /dev/null +++ b/roadmap/mobile/StanleyController/explanation.md @@ -0,0 +1,32 @@ +# Stanley Controller — Overview + +## What it is +A steering law for car-like vehicles that keeps the front axle on a path. It combines two corrections: +turn the wheels parallel to the path, and turn them further towards the path the farther the axle has +drifted from it. This is the controller that steered the vehicle that won the 2005 DARPA Grand Challenge. + +## Why it matters (embedded) +Autonomous cars, agricultural tractors and industrial tugs need lane- or row-keeping that is accurate +at speed, insensitive to speed changes and trivially cheap — one projection onto the path and one +arctangent per cycle. Stanley meets all three and is easy to tune with a single gain. + +## How it works (intuition) +The error is measured at the front axle, where the steered wheels act. The heading part rotates the +wheels to match the path direction. The cross-track part adds an angle whose tangent is the gain times +the distance from the path divided by the speed: at high speed a given offset calls for a smaller +correction, so the vehicle returns to the path at the same rate whatever its speed. A small softening +term keeps the law gentle when crawling, and every command is clipped to the steering limit and the +actuator's slew rate. + +## Key parameters +- **gain `k`** — how fast the cross-track error decays (per second). +- **softening `k_s`** — low-speed regularisation. +- **steering limits** — from the vehicle model (W2). + +## Reference +S. Thrun et al., "Stanley: The robot that won the DARPA Grand Challenge," *J. Field Robotics*, 23(9), +2006; G. M. Hoffmann, C. J. Tomlin, M. Montemerlo, S. Thrun, "Autonomous automobile trajectory tracking +for off-road driving," *American Control Conference*, 2007. + +## See also +`PurePursuit` (W5, the path type), `CarLikeKinematics` (W2), `math::SE2Transform` (NT66). diff --git a/roadmap/mobile/StanleyController/implementation.md b/roadmap/mobile/StanleyController/implementation.md new file mode 100644 index 0000000..5449edb --- /dev/null +++ b/roadmap/mobile/StanleyController/implementation.md @@ -0,0 +1,85 @@ +# Stanley Path Tracking — Implementation Pseudocode + +> Roadmap ref: #W6 (★★☆☆☆) · Target: `robotics/mobile` · Namespace `mobile` · Type: `float` (templated on `T`, instantiated for `float` only) + +The steering law of Stanford's DARPA Grand Challenge winner, for front-steered vehicles. It measures the +error at the front axle and adds the heading error to a speed-normalised arctangent of the cross-track +error. For small errors this gives exponential convergence `ė ≈ −k·e`, at a rate independent of speed. +It reuses W5's `PlanarPath` and W2's steering limits. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct StanleyParameters: + T gain # k, 1/s + T softening # k_s, m/s: keeps the law finite at low speed + std::size_t searchWindow + +template +struct StanleyError: + T crossTrack # front axle, positive when left of the path + T headingError # path heading − vehicle heading, wrapped (NT54) + +template +class StanleyController: + const PlanarPath& path # W5 + const CarLikeKinematics& vehicle # W2: wheelbase, steering limits + StanleyParameters parameters + T progress{ 0 } + std::size_t segment{ 0 } +``` + +## Interface + +```cpp +StanleyController(const PlanarPath& path, const CarLikeKinematics& vehicle, const StanleyParameters& parameters) +T Compute(const math::SE2Transform& rearAxlePose, T speed, T previousSteer, T dt) # steering angle; hot path +StanleyError Error(const math::SE2Transform& rearAxlePose) # updates progress like Compute +void Restart() +``` + +## Algorithm (pseudocode) + +```text +function Error(pose): + front = pose * SE2Transform::FromPose(L, 0, 0) + proj = path.Project(front.translation, segment, searchWindow) + if proj.arcLength ≥ progress: progress = proj.arcLength; segment = proj.segment # monotone, as in W5 + return { proj.crossTrack, math::AngleDifference(proj.heading, pose.Heading()) } + +function Compute(pose, v, δ_prev, dt): # OPTIMIZE_FOR_SPEED + e = Error(pose) + δ = e.headingError − atan2(k·e.crossTrack, k_s + v) # left of the path ⇒ steer right + return vehicle.LimitSteer(δ, δ_prev, dt) # ±δ_max, then ±δ̇_max·dt (W2) +``` + +## Complexity & memory + +- `O(searchWindow)` for the projection plus one `atan2`; state is a reference pair and 4 scalars. No heap. + +## Numerical / embedded notes + +- **Front-axle reference.** Measuring at the front axle makes the cross-track term act directly on the + steered wheels. With the rear axle the law is not stable at speed. +- **Softening `k_s`.** Without it, `atan2(k·e, v)` jumps to `±π/2` at `v → 0` for any `e`. A value near + 1 m/s keeps low-speed steering gentle. +- **Rate limit.** The steering actuator's slew rate dominates behaviour at speed; feed the applied steer + back as `previousSteer` every cycle. +- Forward driving only (`v ≥ 0`). Reversing uses the rear axle as reference with a mirrored law (or pure + pursuit, W5). +- The original paper adds yaw-rate damping and steering-servo delay terms for high speed; the kinematic + law here is the low-to-moderate-speed core. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/mobile/StanleyController.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Compute`, and + `extern template class StanleyController;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/mobile/StanleyController.cpp` → `template class StanleyController;` +- Test: `robotics/mobile/test/TestStanleyController.cpp` +- Doc: `doc/mobile/StanleyController.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.mobile ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.mobile_test`. +- Depends on: W2 (`CarLikeKinematics`), W5 (`PlanarPath`), NT66, NT54 (`AngleDifference`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/mobile/StanleyController/tests.md b/roadmap/mobile/StanleyController/tests.md new file mode 100644 index 0000000..1bbf6ed --- /dev/null +++ b/roadmap/mobile/StanleyController/tests.md @@ -0,0 +1,44 @@ +# Stanley Controller — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestStanleyController : public ::testing::Test: + PlanarPath road # { (0, 0), (50, 0) } + CarLikeKinematics car{ { 2.5, 1.5, 0.6, 10 } } # δ̇_max = 10 rad/s: rate limit inactive unless noted + StanleyController stanley{ road, car, { 2.5, 1.0, 2 } } +# each case below is a TEST_F(TestStanleyController, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +lateral_offset_steers_back_towards_the_path: + Assert: Compute(FromPose(0, 0.5, 0), 5, 0, 1) ≈ −0.205395 # −atan(1.25 / 6) +heading_error_adds_to_the_correction: + Assert: Compute(FromPose(0, 0, 0.2), 5, 0, 1) ≈ −0.404067 +error_is_measured_at_the_front_axle: + Assert: Error(FromPose(0, 0, 0.2)) ≈ { crossTrack 0.496673, headingError −0.2 } # front axle (2.450166, 0.496673) +output_saturates_at_max_steer: + Assert: Compute(FromPose(0, −3, 0), 5, 0, 1) == 0.6 +steering_rate_is_limited: + Arrange: car with δ̇_max = 0.5 + Assert: Compute(FromPose(0, 0.5, 0), 5, 0, 0.1) ≈ −0.05 +closed_loop_converges: + Arrange: rear-axle pose FromPose(0, 1, 0), v = 5, dt = 0.01, δ_prev = 0 + Act: 400 × { δ = Compute(pose, v, δ_prev, dt); pose = pose * Exp(car.Forward(v, δ), dt); δ_prev = δ } + Assert: |Error(pose).crossTrack| < 1e-3 # double reference: 3.0e-4 +``` + +## Reference vectors + +- `k = 2.5`, `k_s = 1`, `v = 5`: offset `0.5` ⇒ `δ = −atan2(1.25, 6) = −0.205395`. +- Heading `0.2`: front axle at `(2.450166, 0.496673)` ⇒ `δ = −0.2 − atan2(1.241683, 6) = −0.404067`. + +## Edge cases + +- `v = 0` stays finite thanks to `k_s`: `Compute(FromPose(0, 0.5, 0), 0, 0, 1) = −atan(1.25) = −0.896055` + before the ±0.6 clamp. +- Vehicle exactly on the path with zero heading error ⇒ steer 0. diff --git a/roadmap/planning/DubinsPath/explanation.md b/roadmap/planning/DubinsPath/explanation.md new file mode 100644 index 0000000..316d38d --- /dev/null +++ b/roadmap/planning/DubinsPath/explanation.md @@ -0,0 +1,33 @@ +# Dubins Paths — Overview + +## What it is +The shortest way for a forward-only vehicle with a limited turning radius — a car that cannot reverse, a +fixed-wing aircraft at constant altitude, a boat — to get from one position and heading to another. +The answer is always a combination of at most three pieces: turn, go straight, turn (or turn, turn the +other way, turn). + +## Why it matters (embedded) +Dubins paths are the local connector inside sampling-based planners for car-like robots and drones, the +basis of UAV loiter and waypoint transitions, and a fast admissible distance for search heuristics. The +exact answer comes from six closed formulas in constant time, so it fits in a control loop on a +microcontroller. + +## How it works (intuition) +L. E. Dubins proved that only six patterns can be optimal: left–straight–left, right–straight–right, +left–straight–right, right–straight–left, and the two three-turn patterns. Each pattern's piece lengths +follow from simple geometry: the straight piece is a tangent between the two turning circles, or a third +circle touches both. The planner computes all six, discards the impossible ones and keeps the shortest. +Floating-point care matters at the edges, where a pattern is just barely possible. + +## Key parameters +- **turning radius `ρ`** — the vehicle's minimum radius (W2's `MinTurningRadius`). +- **start and goal poses**. + +## Reference +L. E. Dubins, "On curves of minimal length with a constraint on average curvature, and with prescribed +initial and terminal positions and tangents," *American J. Mathematics*, 79(3), 1957; A. M. Shkel, +V. Lumelsky, "Classification of the Dubins set," *Robotics and Autonomous Systems*, 34(4), 2001. + +## See also +`ReedsSheppPlanner` (W10, with reversing), `CarLikeKinematics` (W2), `PurePursuit` (W5, to track the +sampled path), `math::SE2Transform` (NT66). diff --git a/roadmap/planning/DubinsPath/implementation.md b/roadmap/planning/DubinsPath/implementation.md new file mode 100644 index 0000000..a40d8d3 --- /dev/null +++ b/roadmap/planning/DubinsPath/implementation.md @@ -0,0 +1,111 @@ +# Dubins Shortest Paths (+ Car Path Type) — Implementation Pseudocode + +> Roadmap ref: #W8 (★★☆☆☆) · Target: `robotics/planning` · Namespace `planning` · Type: `float` (templated on `T`, instantiated for `float` only) + +Shortest forward-only path between two planar poses for a vehicle with a minimum turning radius `ρ`. +Dubins (1957) proved it is one of six words of at most three segments: `CSC` (LSL, RSR, LSR, RSL) or +`CCC` (RLR, LRL), with `C` an arc of radius `ρ` and `S` a straight segment. Each word has a closed form, +so the planner evaluates all six and keeps the shortest. The same header defines `CarPath`, which the +Reeds–Shepp planner (W10) reuses. + +## Data structures + +```cpp +enum class Steering : std::uint8_t { left, straight, right } + +template # static_assert(std::is_floating_point_v); instantiated for float +struct CarPathSegment: + Steering steering + T length # metres; negative = reversing (W10 only) + +template +struct CarPath: + std::array, 5> segments # Dubins uses 3, Reeds–Shepp up to 5 + std::size_t count{ 0 } + T turningRadius + +template +class DubinsPlanner: + T turningRadius # ρ > 0 (really_assert) +``` + +## Interface + +```cpp +# CarPath +T Length() const # Σ |segment length| +math::SE2Transform Sample(const math::SE2Transform& start, T s) const # pose after arc length s, clamped to [0, Length] + +# DubinsPlanner +explicit DubinsPlanner(T turningRadius) +CarPath Shortest(const math::SE2Transform& start, const math::SE2Transform& goal) const +``` + +## Algorithm (pseudocode) + +```text +function CarPath::Sample(start, s): + pose = start; remaining = clamp(s, 0, Length()) + for each segment (steering, ℓ) while remaining > 0: + take = min(remaining, |ℓ|); dir = sign(ℓ) + κ = { left: 1/ρ, straight: 0, right: −1/ρ }[steering] + pose = pose * SE2Transform::Exp(dir·(1, 0, κ), take) # NT66; reversing flips the yaw rate too + remaining −= take + return pose + +function Mod2Pi(x): y = x mod 2π in [0, 2π); return (y > 2π − ε) ? 0 : y # ε = 1e-5: no spurious full loops + +function DubinsPlanner::Shortest(start, goal): + (Δx, Δy) = goal.p − start.p; D = |Δ|; d = D/ρ + θ = (D > 0) ? atan2(Δy, Δx) : 0 + α = Mod2Pi(start.Heading() − θ); β = Mod2Pi(goal.Heading() − θ) + sa, ca, sb, cb = sin α, cos α, sin β, cos β; cab = cos(α − β) + words (t, p, q), each kept only if its existence test passes: + LSL: p² = 2 + d² − 2·cab + 2d(sa − sb); τ = atan2(cb − ca, d + sa − sb) + (Mod2Pi(τ − α), √p², Mod2Pi(β − τ)) + RSR: p² = 2 + d² − 2·cab + 2d(sb − sa); τ = atan2(ca − cb, d − sa + sb) + (Mod2Pi(α − τ), √p², Mod2Pi(τ − β)) + LSR: p² = −2 + d² + 2·cab + 2d(sa + sb); τ = atan2(−ca − cb, d + sa + sb) − atan2(−2, √p²) + (Mod2Pi(τ − α), √p², Mod2Pi(τ − β)) + RSL: p² = −2 + d² + 2·cab − 2d(sa + sb); τ = atan2(ca + cb, d − sa − sb) − atan2(2, √p²) + (Mod2Pi(α − τ), √p², Mod2Pi(β − τ)) + RLR: c = (6 − d² + 2·cab + 2d(sa − sb))/8; φ = atan2(ca − cb, d − sa + sb) + p = Mod2Pi(2π − acos c); t = Mod2Pi(α − φ + p/2); (t, p, Mod2Pi(α − β − t + p)) + LRL: c = (6 − d² + 2·cab + 2d(sb − sa))/8; φ = atan2(ca − cb, d + sa − sb) + p = Mod2Pi(2π − acos c); t = Mod2Pi(−α − φ + p/2); (t, p, Mod2Pi(β − α − t + p)) + existence: CSC needs p² ≥ −ε (then p² = max(p², 0)); CCC needs |c| ≤ 1 + ε (then clamp c to [−1, 1]) + keep the smallest t + p + q; ties keep the first word in the order above + return CarPath{ word's steering letters, lengths (t·ρ, p·ρ, q·ρ), count 3, ρ } +``` + +## Complexity & memory + +- `Shortest`: `O(1)`, six closed forms (about 20 trigonometric calls). `Sample`: up to 3 `Exp` calls. +- `CarPath`: 11 scalars plus steering tags. No heap. + +## Numerical / embedded notes + +- **Float boundary cases (normative).** Poses reachable by a single arc (for example `(ρ, ρ, π/2)`) put + the CSC discriminants at exactly 0. In `float` they come out around `−1e-7`, and a strict `p² ≥ 0` test + then drops every CSC word and returns a path five times too long. The tolerance `ε = 1e-5` (in units of + `ρ`) keeps them, and the test covers it. `Mod2Pi` snaps `2π − ε` to 0 for the same reason. +- Lengths are computed in units of `ρ` and scaled at the end, so one planner serves any radius. +- The Dubins distance is discontinuous in the goal pose (a slightly rotated goal can need a full loop); + planners that need a continuous metric should use Reeds–Shepp (W10). +- Formulas as in A. M. Shkel and V. Lumelsky (2001) and A. Walker's reference implementation; every word + in the spec's tests was checked by integrating its segments to the goal (worst error `3e-11`, double). +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Headers: `robotics/planning/CarPath.hpp`, `robotics/planning/DubinsPath.hpp` — `#pragma once`, and + `extern template struct CarPath;` / `extern template class DubinsPlanner;` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/planning/CarPath.cpp`, `robotics/planning/DubinsPath.cpp` → the same instantiations. +- Test: `robotics/planning/test/TestDubinsPath.cpp` (also covers `CarPath::Sample`) +- Doc: `doc/planning/DubinsPath.md` (per `doc/TEMPLATE.md`) +- CMake: new module `robotics/planning` (`robotics.planning`, test target `robotics.planning_test`), + created by the first `planning` item deployed; `.hpp` → `target_sources`; `.cpp` → + `robotics_add_coverage_sources`. +- Depends on: NT66 (`math::SE2Transform`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/planning/DubinsPath/tests.md b/roadmap/planning/DubinsPath/tests.md new file mode 100644 index 0000000..7cbc4c5 --- /dev/null +++ b/roadmap/planning/DubinsPath/tests.md @@ -0,0 +1,53 @@ +# Dubins Paths — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestDubinsPath : public ::testing::Test: + DubinsPlanner planner{ 1 } + SE2Transform start{ Identity } + # ExpectPath(path, letters, lengths) and ExpectReaches(path, start, goal) helpers (tol 1e-4) +# each case below is a TEST_F(TestDubinsPath, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +straight_ahead_is_a_single_segment: + Assert: Shortest(start, FromPose(4, 0, 0)) ⇒ Length ≈ 4, both arcs ≈ 0, reaches the goal +csc_shortest_is_lsl: + Assert: goal FromPose(4, 4, π/2) ⇒ L 0.785398, S 4.242641, L 0.785398; Length ≈ 5.813437 +csc_shortest_is_rsl: + Assert: goal FromPose(3, −2, π/3) ⇒ R 1.379520, S 0.896575, L 2.426718; Length ≈ 4.702813 +ccc_shortest_is_rlr: + Assert: goal FromPose(0.2, 0.6, −2.5) ⇒ R 0.752181, L 4.974858, R 0.439492; Length ≈ 6.166531 +turning_on_the_spot_needs_lrl: + Assert: goal FromPose(0, 0, π/2) ⇒ L 0.424031, R 5.560451, L 0.424031; Length ≈ 6.408513 +single_arc_goal_survives_the_float_boundary: + Assert: goal FromPose(1, 1, π/2) ⇒ Length ≈ π/2 and reaches the goal # a strict p² ≥ 0 test gives 7.853982 +mirroring_the_goal_swaps_left_and_right: + Assert: goal FromPose(3, 2, −π/3) ⇒ L 1.379520, S 0.896575, R 2.426718 +radius_scales_every_length: + Assert: DubinsPlanner{ 2 }.Shortest(start, FromPose(8, 8, π/2)).Length() ≈ 11.626874 +sample_walks_along_the_path: + Arrange: path to FromPose(4, 4, π/2) + Assert: Sample(start, 0) ≈ start; Sample(start, Length/2) ≈ (2.207107, 1.792893, π/4) + Sample(start, Length) ≈ Sample(start, Length + 1) ≈ goal +every_shortest_path_reaches_its_goal: + Arrange: DubinsPlanner{ 1.5 }, start FromPose(1, 2, π/4); goals x, y ∈ { −3, −1, 0.5, 2 }, θ ∈ { −2.5, −π/2, 0, 1, π } + Assert: Sample(start, Length) ≈ goal for all 80 goals +``` + +## Reference vectors + +- `(0,0,0) → (4,4,π/2)`, `ρ = 1`: LSL with a 45° arc, `3√2` straight, 45° arc. +- `(0,0,0) → (0,0,π/2)`: LRL `0.424031 + 5.560451 + 0.424031 = 6.408513` (RLR 9.550106, LSL 9.268195). +- All six candidate words reach their goal to `3.4e-11` over 20 000 random double-precision cases; in + `float` with `ε = 1e-5` the shortest length matches double to `1.1e-6`. + +## Edge cases + +- `start == goal` ⇒ `Length = 0` (every segment 0). +- `CarPath::Sample` with `s < 0` returns `start`. diff --git a/roadmap/planning/DynamicWindowApproach/explanation.md b/roadmap/planning/DynamicWindowApproach/explanation.md new file mode 100644 index 0000000..7174161 --- /dev/null +++ b/roadmap/planning/DynamicWindowApproach/explanation.md @@ -0,0 +1,35 @@ +# Dynamic Window Approach — Overview + +## What it is +A local obstacle-avoidance method for wheeled robots. Every control cycle it decides which forward speed +and turn rate to use next by testing many candidates against the robot's acceleration limits and the +obstacles its sensors currently see. It keeps the candidate that heads towards the goal, stays clear of +obstacles and moves briskly. + +## Why it matters (embedded) +Service robots, AGVs and robot vacuums must react to people and clutter within a fraction of a second, +on modest processors. The dynamic window reasons directly in the velocity space the motors accept, so +every command it returns is reachable within the next cycle and leaves room to brake. It runs in a fixed +time set by the number of samples and sensor points. + +## How it works (intuition) +Holding a speed and turn rate constant moves a wheeled robot along a circular arc. The method samples a +small grid of speed/turn-rate pairs reachable from the current motion in one control period. For each +it finds, exactly, how far the robot could travel along that arc before touching any obstacle point, and +discards the pairs that could not brake to a stop within that distance. The survivors are scored on how +well they point at the goal after a short look-ahead, how much free space they leave, and how fast they +go. The best score wins. If nothing survives, the robot is moving too fast for the situation and must +brake. + +## Key parameters +- **speed, turn-rate and acceleration limits**, and the **control period** (window size). +- **robot radius** (with safety margin) and **clearance cap**. +- **weights** of the heading, clearance and speed terms; **prediction time** for the heading term. +- **grid size** `NumV × NumW` (compile-time). + +## Reference +D. Fox, W. Burgard, S. Thrun, "The dynamic window approach to collision avoidance," *IEEE Robotics & +Automation Magazine*, 4(1), 1997. + +## See also +`DifferentialDrive` (W1), `PurePursuit` (W5, path for intermediate goals), `math::SE2Transform` (NT66). diff --git a/roadmap/planning/DynamicWindowApproach/implementation.md b/roadmap/planning/DynamicWindowApproach/implementation.md new file mode 100644 index 0000000..f7031cc --- /dev/null +++ b/roadmap/planning/DynamicWindowApproach/implementation.md @@ -0,0 +1,115 @@ +# Dynamic Window Approach (Local Obstacle Avoidance) — Implementation Pseudocode + +> Roadmap ref: #W9 (★★★☆☆) · Target: `robotics/planning` · Namespace `planning` · Type: `float` (templated on `T`, instantiated for `float` only) + +Reactive velocity selection for unicycle-type bases (W1, W4). Every control cycle it samples the +`(v, ω)` pairs reachable within one period under the acceleration limits (the *dynamic window*). It +drops those that could not stop before the nearest obstacle on their arc, and scores the rest on goal +heading, clearance and speed. Obstacles are a bounded set of points in the body frame, typically one +laser scan. The circular-arc clearance is computed in closed form, so no collision library is needed. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +struct DwaParameters: + T minSpeed, maxSpeed # 0 ≤ minSpeed ≤ maxSpeed (forward only) + T maxTurnRate # |ω| ≤ maxTurnRate + T maxAcceleration, maxTurnAcceleration # also the braking deceleration + T controlPeriod # window half-widths = acceleration · period + T predictionTime # horizon of the heading term + T robotRadius # circular footprint (inflate by a safety margin) + T maxClearance # clearance cap, metres + T headingWeight, clearanceWeight, speedWeight + +template +struct VelocityWindow: + T vMin, vMax, omegaMin, omegaMax + +template +struct DwaCommand: + T v, omega, clearance + +template # static_assert(NumV >= 2 && NumW >= 2) +class DynamicWindowApproach: + DwaParameters parameters +``` + +## Interface + +```cpp +explicit DynamicWindowApproach(const DwaParameters& parameters) +VelocityWindow Window(T v, T omega) const +std::optional> Compute(T v, T omega, const math::Vector2& goal, + std::span> obstacles) const # body frame; hot path +static T ArcClearance(T v, T omega, const math::Vector2& obstacle, T robotRadius, T maxClearance) +``` + +## Algorithm (pseudocode) + +```text +function Window(v, ω): + return { max(v_min, v − a·Δt), min(v_max, v + a·Δt), max(−ω_max, ω − α·Δt), min(ω_max, ω + α·Δt) } + +function ArcClearance(v, ω, p, r, cap): # arc length the robot centre travels before its disc touches p + if |p| ≤ r: return 0 # already in contact + if v ≤ 0: return cap # rotating in place: a disc sweeps no new area + κ = ω / v + if |κ| < 1e-6: # straight ahead + return (|p.y| ≤ r and p.x > 0) ? min(cap, p.x − √(r² − p.y²)) : cap + R = 1/κ; c = (0, R); d = |p − c| # the centre moves on the circle of radius |R| about c + if |d − |R|| > r: return cap # the obstacle is never within r of that circle + Δ = acos(clamp((R² + d² − r²) / (2|R|·d), −1, 1)) # half-angle of the contact interval + a0 = atan2(−R, 0); ap = atan2(p.y − R, p.x) # start and obstacle bearings about c + sweep = (κ > 0 ? ap − a0 : a0 − ap) mod 2π # obstacle angle along the direction of travel + return min(cap, |R|·((sweep − Δ) mod 2π)) + +function Compute(v0, ω0, goal, obstacles): # OPTIMIZE_FOR_SPEED + w = Window(v0, ω0) + for i in [0, NumV), j in [0, NumW): # inclusive grid over the window + v = w.vMin + i·(w.vMax − w.vMin)/(NumV − 1); ω = w.omegaMin + j·(w.omegaMax − w.omegaMin)/(NumW − 1) + dist = min(cap, min over obstacles of ArcClearance(v, ω, p, r, cap)) + admissible[i][j] = v ≤ √(2·a·dist) # can brake to a stop within the free arc + P = SE2Transform::Exp((v, 0, ω), predictionTime) # NT66 + heading[i][j] = π − |AngleDifference(atan2(goal − P.p), P.Heading())| # NT54 + clearance[i][j] = dist + if no admissible sample: return nullopt + normalise heading, clearance and speed (v) by their maxima over the admissible samples (skip a zero maximum) + G = w_h·heading_n + w_c·clearance_n + w_v·v_n; pick the admissible sample with the largest G (ties: first in grid order) + return { v, ω, clearance } +``` + +## Complexity & memory + +- `Compute`: `O(NumV·NumW·M)` clearance evaluations for `M` obstacle points (one `acos` and two `atan2` + each, at most), plus one `Exp` per sample. 11 × 21 samples × 360 beams ≈ 83 k arc tests. Budget for it, + or decimate the scan to the nearest points per sector. +- Stack: three `NumV·NumW` arrays of `T` and a flag array. No heap. + +## Numerical / embedded notes + +- **Admissibility.** Fox et al. also bound `ω` by `√(2·α·dist)`. That bound is dimensionally inconsistent + (it mixes metres and radians); for a disc robot that brakes `v` and `ω` together along the same arc, + the `v` bound is the binding one when `α ≥ |κ|·a`. Only the `v` bound is used here. +- **Standstill is always admissible** (`v = 0` has clearance `cap`), so `nullopt` only happens while + moving too fast to stop. The caller then brakes at `maxAcceleration` and calls again. +- **Normalisation.** Each term is divided by its maximum over the admissible set, as Fox et al. normalise, + so the weights stay meaningful when the window shrinks near obstacles. +- **Local minima.** DWA is reactive and can stall in U-shaped traps. Feed it intermediate goals from a + global planner (P3) or a path (W5) rather than the final goal. +- Obstacles are points in the **body frame**; transform world-frame maps with the current pose (NT66) + before the call. Forward motion only (`minSpeed ≥ 0`). +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/planning/DynamicWindowApproach.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on + `Compute`, and `extern template class DynamicWindowApproach;` under + `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/planning/DynamicWindowApproach.cpp` → `template class DynamicWindowApproach;` +- Test: `robotics/planning/test/TestDynamicWindowApproach.cpp` +- Doc: `doc/planning/DynamicWindowApproach.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.planning ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.planning_test`. +- Depends on: W1 (unicycle command model), NT66 (`Exp`), NT54 (`AngleDifference`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/planning/DynamicWindowApproach/tests.md b/roadmap/planning/DynamicWindowApproach/tests.md new file mode 100644 index 0000000..663be34 --- /dev/null +++ b/roadmap/planning/DynamicWindowApproach/tests.md @@ -0,0 +1,52 @@ +# Dynamic Window Approach — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestDynamicWindowApproach : public ::testing::Test: + DwaParameters parameters{ 0, 1.0, 2.0, 1.0, 3.0, 0.25, 1.0, 0.2, 2.0, 1.0, 1.0, 0.5 } + # v_min, v_max, ω_max, a, α, Δt, prediction, radius, cap, w_heading, w_clearance, w_speed + DynamicWindowApproach dwa{ parameters } +# each case below is a TEST_F(TestDynamicWindowApproach, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +straight_clearance_stops_at_the_disc_contact: + Assert: ArcClearance(1, 0, (2, 0.1), 0.2, 2) ≈ 1.826795 # 2 − √0.03 + ArcClearance(1, 0, (−1, 0), 0.2, 2) == 2 # behind the robot +arc_clearance_on_a_turn: + Assert: ArcClearance(1, 1, (1, 1), 0.2, 2) ≈ 1.370461 # π/2 − acos 0.98 + ArcClearance(1, −1, (1, −1), 0.2, 2) ≈ 1.370461; ArcClearance(1, 1, (3, 3), 0.2, 2) == 2 +contact_and_rotation_in_place: + Assert: ArcClearance(1, 0, (0.1, 0.05), 0.2, 2) == 0; ArcClearance(0, 1, (0.5, 0), 0.2, 2) == 2 +window_respects_acceleration_and_bounds: + Assert: Window(0.5, 0) ≈ { 0.25, 0.75, −0.75, 0.75 }; Window(0.95, 1.9) ≈ { 0.7, 1.0, 1.15, 2.0 } + Window(0.1, −1.9) ≈ { 0, 0.35, −2.0, −1.15 } +free_space_drives_fast_towards_the_goal: + Assert: Compute(0.5, 0, (5, 0), {}) ⇒ { 0.75, 0, 2 } +goal_to_the_left_turns_left: + Assert: Compute(0.5, 0, (0, 3), {}) ⇒ { 0.75, 0.75 } +obstacle_slightly_left_turns_right: + Assert: Compute(0.5, 0, (5, 0), { (0.45, 0.05) }) ⇒ { 0.375, −0.75 } + Compute(0.5, 0, (5, 0), { (0.45, −0.05) }) ⇒ { 0.375, 0.75 } # mirror +too_fast_to_stop_returns_nullopt: + Arrange: wall of 21 points x = 0.3, y ∈ [−1, 1] + Assert: Compute(0.75, 0, (5, 0), wall) == nullopt; Compute(0, 0, (5, 0), wall) ⇒ { 0, 0 } +``` + +## Reference vectors + +- Turn `v = ω = 1` (radius 1, centre `(0, 1)`), obstacle `(1, 1)`, `r = 0.2`: contact half-angle + `acos 0.98 = 0.200335` ⇒ clearance `π/2 − 0.200335 = 1.370461`. +- Obstacle `(0.45, 0.05)`: 44 of 55 samples admissible; best score `2.052105` against `2.004153` for the + runner-up `(0.25, −0.6)`. + +## Edge cases + +- Empty obstacle span ⇒ every sample has clearance `cap`. +- `NumV`/`NumW` grid endpoints coincide when the window collapses (`v` at `v_max` with `a·Δt` clipped): + duplicate samples are harmless. diff --git a/roadmap/planning/ReedsSheppPath/explanation.md b/roadmap/planning/ReedsSheppPath/explanation.md new file mode 100644 index 0000000..b35f727 --- /dev/null +++ b/roadmap/planning/ReedsSheppPath/explanation.md @@ -0,0 +1,31 @@ +# Reeds–Shepp Paths — Overview + +## What it is +The shortest way for a car that can drive both forwards and backwards, with a limited turning radius, +to get from one position and heading to another. The answer is a short sequence of turns and straight +pieces with up to two changes of driving direction — exactly how a driver parks. + +## Why it matters (embedded) +Automated parking, warehouse tuggers, forklifts and agricultural vehicles constantly reverse. Reeds–Shepp +paths connect poses in sampling-based and lattice planners for such vehicles, and their length is a +good, smooth distance for search heuristics. Everything is closed-form, so the cost per query is fixed +and small. + +## How it works (intuition) +Reeds and Shepp showed that only 48 patterns of arcs and straights can be optimal. Their lengths come from +eight small geometric formulas; the rest follow by symmetry, because driving a path backwards, mirroring +it left-to-right or traversing it in reverse order maps one valid pattern onto another. The planner +moves the goal into the start's frame, evaluates every formula with its symmetric variants, and keeps +the shortest. Negative piece lengths mean the car reverses along that piece. + +## Key parameters +- **turning radius `ρ`** — the vehicle's minimum radius (W2's `MinTurningRadius`). +- **start and goal poses**. + +## Reference +J. A. Reeds, L. A. Shepp, "Optimal paths for a car that goes both forwards and backwards," *Pacific J. +Mathematics*, 145(2), 1990; P. Souères, J.-P. Laumond, "Shortest paths synthesis for a car-like robot," +*IEEE T-AC*, 41(5), 1996. + +## See also +`DubinsPlanner` and `CarPath` (W8), `CarLikeKinematics` (W2), `math::SE2Transform` (NT66). diff --git a/roadmap/planning/ReedsSheppPath/implementation.md b/roadmap/planning/ReedsSheppPath/implementation.md new file mode 100644 index 0000000..9b726c4 --- /dev/null +++ b/roadmap/planning/ReedsSheppPath/implementation.md @@ -0,0 +1,111 @@ +# Reeds–Shepp Shortest Paths — Implementation Pseudocode + +> Roadmap ref: #W10 (★★★☆☆) · Target: `robotics/planning` · Namespace `planning` · Type: `float` (templated on `T`, instantiated for `float` only) + +Shortest path between two planar poses for a car with minimum turning radius `ρ` that may also reverse. +Reeds and Shepp (1990) showed the optimum is one of 48 words of at most five arcs and straights with at +most two direction changes (cusps). They are generated from 8 closed-form base formulas and three +symmetries, 44 evaluations in all, as in OMPL's `ReedsSheppStateSpace`. Unlike Dubins (W8), the +resulting distance is continuous in the goal pose, which makes it the metric of choice for parking and +car-like sampling planners. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); instantiated for float +class ReedsSheppPlanner: + T turningRadius # ρ > 0 (really_assert) +# result type: W8's CarPath (signed segment lengths, up to 5 segments) +``` + +## Interface + +```cpp +explicit ReedsSheppPlanner(T turningRadius) +CarPath Shortest(const math::SE2Transform& start, const math::SE2Transform& goal) const +``` + +## Algorithm (pseudocode) + +```text +# Normalised problem: start at the origin heading +x, ρ = 1. W(x) = WrapToPi(x) (NT54). ζ = 1e-5. +function Shortest(start, goal): + rel = start.Inverse() * goal + (x, y) = rel.translation / ρ; φ = rel.Heading() + best = none + evaluate every family below; keep the candidate with the smallest Σ|ℓᵢ| (ties keep the first found) + return CarPath{ best letters, best lengths · ρ, count, ρ } + +# Symmetries of a base formula f with letters P and length builder B(t, u, v): +# direct f(x, y, φ) → P, B(t, u, v) +# time-flip f(−x, y, −φ) → P, −B(t, u, v) (drive it backwards) +# reflect f(x, −y, −φ) → mirror(P), B(t, u, v) (swap L and R) +# time-flip + reflect f(−x, −y, φ) → mirror(P), −B(t, u, v) +# backwards (CCC, CCSC only): the same four on (xb, yb) = (x cos φ + y sin φ, x sin φ − y cos φ), +# with the letters and the lengths in reverse order. + +polar(a, b) = (Hypot(a, b), atan2(b, a)) + +function τω(u, v, ξ, η, φ): + δ = W(u − v); A = sin u − sin δ; B = cos u − cos δ − 1 + t1 = atan2(η·A − ξ·B, ξ·A + η·B); t2 = 2(cos δ − cos v − cos u) + 3 + τ = (t2 < 0) ? W(t1 + π) : W(t1) + return (τ, W(τ − u + v − φ)) + +# Base formulas (Reeds–Shepp eq. numbers); each returns (t, u, v) or fails. +LpSpLp (8.1): (u, t) = polar(x − sin φ, y − 1 + cos φ); t ≥ −ζ; v = W(φ − t); v ≥ −ζ +LpSpRp (8.2): (u1, t1) = polar(x + sin φ, y − 1 − cos φ); u1² ≥ 4 + u = √(u1² − 4); t = W(t1 + atan2(2, u)); v = W(t − φ); t ≥ −ζ, v ≥ −ζ +LpRmL (8.3): ξ = x − sin φ; η = y − 1 + cos φ; (u1, θ) = polar(ξ, η); u1 ≤ 4 + u = −2·asin(u1/4); t = W(θ + u/2 + π); v = W(φ − t + u); t ≥ −ζ, u ≤ ζ +LpRupLumRm (8.7): ξ = x + sin φ; η = y − 1 − cos φ; r = (2 + Hypot(ξ, η))/4; r ≤ 1 + u = acos r; (t, v) = τω(u, −u, ξ, η, φ); t ≥ −ζ, v ≤ ζ +LpRumLumRp (8.8): ξ = x + sin φ; η = y − 1 − cos φ; r = (20 − ξ² − η²)/16; 0 ≤ r ≤ 1 + u = −acos r; u ≥ −π/2; (t, v) = τω(u, u, ξ, η, φ); t ≥ −ζ, v ≥ −ζ +LpRmSmLm (8.9): ξ = x − sin φ; η = y − 1 + cos φ; (r, θ) = polar(ξ, η); r ≥ 2 + s = √(r² − 4); u = 2 − s; t = W(θ + atan2(s, −2)); v = W(φ − π/2 − t); t ≥ −ζ, u ≤ ζ, v ≤ ζ +LpRmSmRm (8.10): ξ = x + sin φ; η = y − 1 − cos φ; (r, θ) = polar(−η, ξ); r ≥ 2 + t = θ; u = 2 − r; v = W(t + π/2 − φ); t ≥ −ζ, u ≤ ζ, v ≤ ζ +LpRmSLmRp (8.11): ξ = x + sin φ; η = y − 1 − cos φ; (r, θ) = polar(ξ, η); r ≥ 2 + u = 4 − √(r² − 4); u ≤ ζ; t = W(atan2((4 − u)ξ − 2η, −2ξ + (u − 4)η)); v = W(t − φ); t ≥ −ζ, v ≥ −ζ + +# Families (letters, length builder); "+ backwards" adds the backwards variants +CSC: LpSpLp → LSL (t, u, v); LpSpRp → LSR (t, u, v) +CCC: LpRmL → LRL (t, u, v) + backwards → LRL (v, u, t) +CCCC: LpRupLumRm → LRLR (t, u, −u, v); LpRumLumRp → LRLR (t, u, u, v) +CCSC: LpRmSmLm → LRSL (t, −π/2, u, v) + backwards → LSRL (v, u, −π/2, t) + LpRmSmRm → LRSR (t, −π/2, u, v) + backwards → RSRL (v, u, −π/2, t) +CCSCC: LpRmSLmRp → LRSLR (t, −π/2, u, −π/2, v) +``` + +## Complexity & memory + +- `Shortest`: `O(1)`, 44 closed-form evaluations (about 150 trigonometric calls). Use it as a heuristic or + local connector, not inside an inner loop that runs millions of times per cycle. +- Stack only: one `CarPath` candidate and the running best. No heap. + +## Numerical / embedded notes + +- **Corrected formulas.** Equations 8.3/8.4 and 8.11 contain typos in the original paper. The forms + above are OMPL's corrected ones, and each was verified here by integrating every candidate to its goal: + worst endpoint error `4.8e-11` over 20 000 random cases, double precision. +- **Float.** The shortest length matches double to `3.3e-7` in `float` on boundary and random goals, + whether `ζ` is 1e-6 or 1e-4. `ζ = 1e-5` is used. +- **Sanity relations (tested).** `RS ≤ Dubins` (W8) for every pair, and `RS(a → b) = RS(b → a)`. The + distance is continuous: perturbing the goal by 1e-4 changes the length by at most 4.4e-4 in the + validation sweep. +- Signed lengths: a negative `ℓ` means reversing. `CarPath::Sample` (W8) applies `dir·(1, 0, κ)`, so a + reversing left arc turns the heading clockwise, as a real car does. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `robotics/planning/ReedsSheppPath.hpp` — `#pragma once`, and + `extern template class ReedsSheppPlanner;` under `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `robotics/planning/ReedsSheppPath.cpp` → `template class ReedsSheppPlanner;` +- Test: `robotics/planning/test/TestReedsSheppPath.cpp` +- Doc: `doc/planning/ReedsSheppPath.md` (per `doc/TEMPLATE.md`) +- CMake: `.hpp` → `target_sources(robotics.planning ...)`; `.cpp` → `robotics_add_coverage_sources`; + test → `robotics.planning_test`. +- Depends on: W8 (`CarPath`, and `DubinsPlanner` in the test), NT66, NT54 (`WrapToPi`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/planning/ReedsSheppPath/tests.md b/roadmap/planning/ReedsSheppPath/tests.md new file mode 100644 index 0000000..4ea55c3 --- /dev/null +++ b/roadmap/planning/ReedsSheppPath/tests.md @@ -0,0 +1,47 @@ +# Reeds–Shepp Paths — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestReedsSheppPath : public ::testing::Test: + ReedsSheppPlanner planner{ 1 } + SE2Transform start{ Identity } + # ExpectReaches(path, start, goal) helper: path.Sample(start, path.Length()) ≈ goal (tol 1e-4) +# each case below is a TEST_F(TestReedsSheppPath, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +straight_reverse_is_a_single_backward_segment: + Assert: goal FromPose(−3, 0, 0) ⇒ Length ≈ 3, one straight segment of −3, the rest ≈ 0; reaches the goal +forward_path_when_it_is_shortest: + Assert: goal FromPose(3, 0.5, 0.2) ⇒ L 0.169722, S 2.842168, L 0.030278 (all forward); Length ≈ 3.042168 +sideways_shift_uses_cusps: + Assert: goal FromPose(0, 1, 0) ⇒ Length ≈ 2.636232, contains forward and reversing segments, reaches the goal +ccsc_word_with_a_reversed_quarter_turn: + Assert: goal FromPose(−2, 1, π/2) ⇒ L −0.729728, S −0.236068, R −π/2, L 0.729728; Length ≈ 3.266320 +ccc_word_with_a_cusp: + Assert: goal FromPose(−1, −1, 0.5) ⇒ L 0.166243, R −0.965586, L −0.631829; Length ≈ 1.763658 +three_point_turn_reverses_heading_in_place: + Assert: goal FromPose(0, 0, π) ⇒ Length ≈ π, reaches the goal +never_longer_than_dubins_symmetric_and_reaching: + Arrange: ReedsSheppPlanner{ 1.5 } and DubinsPlanner{ 1.5 }, start FromPose(1, 2, π/4); + goals x, y ∈ { −3, −1, 0.5, 2 }, θ ∈ { −2.5, −π/2, 0, 1, π } + Assert: for all 80 goals: RS Length ≤ Dubins Length + 1e-5; Shortest(goal, start).Length ≈ Shortest(start, goal).Length; + Sample(start, Length) ≈ goal +``` + +## Reference vectors + +- `(0,0,0) → (0,1,0)`: Dubins needs `7.283185`, Reeds–Shepp `2.636232` (RLRL with two cusps). +- `(0,0,0) → (−2,1,π/2)`: LSRL `3.266320`, just ahead of the CCC alternative at `3.266920`. +- `(0,0,0) → (0,0,π)`: `3.141593` (three π/3 arcs: forward, reverse, forward). + +## Edge cases + +- `start == goal` ⇒ `Length = 0`. +- Ties between symmetric words (e.g. the sideways shift) may pick either word; tests assert only the + length and the endpoint there.