From 728c6484498aaf497a4c8f3758ada3aa89d9999f Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 24 Oct 2026 09:03:52 +0000 Subject: [PATCH] =?UTF-8?q?docs(roadmap):=20add=20the=20wheeled-robot=20sp?= =?UTF-8?q?ecs=20(F1,=20W1=E2=80=93W10)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Spec triplets for planar odometry (F1) and the wheeled family: differential-drive, car-like, omnidirectional and skid-steer kinematics (W1–W4), pure pursuit with the shared PlanarPath, Stanley and Kanayama trackers (W5–W7), and Dubins paths, the dynamic window approach and Reeds–Shepp paths (W8–W10). They live in the new `mobile` and `planning` modules and build on upstream SE2Transform (NT66, spec in numerical-toolbox-cpp#363). Roadmap changes: - Reeds–Shepp moves out of W8 into a new W10. - W9 computes arc clearance to scan points in closed form, so it no longer depends on P1. - W6 reuses W5's path. - M24 now takes its base pose from NT66 and its wheel speeds from W1. - NT53–NT65 have shipped upstream, so their links now point to the upstream docs instead of the deleted spec folders. The table notes the pin those items need (1f6d717 or later). Every reference value in the test plans was checked in double precision and, where float rounding matters, in float32: closed-loop runs for W5–W7, endpoint integration of every Dubins and Reeds–Shepp candidate, and DWA scoring margins. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Mmp77ieEy16GxJL8KsLsVx --- ROADMAP.md | 82 ++++++----- roadmap/DEPLOYMENT.md | 2 +- roadmap/README.md | 13 +- .../explanation.md | 3 +- .../implementation.md | 30 ++-- .../MobileManipulatorKinematics/tests.md | 6 +- .../mobile/CarLikeKinematics/explanation.md | 34 +++++ .../CarLikeKinematics/implementation.md | 92 ++++++++++++ roadmap/mobile/CarLikeKinematics/tests.md | 44 ++++++ .../mobile/DifferentialDrive/explanation.md | 32 +++++ .../DifferentialDrive/implementation.md | 85 +++++++++++ roadmap/mobile/DifferentialDrive/tests.md | 40 ++++++ .../mobile/KanayamaController/explanation.md | 33 +++++ .../KanayamaController/implementation.md | 82 +++++++++++ roadmap/mobile/KanayamaController/tests.md | 44 ++++++ .../OmnidirectionalKinematics/explanation.md | 31 ++++ .../implementation.md | 87 +++++++++++ .../mobile/OmnidirectionalKinematics/tests.md | 45 ++++++ roadmap/mobile/PlanarOdometry/explanation.md | 30 ++++ .../mobile/PlanarOdometry/implementation.md | 68 +++++++++ roadmap/mobile/PlanarOdometry/tests.md | 47 ++++++ roadmap/mobile/PurePursuit/explanation.md | 35 +++++ roadmap/mobile/PurePursuit/implementation.md | 136 ++++++++++++++++++ roadmap/mobile/PurePursuit/tests.md | 64 +++++++++ .../mobile/SkidSteerKinematics/explanation.md | 34 +++++ .../SkidSteerKinematics/implementation.md | 106 ++++++++++++++ roadmap/mobile/SkidSteerKinematics/tests.md | 43 ++++++ .../mobile/StanleyController/explanation.md | 32 +++++ .../StanleyController/implementation.md | 85 +++++++++++ roadmap/mobile/StanleyController/tests.md | 44 ++++++ roadmap/planning/DubinsPath/explanation.md | 33 +++++ roadmap/planning/DubinsPath/implementation.md | 111 ++++++++++++++ roadmap/planning/DubinsPath/tests.md | 53 +++++++ .../DynamicWindowApproach/explanation.md | 35 +++++ .../DynamicWindowApproach/implementation.md | 115 +++++++++++++++ .../planning/DynamicWindowApproach/tests.md | 52 +++++++ .../planning/ReedsSheppPath/explanation.md | 31 ++++ .../planning/ReedsSheppPath/implementation.md | 111 ++++++++++++++ roadmap/planning/ReedsSheppPath/tests.md | 47 ++++++ 39 files changed, 2032 insertions(+), 65 deletions(-) create mode 100644 roadmap/mobile/CarLikeKinematics/explanation.md create mode 100644 roadmap/mobile/CarLikeKinematics/implementation.md create mode 100644 roadmap/mobile/CarLikeKinematics/tests.md create mode 100644 roadmap/mobile/DifferentialDrive/explanation.md create mode 100644 roadmap/mobile/DifferentialDrive/implementation.md create mode 100644 roadmap/mobile/DifferentialDrive/tests.md create mode 100644 roadmap/mobile/KanayamaController/explanation.md create mode 100644 roadmap/mobile/KanayamaController/implementation.md create mode 100644 roadmap/mobile/KanayamaController/tests.md create mode 100644 roadmap/mobile/OmnidirectionalKinematics/explanation.md create mode 100644 roadmap/mobile/OmnidirectionalKinematics/implementation.md create mode 100644 roadmap/mobile/OmnidirectionalKinematics/tests.md create mode 100644 roadmap/mobile/PlanarOdometry/explanation.md create mode 100644 roadmap/mobile/PlanarOdometry/implementation.md create mode 100644 roadmap/mobile/PlanarOdometry/tests.md create mode 100644 roadmap/mobile/PurePursuit/explanation.md create mode 100644 roadmap/mobile/PurePursuit/implementation.md create mode 100644 roadmap/mobile/PurePursuit/tests.md create mode 100644 roadmap/mobile/SkidSteerKinematics/explanation.md create mode 100644 roadmap/mobile/SkidSteerKinematics/implementation.md create mode 100644 roadmap/mobile/SkidSteerKinematics/tests.md create mode 100644 roadmap/mobile/StanleyController/explanation.md create mode 100644 roadmap/mobile/StanleyController/implementation.md create mode 100644 roadmap/mobile/StanleyController/tests.md create mode 100644 roadmap/planning/DubinsPath/explanation.md create mode 100644 roadmap/planning/DubinsPath/implementation.md create mode 100644 roadmap/planning/DubinsPath/tests.md create mode 100644 roadmap/planning/DynamicWindowApproach/explanation.md create mode 100644 roadmap/planning/DynamicWindowApproach/implementation.md create mode 100644 roadmap/planning/DynamicWindowApproach/tests.md create mode 100644 roadmap/planning/ReedsSheppPath/explanation.md create mode 100644 roadmap/planning/ReedsSheppPath/implementation.md create mode 100644 roadmap/planning/ReedsSheppPath/tests.md 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.