diff --git a/CMakeLists.txt b/CMakeLists.txt index 4edcc76..28fda9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,7 +47,7 @@ if (NOT TARGET numerical.math) FetchContent_Declare( numerical_toolbox GIT_REPOSITORY https://github.com/embedded-pro/numerical-toolbox-cpp.git - GIT_TAG 63c9b91f4f6f1b306bc15fbf59d01a106da02697 # main + GIT_TAG dc147fb8672e983e5935290b94cc45ff00cbee29 # main ) set(NUMERICAL_TOOLBOX_ENABLE_OPTIMIZATIONS ${ROBOTICS_TOOLBOX_ENABLE_OPTIMIZATIONS} CACHE BOOL "" FORCE) diff --git a/README.md b/README.md index c51099e..6a59ef7 100644 --- a/README.md +++ b/README.md @@ -3,10 +3,10 @@ ## Overview This library provides robotics algorithms — today robot-manipulator kinematics, rigid-body dynamics, -trajectory generation and manipulator control; mobile, aerial, legged and marine robots are on the -[roadmap](ROADMAP.md) — designed for robust and efficient real-time use on resource-constrained -embedded systems (no heap, deterministic, float-only). It builds on the shared -numerical primitives (linear algebra, solvers, controllers) provided by +trajectory generation, manipulator control, motion planning, wheeled mobile robots and occupancy-grid +mapping; aerial, legged and marine robots are on the [roadmap](ROADMAP.md) — designed for robust and +efficient real-time use on resource-constrained embedded systems (no heap, deterministic, float-only). +It builds on the shared numerical primitives (linear algebra, solvers, controllers) provided by [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp), which it consumes via CMake `FetchContent`. @@ -35,13 +35,15 @@ project already defines those targets. Includes are namespaced by domain, e.g. ## Documentation -| Category | Description | -|--------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (Damped Least Squares), Pose Inverse Kinematics, Analytical Inverse Kinematics (OPW), Continuum Kinematics, Parallel Manipulator Kinematics | -| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Dynamic Parameter Identification, Forward-Dynamics Integration, Generic Joint Link, Generalized-Momentum Observer | -| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking, Convex Collision (GJK and EPA), Grid Search (A* and Dijkstra), RRT-Connect | -| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | -| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control, Impedance Control, Operational-Space Control, Hybrid Position/Force Control, Slotine–Li Adaptive Control, Cable Tension Distribution | +| Category | Description | +|--------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Kinematics](doc/kinematics/README.md) | Forward Kinematics, Chain Pose Kinematics, Denavit-Hartenberg Parameters, Geometric Jacobian, Manipulability Index, Product of Exponentials, Redundancy Resolution, Inverse Kinematics (DLS), Pose Inverse Kinematics, Analytical IK (OPW), Continuum Kinematics, Parallel Manipulator Kinematics, Mobile-Manipulator Kinematics | +| [Dynamics](doc/dynamics/README.md) | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler (RNEA), Articulated Body Algorithm (ABA), Chain Dynamics Model, Composite Rigid Body Algorithm (CRBA), Coriolis Matrix and Inertial Regressor, Dynamic Parameter Identification, Forward-Dynamics Integration, Generic Joint Link, Generalized-Momentum Observer | +| [Planning](doc/planning/README.md) | Primitive Distance Queries, Artificial Potential Fields, Self-Collision Checking, Convex Collision (GJK and EPA), Grid Search (A* and Dijkstra), RRT-Connect, Dubins Shortest Paths, Dynamic Window Approach, Reeds–Shepp Shortest Paths | +| [Trajectory](doc/trajectory/README.md) | Cubic Spline Trajectory, Cartesian Path with SLERP Orientation | +| [Mobile Robots](doc/mobile/README.md) | Planar Odometry, Differential-Drive Kinematics, Car-Like Kinematics, Omnidirectional Kinematics, Skid-Steer Kinematics, Pure-Pursuit Path Tracking, Kanayama Trajectory Tracking, Stanley Path Tracking | +| [Localization & Mapping](doc/localization/README.md) | Occupancy-Grid Mapping | +| [Manipulator Control](doc/controllers/manipulator/README.md) | PD Control with Gravity Compensation, Computed-Torque Control, Impedance Control, Operational-Space Control, Hybrid Position/Force Control, Slotine–Li Adaptive Control, Cable Tension Distribution | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index 36b1794..2adbcc7 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -40,8 +40,8 @@ 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. NT53–NT65 have shipped upstream, and their links point to the upstream -docs; the current pin (`63c9b91`) includes them. +numerical-toolbox pin that ships it. NT53–NT66 have shipped upstream, and their links point to the upstream +docs; the current pin (`dc147fb`) includes them. | # | Component | Upstream module | Spec / doc | Needed by | |------|-------------------------------------------------------|----------------------|-------------------------------------|----------------------------------------------------------------------------| @@ -58,7 +58,7 @@ docs; the current pin (`63c9b91`) includes them. | 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` | [SE2Transform][nt66] (spec) | F1, W1–W10, M24, E1, E2, E3, E5 | +| NT66 | SE(2) transform, twists, exp/log, pose error | `math` | [SE2Transform][nt66] | F1, W1–W10, M24, E1, E2, E3, E5 | | NT67 | Kabsch / Umeyama point-set alignment | `solvers` | [PointSetAlignment][nt67] (spec) | 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 | @@ -82,7 +82,7 @@ docs; the current pin (`63c9b91`) includes them. [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 +[nt66]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE2Transform.md [nt67]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/solvers/PointSetAlignment/implementation.md [nt70]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/filters/active/ParticleFilter/implementation.md [nt74]: https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/roadmap/filters/active/ResidualKalmanUpdate/implementation.md @@ -117,10 +117,9 @@ existing `dynamics/` convention. ### Manipulator list (by priority) -| # | Component | Target module | Depends on | Difficulty | -|-----|---------------------------------------------------|--------------------|-------------|------------| -| M24 | Mobile-manipulator / nonholonomic-base kinematics | `kinematics` | M8, M14, W1 | ★★★★☆ | -| M27 | Time-optimal path parameterization (TOPP-RA) | `trajectory` (new) | M29 | ★★★★★ | +| # | Component | Target module | Depends on | Difficulty | +|-----|----------------------------------------------|--------------------|------------|------------| +| M27 | Time-optimal path parameterization (TOPP-RA) | `trajectory` (new) | M29 | ★★★★★ | M2 (polynomial profile), M3 (trapezoidal profile), M4 (friction compensation), M6 (SE(3)) and M9 (S-curve profile) moved to numerical-toolbox as NT55, NT56, NT57, NT53 and NT60; their numbers are @@ -132,10 +131,6 @@ retired here. ### Tier 4 — Advanced ★★★★☆ -**M24. Mobile-manipulator / nonholonomic-base kinematics.** Combined base + arm Jacobian in the world frame with differential-drive constraints. -- *Algorithm / paper:* Y. Yamamoto, X. Yun, *IEEE Trans. Automatic Control*, 39(6), 1994. -- *Reuses / builds on:* the W1 differential-drive model for the base. - ### Tier 5 — Hard / research-grade ★★★★★ **M27. Time-optimal path parameterization (TOPP-RA).** Minimum-time traversal of a fixed path under joint velocity and torque limits via reachability analysis (two-variable LPs per grid stage, solved locally by Fourier–Motzkin elimination). @@ -149,16 +144,11 @@ Cross-family building blocks that several of the families below depend on. | # | Component | Target module | Depends on | Difficulty | |----|--------------------------------------------------------------|---------------|-----------------|------------| -| F1 | Planar odometry on SE(2) (exact arc integration) | `mobile` | NT66 | ★★☆☆☆ | | F2 | Online multi-axis trajectory generation (time-synchronized) | `trajectory` | NT73, M1 | ★★☆☆☆ | | F3 | Floating-base RNEA / ABA / CRBA | `dynamics` | M28, NT53 | ★★★☆☆ | | F4 | Via-point blending (joint and Cartesian) | `trajectory` | NT55, NT56, M10 | ★★★☆☆ | | F5 | Contact Jacobians and constrained (contact) forward dynamics | `dynamics` | F3, M8 | ★★★★☆ | -**F1. Planar odometry on SE(2).** Integrates a body twist `(vx, vy, ω)` over a sample period with the SE(2) exponential (exact constant-twist arc), shared by every wheeled model and the localization filters. -- *Algorithm / paper:* R. Siegwart, I. Nourbakhsh, D. Scaramuzza, *Introduction to Autonomous Mobile Robots*, 2nd ed. (2011), Ch. 5. -- *Reuses / builds on:* NT66. - **F2. Online multi-axis trajectory generation.** Joint-space wrapper over the upstream online generator: per-joint limits from M1, recomputation every cycle, all joints time-synchronized to the slowest. - *Algorithm / paper:* T. Kröger, F. Wahl, *IEEE T-RO*, 26(1), 2010. - *Reuses / builds on:* NT73, M1. @@ -177,58 +167,8 @@ 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 (+ 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. -- *Reuses / builds on:* NT66. - -**W2. Car-like kinematics.** Kinematic bicycle model with steering-angle and steering-rate limits, Ackermann wheel angles for the two front wheels. -- *Algorithm / paper:* R. Rajamani, *Vehicle Dynamics and Control*, 2nd ed. (2012), Ch. 2. -- *Reuses / builds on:* NT66. - -**W3. Omnidirectional kinematics.** Wheel-speed ↔ body-twist maps for mecanum and Swedish-wheel bases, including the roller-angle geometry. -- *Algorithm / paper:* P. Muir, C. Neuman, "Kinematic modeling of wheeled mobile robots," *J. Robotic Systems*, 4(2), 1987. -- *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. 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 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, 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 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; 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, 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. +All wheeled-robot items have shipped: see [Wheeled Mobile Robots](doc/mobile/README.md), and +[Planning](doc/planning/README.md) for the Dubins, Reeds–Shepp and dynamic-window planners. ## Aerial robots @@ -344,15 +284,13 @@ Cross-family building blocks that several of the families below depend on. ## Motion planning and collision -| # | Component | Target module | Depends on | Difficulty | -|---|-----------|---------------|------------|------------| +All planning items have shipped: see [Planning](doc/planning/README.md). ## Localization and mapping | # | Component | Target module | Depends on | Difficulty | |----|--------------------------------------------------------|----------------|------------------|------------| | E1 | Wheel + gyro odometry fusion (EKF on SE(2)) | `localization` | F1, NT66, NT74 | ★★★☆☆ | -| E2 | Occupancy-grid mapping (log-odds, ray casting) | `localization` | NT66 | ★★★☆☆ | | E3 | EKF landmark localization (known map, range–bearing) | `localization` | F1, NT66, NT74 | ★★★☆☆ | | E4 | Monte Carlo localization | `localization` | NT70, E2, F1 | ★★★★☆ | | E5 | 2-D scan matching (point-to-point / point-to-line ICP) | `localization` | NT67, NT64, NT66 | ★★★★☆ | @@ -361,10 +299,6 @@ Cross-family building blocks that several of the families below depend on. - *Algorithm / paper:* S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 5 and 7; J. Borenstein, L. Feng, "Gyrodometry," *ICRA* 1996. - *Reuses / builds on:* F1, NT66, NT74 (upstream `ExtendedKalmanFilter` cannot wrap residuals or gate). -**E2. Occupancy-grid mapping.** Log-odds cell updates from range scans along Bresenham rays on a bounded grid. -- *Algorithm / paper:* A. Elfes, *Computer*, 22(6), 1989; Thrun et al. (2005), Ch. 9. -- *Reuses / builds on:* NT66. - **E3. EKF landmark localization.** Pose tracking against a known landmark map with range–bearing measurements and gated data association. - *Algorithm / paper:* Thrun et al. (2005), Ch. 7. - *Reuses / builds on:* F1 (odometry noise), NT66, NT74. diff --git a/doc/kinematics/MobileManipulatorKinematics.md b/doc/kinematics/MobileManipulatorKinematics.md new file mode 100644 index 0000000..09e9cbd --- /dev/null +++ b/doc/kinematics/MobileManipulatorKinematics.md @@ -0,0 +1,95 @@ +# Mobile-Manipulator Kinematics + +## Overview & Motivation + +A **mobile manipulator** mounts a robot arm on a wheeled base. The base extends the arm's reach from a fixed workcell to a whole building. The combination is the standard platform for logistics picking, laboratory automation and service robots. + +Controlling the tool needs one Jacobian that maps **all** the robot's inputs, base and arm together, to the tool twist in the world frame. A differential-drive base adds a complication: it has three planar coordinates but only two controls, forward speed and yaw rate, because its wheels cannot slide sideways. +The combined Jacobian therefore uses the base's two controls directly, so every column is a velocity the robot can actually command. + +## Mathematical Theory + +### Tool Pose + +With the base pose $T_{wb}$ in SE(2), lifted to SE(3), the arm mounted at $T_{ba}$ on the base and the arm's tool pose $T_{at}(q)$: + +$$T_{wt} = T_{wb}\,T_{ba}\,T_{at}(q)$$ + +### Combined Jacobian + +The input is $u = (v, \omega, \dot{q})$: base forward speed, base yaw rate and arm joint rates. The tool twist $(\dot{p};\, \omega_t)$ in the world frame is $J u$, with $J \in \mathbb{R}^{6 \times (2 + n)}$ built column by column. + +**Forward speed** moves the whole robot along the heading $\varphi$: + +$$J_v = \begin{pmatrix} (\cos\varphi,\; \sin\varphi,\; 0) \\ 0 \end{pmatrix}$$ + +**Yaw rate** rotates the robot about the vertical axis through the axle midpoint. With the lever arm $r$ from the axle midpoint to the tool point: + +$$J_\omega = \begin{pmatrix} \hat{z} \times r \\ \hat{z} \end{pmatrix}$$ + +**Arm joints** contribute the arm's geometric Jacobian, expressed in the arm-base frame and rotated into the world with $R_{wa} = R_z(\varphi)\,R_{ba}$: + +$$J_q = \begin{pmatrix} R_{wa} & 0 \\ 0 & R_{wa} \end{pmatrix} J_{\text{arm}}(q)$$ + +No lever-arm correction is needed for the arm block, because its linear rows already describe the tool point. + +### Nonholonomic Base + +The base velocity in planar coordinates follows from the two controls: + +$$\begin{pmatrix} \dot{x} \\ \dot{y} \\ \dot{\varphi} \end{pmatrix} = S(\varphi) \begin{pmatrix} v \\ \omega \end{pmatrix}, \qquad S(\varphi) = \begin{pmatrix} \cos\varphi & 0 \\ \sin\varphi & 0 \\ 0 & 1 \end{pmatrix}$$ + +Every base velocity of this form satisfies the no-slip constraint $-\sin\varphi\,\dot{x} + \cos\varphi\,\dot{y} = 0$. The wheel speeds for $(v, \omega)$ come from the differential-drive inverse kinematics. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-------------------|--------|--------|------------------------------------------------------------| +| Combined Jacobian | $O(n)$ | $O(n)$ | One arm Jacobian and tool pose, two $3 \times n$ rotations | +| Tool pose | $O(n)$ | $O(1)$ | One arm forward kinematics and two compositions | +| Base constraint | $O(1)$ | $O(1)$ | One sine and one cosine | + +## Step-by-Step Walkthrough + +A three-joint arm (a yaw joint, then two pitch joints with links of $0.2$ m and $0.3$ m and a $0.25$ m tool) is mounted at $(0.1, 0, 0.3)$ on the base, rotated $90°$. The base stands at $(1, 2)$ heading $\pi/6$, and the arm is at $q = (0.4, -0.6, 0.9)$: + +1. **Tool pose**: the tool point is at $(0.699, 2.343, 0.696)$ in the world. +2. **Combined twist**: the input $(0.3, -0.5, 0.2, -0.1, 0.4)$ moves the tool at $(0.404, 0.200, -0.047)$ m/s and rotates it at $(-0.181, -0.239, -0.3)$ rad/s, matching a finite difference of the tool pose. +3. **Frame rotation**: stacking the arm Jacobian without rotating it into the world would give a twist error of $0.52$. +4. **Forward drive**: with the base heading along $x$, driving at $1$ m/s moves the tool at $(1, 0, 0)$ m/s without rotating it. +5. **Yaw**: turning the base at $1$ rad/s moves the tool sideways at $0.457$ m/s, the horizontal length of the lever arm. +6. **Base constraint**: for any heading, the base velocity has no sideways component. + +## Pitfalls & Edge Cases + +- **Frames**: the arm Jacobian lives in the arm-base frame. Forgetting the rotation into the world gives large errors as soon as the base turns. +- **Redundancy**: base plus arm has more inputs than the six task dimensions. Resolve it with a weighted pseudo-inverse or a null-space objective, for example the arm for fine motion and the base for reach. +- **Scaling**: the yaw column grows with the lever arm, and base speeds have different units from joint rates. Weight the columns before resolving redundancy. +- **Tool above the axle**: with no horizontal lever arm, yaw only rotates the tool and adds no translation. +- **Base dynamics**: the kinematic model ignores base acceleration limits and wheel slip. Saturate the base commands separately. + +## Variants & Generalizations + +- **Omnidirectional bases**: a third base column for lateral velocity, from the mecanum or Swedish-wheel kinematics. +- **Car-like bases**: steering angle and its rate limit replace the yaw rate. +- **Whole-body control**: task-priority and quadratic-programming controllers over the combined Jacobian. +- **Floating bases**: legged and aerial manipulators with a full 6-DOF base. + +## Applications + +- Fetch-and-carry and shelf picking in warehouses and hospitals. +- Laboratory automation moving samples between instruments. +- Mobile machining and inspection of large workpieces. + +## Connections to Other Algorithms + +- [Geometric Jacobian](GeometricJacobian.md): the arm block of the combined Jacobian. +- [Redundancy Resolution](RedundancyResolution.md): splits a task between base and arm. +- [Differential-Drive Kinematics](../mobile/DifferentialDrive.md): turns the base commands into wheel speeds. +- The SE(2) and SE(3) transforms come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Yamamoto, Y. and Yun, X. (1992). "Coordinating locomotion and manipulation of a mobile manipulator." *IEEE Conference on Decision and Control*, 2643–2648. +- Bayle, B., Fourquet, J.-Y. and Renaud, M. (2003). "Manipulability of wheeled mobile manipulators: application to motion generation." *International Journal of Robotics Research*, 22(7–8), 565–581. +- Lynch, K.M. and Park, F.C. (2017). *Modern Robotics*. Cambridge University Press. Chapter 13.5. diff --git a/doc/kinematics/README.md b/doc/kinematics/README.md index 8cc5f3a..5d7ac98 100644 --- a/doc/kinematics/README.md +++ b/doc/kinematics/README.md @@ -14,6 +14,7 @@ Geometry and motion algorithms for serial kinematic chains. | [Product of Exponentials](ProductOfExponentials.md) | Screw-axis forward kinematics, space Jacobian and its conversion to the geometric Jacobian — $O(n)$ | | [Continuum Kinematics](ContinuumKinematics.md) | Piecewise constant-curvature arcs $(\kappa, \phi, s)$: singularity-free section transform, multi-section forward kinematics and closed-form single-section inverse | | [Parallel Manipulator Kinematics](ParallelManipulatorKinematics.md) | Stewart–Gough leg lengths, leg Jacobian and Newton forward kinematics on SE(3); closed-form Delta-robot inverse and forward kinematics | +| [Mobile-Manipulator Kinematics](MobileManipulatorKinematics.md) | Combined world-frame Jacobian of a differential-drive base and an arm over forward speed, yaw rate and joint rates, with the no-slip base constraint — $O(n)$ | | [Redundancy Resolution](RedundancyResolution.md) | Damped pseudo-inverse primary task plus exact null-space projection of a secondary objective for redundant arms | | [Inverse Kinematics](InverseKinematics.md) | Solves joint angles that place the tool point on a target via Damped Least Squares — $O(k \cdot n)$ iterative | | [Pose Inverse Kinematics](PoseInverseKinematics.md) | Full 6-DoF pose IK: damped least squares on the weighted log-map pose error with manipulability-adaptive damping, step clamping and joint limits | diff --git a/doc/localization/OccupancyGridMap.md b/doc/localization/OccupancyGridMap.md new file mode 100644 index 0000000..6a6e960 --- /dev/null +++ b/doc/localization/OccupancyGridMap.md @@ -0,0 +1,90 @@ +# Occupancy-Grid Mapping + +## Overview & Motivation + +A mobile robot needs a map to plan collision-free motion and to localise itself. The **occupancy grid** (Moravec and Elfes, 1985) is the simplest and most widely used representation: the plane is divided into square cells, and each cell holds the probability that it is occupied. + +The grid is built from range scans taken at known sensor poses. Every beam passes through free space and ends on an obstacle, so the cells it crosses become more likely free and its end cell more likely occupied. Storing probabilities as **log odds** turns each Bayesian update into a single addition, and walking each beam with **Bresenham's line algorithm** needs only integer arithmetic. +That makes the update cheap enough for a microcontroller. + +## Mathematical Theory + +### Log-Odds Representation + +A cell with occupancy probability $p$ stores + +$$l = \ln\frac{p}{1 - p}, \qquad p = 1 - \frac{1}{1 + e^{l}}$$ + +An unknown cell has $l = 0$ ($p = 0.5$). With a static world and independent measurements, Bayes' rule for a new observation $z$ becomes an addition of the inverse sensor model: + +$$l_k = l_{k-1} + \ln\frac{p(\text{occ} \mid z)}{1 - p(\text{occ} \mid z)}$$ + +### Inverse Sensor Model + +Each beam adds $l_{\text{occ}} > 0$ to the cell containing its end point and $l_{\text{free}} < 0$ to every cell it crosses before. With $l_{\text{occ}} = 0.85$ ($p = 0.70$) and $l_{\text{free}} = -0.4$ ($p = 0.40$), one hit outweighs about two pass-throughs. + +Readings at or beyond the maximum range have no reliable end point. They only clear the cells along the beam up to the maximum range, so they never paint phantom walls. + +### Clamping + +Each cell is clamped to $[l_{\min}, l_{\max}]$. Without the clamp, a cell observed thousands of times would need thousands of contrary observations to change, so the map could not follow doors opening or people moving. + +### Ray Traversal + +The sensor's cell and the beam's end cell are found by $\lfloor (x - o)/r \rfloor$ for the grid origin $o$ and resolution $r$. Bresenham's algorithm steps one cell at a time along the major axis, deciding with an integer error term when to step along the minor axis. It visits one cell per major-axis step in every octant. A beam that leaves the grid stops at the border. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|----------------|----------------------|----------|---------------------------------------------------| +| Integrate scan | $O(\sum_i \rho_i/r)$ | $O(1)$ | Integer stepping, one addition and clamp per cell | +| Cell query | $O(1)$ | $O(1)$ | Probability needs one exponential | +| Map storage | — | $O(W H)$ | One scalar per cell, no heap | + +A 360-beam scan at 4 m and 5 cm resolution updates about 29 000 cells. A 10 m by 10 m map at 5 cm takes 160 KB in single precision. + +## Step-by-Step Walkthrough + +A 1 m by 1 m grid at 10 cm resolution, with the sensor at the centre of cell $(0, 0)$, $l_{\text{occ}} = 0.85$, $l_{\text{free}} = -0.4$, a clamp of $[-2, 3.5]$ and a maximum range of 4 m: + +1. **Axis beam**: a 0.5 m reading straight ahead clears cells $(0, 0)$ to $(4, 0)$ and marks $(5, 0)$ occupied. +2. **Shallow beam**: a reading ending at $(0.75, 0.35)$ crosses $(0,0)$, $(1,0)$, $(2,1)$, $(3,1)$, $(4,2)$, $(5,2)$ and $(6,3)$, and hits $(7,3)$. +3. **Maximum range**: with the maximum range lowered to 0.5 m, the same axis reading clears six cells and marks none. +4. **Leaving the map**: a 2 m reading clears the whole first row and stops at the border. +5. **Clamping**: after ten identical scans, the hit cell saturates at $3.5$ ($p = 0.971$) and the sensor cell at $-2$ ($p = 0.119$). +6. **Origin**: with the grid corner at $(-0.5, -0.5)$, the world origin falls in cell $(5, 5)$, whose centre is $(0.05, 0.05)$. + +## Pitfalls & Edge Cases + +- **Pose errors**: the update assumes the sensor pose is exact. Odometry drift smears walls, so map with a localised pose or with SLAM. +- **Corner cells**: Bresenham skips cells a diagonal beam only touches at a corner. An exact traversal (Amanatides and Woo) visits them at a small extra cost. +- **Invalid readings**: zero or negative ranges mark the sensor's own cell. Filter them before integrating. +- **Dynamic obstacles**: people walking through a scan leave traces. The clamp limits how long they persist. +- **Memory**: the grid size is fixed at compile time. Size it to the area and resolution the target can afford. + +## Variants & Generalizations + +- **Exact ray traversal**: Amanatides–Woo visits every cell a beam crosses. +- **Beam-endpoint and likelihood fields**: precomputed distance maps for fast scan matching and Monte Carlo localization. +- **Multi-resolution and sparse maps**: quadtrees and OctoMap for large or 3-D environments. +- **Occupancy grid SLAM**: Rao–Blackwellised particle filters and graph optimisation that estimate the poses as well. + +## Applications + +- Maps for indoor service robots, warehouse vehicles and robot vacuums. +- Obstacle maps for grid planners such as A*. +- The map against which Monte Carlo localization tracks the robot. + +## Connections to Other Algorithms + +- [Grid Search (A* and Dijkstra)](../planning/GridSearch.md): plans shortest paths on the thresholded grid. +- [Planar Odometry](../mobile/PlanarOdometry.md): provides sensor poses for mapping when no better pose is available. +- The SE(2) pose used to place each beam comes from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Moravec, H. and Elfes, A. (1985). "High resolution maps from wide angle sonar." *IEEE International Conference on Robotics and Automation*, 116–121. +- Elfes, A. (1989). "Using occupancy grids for mobile robot perception and navigation." *Computer*, 22(6), 46–57. +- Thrun, S., Burgard, W. and Fox, D. (2005). *Probabilistic Robotics*. MIT Press. Chapter 9. +- Bresenham, J.E. (1965). "Algorithm for computer control of a digital plotter." *IBM Systems Journal*, 4(1), 25–30. +- Amanatides, J. and Woo, A. (1987). "A fast voxel traversal algorithm for ray tracing." *Eurographics*, 3–10. diff --git a/doc/localization/README.md b/doc/localization/README.md new file mode 100644 index 0000000..9dd0b5a --- /dev/null +++ b/doc/localization/README.md @@ -0,0 +1,9 @@ +# Localization & Mapping + +Maps, state estimation and localization for mobile robots moving in the plane. + +## Algorithms + +| Algorithm | Description | +|-----------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------| +| [Occupancy-Grid Mapping](OccupancyGridMap.md) | Bounded grid of clamped log-odds cells updated from range scans along integer Bresenham rays, with max-range clearing — $O(\sum \rho / r)$ | diff --git a/doc/mobile/CarLikeKinematics.md b/doc/mobile/CarLikeKinematics.md new file mode 100644 index 0000000..ab18818 --- /dev/null +++ b/doc/mobile/CarLikeKinematics.md @@ -0,0 +1,97 @@ +# Car-Like Kinematics (Kinematic Bicycle and Ackermann) + +## Overview & Motivation + +Cars, tractors, forklifts and many outdoor robots steer with their front wheels. They cannot turn in place or move sideways. Their motion is limited by the steering angle, and the steering angle itself is limited in range and rate. + +The **kinematic bicycle** model merges the two front wheels into one virtual wheel at steering angle $\delta$ and the two rear wheels into one, with no tyre slip. It is accurate at low lateral acceleration (below roughly $0.4\,g$), and it is the model behind classical path trackers such as pure pursuit and Stanley. +The **Ackermann** geometry maps the virtual steering angle back to the two real front wheels so that all four wheels turn about one common centre. + +## Mathematical Theory + +### Bicycle Model + +With wheel base $L$ and the reference point at the rear-axle midpoint, the body twist for rear-axle speed $v$ and steering angle $\delta$ is + +$$\xi = \left(v,\; 0,\; \frac{v \tan\delta}{L}\right), \qquad \kappa = \frac{\tan\delta}{L}$$ + +The rear-axle midpoint has no lateral velocity, which makes it the natural reference for pure pursuit. A negative speed reverses, and with it the sign of the yaw rate. + +### Other Reference Points + +A point a distance $a$ ahead of the rear axle (the front axle at $a = L$, the centre of gravity at $a = l_r$) moves with + +$$\xi_a = (v,\; a\,\omega,\; \omega), \qquad \omega = \frac{v\tan\delta}{L}$$ + +This is the SE(2) adjoint of the rear-axle twist, so it agrees exactly with composing poses. + +### Steering for a Curvature + +$$\delta = \operatorname{clamp}\left(\arctan(L\kappa),\; -\delta_{\max},\; \delta_{\max}\right)$$ + +### Steering Actuator Limits + +A requested angle is first clamped to $\pm\delta_{\max}$, then limited in rate relative to the previous command: + +$$\delta_k = \operatorname{clamp}\left(\delta_{\text{req}},\; \delta_{k-1} - \dot{\delta}_{\max}\Delta t,\; \delta_{k-1} + \dot{\delta}_{\max}\Delta t\right)$$ + +### Ackermann Geometry + +For both front wheels to roll about the turning centre on the rear-axle line at radius $R = L/\tan\delta$, with track width $W$: + +$$\delta_{\text{inner}} = \arctan\frac{L\tan\delta}{L - \tfrac{W}{2}\tan\delta}, \qquad \delta_{\text{outer}} = \arctan\frac{L\tan\delta}{L + \tfrac{W}{2}\tan\delta}$$ + +This form stays finite at $\delta = 0$. The inner wheel must stay below $90°$, which requires $|\delta| < \arctan(2L/W)$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|------------------------|--------|--------|---------------------------------------------| +| Twist and curvature | $O(1)$ | $O(1)$ | One tangent | +| Steering for curvature | $O(1)$ | $O(1)$ | One arctangent and a clamp | +| Ackermann angles | $O(1)$ | $O(1)$ | One tangent, two arctangents | +| State | — | $O(1)$ | Wheel base, track width and steering limits | + +## Step-by-Step Walkthrough + +A vehicle with $L = 2.5$ m, $W = 1.5$ m, $\delta_{\max} = 0.6$ rad and $\dot{\delta}_{\max} = 0.5$ rad/s: + +1. **Twist**: at $10$ m/s and $\delta = 0.1$ rad, the yaw rate is $10 \cdot \tan(0.1)/2.5 = 0.401$ rad/s. +2. **Front axle**: a point $1.2$ m ahead of the rear axle also moves sideways at $1.2 \cdot 0.401 = 0.482$ m/s, matching the SE(2) adjoint. +3. **Curvature**: $\kappa = 0.1$ needs $\delta = 0.245$ rad. $\kappa = 0.5$ would need $0.896$ rad, so it saturates at $0.6$. +4. **Rate limit**: from $0.1$ rad, a request of $0.5$ rad reaches only $0.2$ rad in $0.2$ s. From $0.55$ rad, a request of $-0.9$ rad is clamped to $-0.6$, then rate-limited to $0.05$ rad after one second. +5. **Ackermann**: at $\delta = 0.3$ rad, the inner wheel steers $0.329$ rad and the outer $0.276$ rad, and both point at the same centre $8.08$ m away. +6. **Turning radius**: the minimum turning radius is $2.5/\tan(0.6) = 3.65$ m. + +## Pitfalls & Edge Cases + +- **High lateral acceleration**: beyond about $0.4\,g$, tyre slip makes the kinematic model wrong. Use a dynamic bicycle model with cornering stiffness. +- **Reference point mismatch**: pure pursuit uses the rear axle, Stanley the front axle. Mixing them biases the tracker. +- **Ignoring the rate limit**: a real steering actuator lags. Trackers that assume instant steering overshoot at speed. +- **Steering near $90°$**: the Ackermann inner angle diverges as $\tan\delta \to 2L/W$. The maximum steering angle must stay below that. +- **Reversing**: negative speeds flip the yaw rate for the same steering angle. + +## Variants & Generalizations + +- **Dynamic bicycle model**: lateral tyre forces with slip angles for high-speed driving. +- **Four-wheel steering**: front and rear steering angles with a movable turning centre. +- **Trailers**: articulated kinematics for tractor–trailer combinations. +- **Curvature-rate models**: clothoid-based planning that respects the steering rate. + +## Applications + +- Autonomous cars, agricultural vehicles and outdoor delivery robots. +- Path tracking with pure pursuit, Stanley or model-predictive control. +- Feasibility checks for curvature-constrained planners such as Dubins and Reeds–Shepp. + +## Connections to Other Algorithms + +- [Differential-Drive Kinematics](DifferentialDrive.md): the turn-in-place alternative with the same unicycle twist form. +- [Planar Odometry](PlanarOdometry.md): integrates the rear-axle twist into a pose. +- The SE(2) adjoint used for other reference points comes from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Rajamani, R. (2012). *Vehicle Dynamics and Control*, 2nd ed. Springer. Chapter 2. +- Paden, B., Čáp, M., Yong, S.Z., Yershov, D. and Frazzoli, E. (2016). "A survey of motion planning and control techniques for self-driving urban vehicles." *IEEE Transactions on Intelligent Vehicles*, 1(1), 33–55. +- Kong, J., Pfeiffer, M., Schildbach, G. and Borrelli, F. (2015). "Kinematic and dynamic vehicle models for autonomous driving control design." *IEEE Intelligent Vehicles Symposium*, 1094–1099. diff --git a/doc/mobile/DifferentialDrive.md b/doc/mobile/DifferentialDrive.md new file mode 100644 index 0000000..b16491e --- /dev/null +++ b/doc/mobile/DifferentialDrive.md @@ -0,0 +1,84 @@ +# Differential-Drive Kinematics + +## Overview & Motivation + +Most indoor robots, from vacuum cleaners to warehouse carriers, drive on **two independently powered wheels on one axle**, with passive casters for balance. Steering comes purely from the speed difference between the wheels: equal speeds drive straight, opposite speeds spin in place. + +The kinematics are linear and cheap, but two details matter in practice. The wheels cannot slide sideways, so the robot moves like a unicycle with no lateral velocity. And when a command exceeds the motor limits, how it is saturated decides whether the robot still follows the planned curve or veers off it. + +## Mathematical Theory + +### Forward Kinematics + +With wheel radius $r$, track width $B$ between the contact points, and wheel angular speeds $\omega_L$, $\omega_R$, the body twist at the axle midpoint is + +$$v = r\,\frac{\omega_L + \omega_R}{2}, \qquad v_y = 0, \qquad \omega = r\,\frac{\omega_R - \omega_L}{B}$$ + +The lateral velocity is zero because the wheels roll without slipping sideways (the nonholonomic constraint). + +### Inverse Kinematics + +$$\omega_L = \frac{v - \omega B/2}{r}, \qquad \omega_R = \frac{v + \omega B/2}{r}$$ + +### Saturation That Preserves the Path + +When either wheel would exceed $\omega_{\max}$, both are scaled by the same factor: + +$$k = \frac{\omega_{\max}}{\max(|\omega_L|, |\omega_R|)}, \qquad (\omega_L, \omega_R) \leftarrow k\,(\omega_L, \omega_R)$$ + +Because $v$ and $\omega$ scale together, the curvature $\omega / v$ is unchanged. The robot follows the same arc, only more slowly. Clipping each wheel separately would change the curvature. + +### Speed Envelope + +The feasible commands form a diamond in the $(v, \omega)$ plane: + +$$\frac{|v|}{v_{\max}} + \frac{|\omega|}{\omega_{\text{body},\max}} \le 1, \qquad v_{\max} = r\,\omega_{\max}, \qquad \omega_{\text{body},\max} = \frac{2 r\,\omega_{\max}}{B}$$ + +Saturating by a common factor projects an infeasible command onto the diamond along the ray through the origin. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-------------------|--------|--------|----------------------------------------| +| Forward / inverse | $O(1)$ | $O(1)$ | A few multiplications, no trigonometry | +| Saturation | $O(1)$ | $O(1)$ | One maximum and one division | +| State | — | $O(1)$ | Radius, track width and wheel limit | + +## Step-by-Step Walkthrough + +A robot with $r = 5$ cm, $B = 30$ cm and $\omega_{\max} = 20$ rad/s: + +1. **Forward**: wheels at $(10, 14)$ rad/s give $v = 0.6$ m/s and $\omega = 0.667$ rad/s. +2. **Spin in place**: $(v, \omega) = (0, 2)$ needs the wheels at $(-6, 6)$ rad/s. +3. **Saturation**: $(1.2, 2)$ needs $(18, 30)$ rad/s, beyond the limit. Scaling by $20/30$ gives $(12, 20)$ rad/s, which is $(0.8, 1.333)$. The curvature $\omega / v = 1.667$ is the same before and after. +4. **Envelope corners**: the top linear speed is $1$ m/s and the top turn rate is $6.667$ rad/s. + +## Pitfalls & Edge Cases + +- **Per-wheel clipping**: saturating wheels independently bends the path and makes path trackers overshoot. Scale both wheels together. +- **Calibration**: odometry accuracy depends directly on $r$ (possibly different left and right) and $B$. Calibrate them, for example with the UMBmark square-path test. +- **Slip**: on carpet, ramps or under acceleration the no-slip assumption breaks. Use a skid-steer model or fuse a gyroscope. +- **Reversing**: negative $v$ maps symmetrically. The saturation uses absolute values. +- **Zero command**: scaling with both wheels at rest returns zeros without dividing. + +## Variants & Generalizations + +- **Skid-steer**: four or more wheels or tracks that must slip to turn, modelled with instantaneous centres of rotation. +- **Dynamic models**: motor torque, inertia and traction limits instead of pure kinematics. +- **Unicycle control**: path trackers designed on the $(v, \omega)$ model and mapped to wheels through the inverse kinematics. + +## Applications + +- Indoor service and logistics robots. +- Educational and research platforms. +- The mobile base of mobile manipulators. + +## Connections to Other Algorithms + +- [Planar Odometry](PlanarOdometry.md): integrates the body twist, or wheel-angle increments mapped through the forward kinematics, into a pose. + +## References & Further Reading + +- Siegwart, R., Nourbakhsh, I.R. and Scaramuzza, D. (2011). *Introduction to Autonomous Mobile Robots*, 2nd ed. MIT Press. Chapter 3. +- Lynch, K.M. and Park, F.C. (2017). *Modern Robotics*. Cambridge University Press. Chapter 13. +- Borenstein, J. and Feng, L. (1996). "Measurement and correction of systematic odometry errors in mobile robots." *IEEE Transactions on Robotics and Automation*, 12(6), 869–880. diff --git a/doc/mobile/KanayamaController.md b/doc/mobile/KanayamaController.md new file mode 100644 index 0000000..ab705fc --- /dev/null +++ b/doc/mobile/KanayamaController.md @@ -0,0 +1,95 @@ +# Kanayama Trajectory Tracking + +## Overview & Motivation + +Path trackers such as pure pursuit follow a geometric path at whatever speed the robot happens to drive. Many tasks also fix **when** the robot should be where: a delivery robot keeping to a schedule, several robots moving in formation, or a base that must arrive in sync with an arm. These need **trajectory tracking**, where a reference pose and its velocity are given at every instant. + +The Kanayama controller (Kanayama et al., 1990) is the classical solution for unicycle-type bases such as differential-drive and skid-steer robots. It expresses the tracking error in the robot's own frame and adds a feedback correction to the reference velocity. A Lyapunov function proves that the error converges to zero from any initial error while the reference keeps moving forward. + +## Mathematical Theory + +### Tracking Error + +With the robot pose $T$ and the reference pose $T_r$ in SE(2), the reference seen from the robot is + +$$T_e = T^{-1} T_r, \qquad e = (e_x,\; e_y,\; e_\theta)$$ + +$e_x$ is the along-track error, $e_y$ the cross-track error and $e_\theta \in (-\pi, \pi]$ the heading error, all in the robot frame. + +### Control Law + +Given the reference speed $v_r$ and turn rate $\omega_r$, with positive gains $k_x$, $k_y$ and $k_\theta$: + +$$v = v_r \cos e_\theta + k_x\,e_x, \qquad \omega = \omega_r + v_r\left(k_y\,e_y + k_\theta \sin e_\theta\right)$$ + +With zero error the command is exactly the feedforward $(v_r, \omega_r)$. + +### Stability + +The candidate function + +$$V = e_x^2 + e_y^2 + \frac{2\,(1 - \cos e_\theta)}{k_y}$$ + +is positive definite on $e_\theta \in (-\pi, \pi)$. Along the error dynamics of the unicycle it satisfies + +$$\dot{V} = -k_x\,e_x^2 - \frac{v_r\,k_\theta}{k_y}\sin^2 e_\theta \le 0$$ + +whenever $v_r > 0$, so the error never grows and converges to zero. + +### Tuning + +Linearised about a straight reference, the lateral loop is a second-order system with natural frequency $v_r\sqrt{k_y}$ and damping ratio $k_\theta / (2\sqrt{k_y})$. The critically damped choice is + +$$k_\theta = 2\sqrt{k_y}$$ + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|----------------|--------|--------|--------------------------------------------------| +| Tracking error | $O(1)$ | $O(1)$ | One SE(2) inverse and composition, one $\arctan$ | +| Control law | $O(1)$ | $O(1)$ | One sine and one cosine | +| State | — | $O(1)$ | Three gains | + +## Step-by-Step Walkthrough + +Gains $k_x = 1$ s⁻¹, $k_y = 4$ m⁻² and $k_\theta = 4$ m⁻¹ (critically damped), with reference speed $v_r = 0.5$ m/s: + +1. **Zero error**: the command is the feedforward, $(0.5, 0.1)$ for a reference turning at $0.1$ rad/s. +2. **Along-track error**: a reference $0.2$ m ahead raises the speed to $0.5 + 0.2 = 0.7$ m/s. +3. **Cross-track error**: a reference $0.1$ m to the left gives $\omega = 0.5 \cdot 4 \cdot 0.1 = 0.2$ rad/s, turning towards it. +4. **Heading error**: a reference rotated $0.3$ rad gives $v = 0.5\cos 0.3 = 0.478$ m/s and $\omega = 0.5 \cdot 4 \sin 0.3 = 0.591$ rad/s. +5. **Robot frame**: a robot at $(1, 1)$ facing north with the reference at $(1, 2)$ sees an along-track error of $1$ m, not a lateral one, and speeds up to $1.5$ m/s. +6. **Closed loop**: starting at $(-0.3, 0.5, -0.4)$ with $V = 0.379$, the error falls below $10^{-4}$ within $20$ s, and $V$ decreases at every $10$ ms step. + +## Pitfalls & Edge Cases + +- **Stationary reference**: with $v_r = 0$ the cross-track error has no effect on the command. A unicycle cannot move sideways, so point stabilisation needs a time-varying or discontinuous law. +- **Reversed reference**: at $e_\theta = \pi$ the law drives backwards at $-v_r + k_x e_x$. The output stays bounded, but $V$ is no longer a valid Lyapunov function there. +- **Saturation**: the proof assumes the command is applied as computed. Saturate with a curvature-preserving scaling and keep the reference within about 70 % of the actuator limits. +- **Reference consistency**: $(v_r, \omega_r)$ must be the actual velocity of the reference pose, or the controller tracks a steady error. + +## Variants & Generalizations + +- **Samson's and de Luca's laws**: alternative Lyapunov designs with different error coordinates. +- **Dynamic extension**: backstepping through the robot's dynamics for torque-controlled bases. +- **Car-like vehicles**: the same structure with the steering angle as input and its limits. +- **Model-predictive tracking**: explicit handling of velocity and acceleration limits. + +## Applications + +- Time-parameterised trajectory following for differential-drive and skid-steer robots. +- Formation control, where each robot tracks a reference derived from a leader. +- Mobile manipulation, where the base must keep pace with an arm trajectory. + +## Connections to Other Algorithms + +- [Differential-Drive Kinematics](DifferentialDrive.md): maps $(v, \omega)$ to wheel speeds and saturates them while preserving curvature. +- [Skid-Steer Kinematics](SkidSteerKinematics.md): another unicycle-type base the controller drives. +- [Planar Odometry](PlanarOdometry.md): provides the pose the controller uses. +- [Pure-Pursuit Path Tracking](PurePursuit.md): the geometric alternative when timing does not matter. + +## References & Further Reading + +- Kanayama, Y., Kimura, Y., Miyazaki, F. and Noguchi, T. (1990). "A stable tracking control method for an autonomous mobile robot." *IEEE International Conference on Robotics and Automation*, 384–389. +- Siciliano, B., Sciavicco, L., Villani, L. and Oriolo, G. (2009). *Robotics: Modelling, Planning and Control*. Springer. Chapter 11. +- Samson, C. and Ait-Abderrahim, K. (1991). "Feedback control of a nonholonomic wheeled cart in Cartesian space." *IEEE International Conference on Robotics and Automation*, 1136–1141. diff --git a/doc/mobile/OmnidirectionalKinematics.md b/doc/mobile/OmnidirectionalKinematics.md new file mode 100644 index 0000000..6e17e8a --- /dev/null +++ b/doc/mobile/OmnidirectionalKinematics.md @@ -0,0 +1,94 @@ +# Omnidirectional Kinematics (Mecanum and Swedish Wheels) + +## Overview & Motivation + +A **holonomic** base can move in any planar direction and rotate at the same time, with no need to turn before translating. It can slide sideways into a narrow aisle or dock against a workstation. Holonomic bases are built from **Swedish wheels**: wheels with free rollers around the rim, so each one drives in one direction and slides freely in another. + +The common layouts are three omniwheels at $120°$ (the "kiwi" drive) and four mecanum wheels with $45°$ rollers. Both follow from one general model in which every wheel contributes one linear constraint between its speed and the body twist. With more than three wheels the constraints overdetermine the twist, which also allows slip to be detected. + +## Mathematical Theory + +### Wheel Constraint + +Wheel $i$ sits at $(x_i, y_i)$ in the body frame, drives along angle $\beta_i$, has roller angle $\gamma_i$ (zero for omniwheels, $\pm 45°$ for mecanum) and radius $r_i$. With $a_i = \beta_i + \gamma_i$, its angular speed for a body twist $(v_x, v_y, \omega)$ is + +$$u_i = \frac{1}{r_i\cos\gamma_i}\begin{pmatrix} \cos a_i & \sin a_i & x_i\sin a_i - y_i\cos a_i \end{pmatrix}\begin{pmatrix} v_x \\ v_y \\ \omega \end{pmatrix}$$ + +The roller absorbs the velocity component along its own axis. A roller aligned with the drive direction ($\cos\gamma_i = 0$) transmits nothing and is rejected. + +### Inverse and Forward Kinematics + +Stacking the rows gives the wheel Jacobian $H \in \mathbb{R}^{N \times 3}$: + +$$u = H\,\xi, \qquad \xi = H^{+} u, \qquad H^{+} = (H^T H)^{-1} H^T$$ + +$H^{+}$ is computed once at construction from a $3 \times 3$ Cholesky factorization. It is exact for three wheels and a least-squares fit for more. The layout must span all three degrees of freedom, or $H^T H$ is singular. + +### Standard Mecanum Cart + +For wheels at $(\pm l_x, \pm l_y)$ with $\beta = 0$, rollers $-45°, +45°, +45°, -45°$ for front-left, front-right, rear-left, rear-right, and $k = l_x + l_y$: + +$$r\,u = (v_x - v_y - k\omega,\;\; v_x + v_y + k\omega,\;\; v_x + v_y - k\omega,\;\; v_x - v_y + k\omega)$$ + +### Slip Detection + +With $N > 3$, measured wheel speeds should lie in the range of $H$. The residual + +$$\rho = \left\|u - H H^{+} u\right\|$$ + +is zero for consistent wheels and grows when a wheel slips, lifts or reports a faulty encoder reading. + +### Saturation + +When any wheel would exceed $\omega_{\max}$, all wheels are scaled by $\omega_{\max}/\max_i |u_i|$, so the body keeps moving in the commanded direction, more slowly. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|--------------------|--------|--------|--------------------------------------------| +| Inverse kinematics | $O(N)$ | $O(N)$ | $3N$ multiply-adds | +| Forward kinematics | $O(N)$ | $O(1)$ | $3N$ multiply-adds with the stored $H^{+}$ | +| Slip residual | $O(N)$ | $O(N)$ | One forward and one inverse map | +| Construction | $O(N)$ | $O(N)$ | One $3 \times 3$ Cholesky factorization | + +## Step-by-Step Walkthrough + +A mecanum cart with $r = 5$ cm, $l_x = 0.2$ m and $l_y = 0.15$ m, and a kiwi base with omniwheels at radius $0.2$ m: + +1. **Mecanum Jacobian**: the rows are $(20, -20, -7)$, $(20, 20, 7)$, $(20, 20, -7)$ and $(20, -20, 7)$, the closed form scaled by $1/r$. +2. **Round trip**: the twist $(0.3, -0.2, 0.5)$ needs wheels at $(6.5, 5.5, -1.5, 13.5)$ rad/s, which map back to the same twist with zero slip residual. +3. **One wheel spinning**: wheels at $(10, 0, 0, 0)$ give the least-squares twist $(0.125, -0.125, -0.357)$ and a slip residual of $5$, flagging the inconsistency. +4. **Kiwi**: three wheels determine the twist exactly. Equal wheel speeds of $5$ rad/s spin the base in place at $1.25$ rad/s. +5. **Saturation**: $(1, 0.5, 1)$ needs $(3, 37, 23, 17)$ rad/s. Scaling by $20/37$ keeps the direction and gives $(0.541, 0.270, 0.541)$. + +## Pitfalls & Edge Cases + +- **Roller handedness**: a mecanum wheel mounted with the wrong roller direction inverts lateral motion. Confirm each roller sign by commanding pure sideways motion. +- **Degenerate layouts**: all wheels driving parallel cannot rotate or slide sideways. The construction rejects them. +- **Floor contact**: mecanum wheels need all four wheels on the ground. On uneven floors, slip and wheel lift-off are common, so watch the residual. +- **Per-wheel clipping**: saturating wheels independently changes the direction of motion. Scale them together. +- **Efficiency**: rollers waste energy and wear quickly, and mecanum traction is lower than a plain wheel's. + +## Variants & Generalizations + +- **Powered caster vehicles**: steerable offset wheels for holonomic motion without rollers. +- **Weighted least squares**: down-weighting wheels known to slip in the forward kinematics. +- **Dynamic models**: roller friction and wheel inertia for high-acceleration platforms. + +## Applications + +- Industrial carts and mobile manipulators in tight workcells. +- Robot soccer and education platforms. +- Omnidirectional camera dollies and patient-transfer devices. + +## Connections to Other Algorithms + +- [Differential-Drive Kinematics](DifferentialDrive.md): the nonholonomic two-wheel counterpart. +- [Planar Odometry](PlanarOdometry.md): integrates the forward-kinematics twist into a pose. +- The Cholesky factorization comes from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Lynch, K.M. and Park, F.C. (2017). *Modern Robotics*. Cambridge University Press. Chapter 13.2. +- Siegwart, R., Nourbakhsh, I.R. and Scaramuzza, D. (2011). *Introduction to Autonomous Mobile Robots*, 2nd ed. MIT Press. Chapter 3. +- Muir, P.F. and Neuman, C.P. (1987). "Kinematic modeling of wheeled mobile robots." *Journal of Robotic Systems*, 4(2), 281–340. diff --git a/doc/mobile/PlanarOdometry.md b/doc/mobile/PlanarOdometry.md new file mode 100644 index 0000000..3bd44ef --- /dev/null +++ b/doc/mobile/PlanarOdometry.md @@ -0,0 +1,89 @@ +# Planar Odometry on SE(2) + +## Overview & Motivation + +Every wheeled robot estimates where it is by **dead reckoning**: it integrates its measured body velocity, from wheel encoders and sometimes a gyro, into a pose. The result drifts, but it is smooth, fast and always available, and it feeds every localization filter as the prediction step. + +The usual way to integrate, Euler or Runge–Kutta on $(\dot{x}, \dot{y}, \dot{\theta})$, treats the heading as constant within a step. When the robot turns, that biases the position. A constant body twist held for one period moves the robot along a **circular arc**. Integrating with the SE(2) exponential lands exactly on that arc, so turning inside a step introduces no error. + +## Mathematical Theory + +### Exact Arc Integration + +With the pose $T_k \in SE(2)$ and the body twist $\xi = (v_x, v_y, \omega)$ held over a period $\Delta t$: + +$$T_{k+1} = T_k \exp(\xi\,\Delta t)$$ + +The exponential moves the body by $V(\omega\Delta t)\,(v_x, v_y)\,\Delta t$ in its own frame and turns it by $\omega\Delta t$. For $\omega = 0$ it is a straight segment. The update is exact for any step size as long as the twist is constant, so one step of $2\pi$ s along a radius-2 arc gives the same pose as a thousand small steps. + +### Encoder Increments + +The wheel models map wheel speeds linearly to body twists. Applied to wheel-angle increments instead, they give the body displacement $\Delta = \xi\,\Delta t$ directly, which is integrated as $\exp(\Delta)$ with unit time. No division by a jittery sample period is needed. + +### Renormalisation + +The rotation is a unit complex number. Rounding in each composition makes $\cos^2 + \sin^2$ drift by about one unit in the last place, and long runs accumulate it. Rescaling to unit length after every step keeps the rotation valid indefinitely. + +### Odometry Increments + +Motion models in localization filters consume the body-frame motion between two odometry readings: + +$$\Delta = \log\left(T_{\text{from}}^{-1}\, T_{\text{to}}\right)$$ + +This is the twist that, held for unit time, carries one reading into the other. It requires the heading change between readings to stay below $\pi$. + +### Increment Noise + +The noise of an increment grows with the distance travelled $\Delta s$ and the rotation $\Delta\theta$: + +$$\sigma_{xy}^2 = \alpha_3\,\Delta s^2 + \alpha_4\,\Delta\theta^2, \qquad \sigma_\theta^2 = \alpha_1\,\Delta\theta^2 + \alpha_2\,\Delta s^2$$ + +Applying Thrun's odometry noise model to the body-frame increment, rather than to a rotate–translate–rotate decomposition, keeps it continuous when the robot reverses. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|---------------------|--------|--------|--------------------------------------------------| +| Integrate one step | $O(1)$ | $O(1)$ | One exponential, one composition, one hypotenuse | +| Increment | $O(1)$ | $O(1)$ | One inverse, one composition, one logarithm | +| Increment deviation | $O(1)$ | $O(1)$ | Two square roots | +| State | — | $O(1)$ | Four scalars | + +## Step-by-Step Walkthrough + +1. **Straight line**: four steps of $(1, 0, 0)$ for $0.25$ s each end at $(1, 0, 0)$. +2. **Quarter arc in one step**: the twist $(0.5, 0, 0.25)$ describes a radius-2 circle. One step of $2\pi$ s lands exactly at $(2, 2, \pi/2)$. +3. **Full circle**: seven steps of $8\pi/7$ s, or a thousand steps of $8\pi/1000$ s, both close the circle back at the origin. +4. **Increment**: starting at $(1, 2, 0.3)$ and integrating $(0.4, 0.1, -0.6)$ for $0.5$ s, the recovered increment is $(0.2, 0.05, -0.3)$. +5. **Noise**: with $\alpha = (0.05, 0.01, 0.05, 0.01)$, an increment of $0.5$ m and $0.1$ rad has standard deviations $0.112$ m in position and $0.055$ rad in heading. Driving backwards by the same distance gives the same values. + +## Pitfalls & Edge Cases + +- **Changing twist**: the update is exact only for a constant twist. For a twist that changes during the step, use the mean of the twists at both ends. +- **Slow sampling**: increments through the logarithm wrap if the robot turns more than half a revolution between readings. Sample faster. +- **Unbounded drift**: odometry error grows without limit. Wheel-radius and track-width errors dominate it and must be calibrated in the wheel model, not here. +- **Skipping renormalisation**: long runs without it let the rotation lose unit length and distort every later pose. +- **Zero period**: a step of zero length leaves the pose unchanged. + +## Variants & Generalizations + +- **Gyro-aided odometry**: replace the wheel-derived turn rate with a gyroscope's, which suffers far less from slip. +- **Odometry fusion**: an extended Kalman filter combining wheel and inertial measurements. +- **Three-dimensional odometry**: the same exponential update on SE(3) for legged or aerial platforms. +- **Higher-order integration**: Magnus-series updates for twists that vary within a step. + +## Applications + +- Dead reckoning for differential-drive, car-like, omnidirectional and skid-steer robots. +- The prediction step of landmark, Monte Carlo and scan-matching localization. +- Short-term pose tracking between absolute fixes such as GNSS or visual markers. + +## Connections to Other Algorithms + +- The SE(2) exponential, logarithm and composition come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Thrun, S., Burgard, W. and Fox, D. (2005). *Probabilistic Robotics*. MIT Press. Chapter 5. +- Borenstein, J. and Feng, L. (1996). "Measurement and correction of systematic odometry errors in mobile robots." *IEEE Transactions on Robotics and Automation*, 12(6), 869–880. +- Solà, J., Deray, J. and Atchuthan, D. (2018). "A micro Lie theory for state estimation in robotics." arXiv:1812.01537. diff --git a/doc/mobile/PurePursuit.md b/doc/mobile/PurePursuit.md new file mode 100644 index 0000000..800c5a3 --- /dev/null +++ b/doc/mobile/PurePursuit.md @@ -0,0 +1,98 @@ +# Pure-Pursuit Path Tracking + +## Overview & Motivation + +A mobile robot that must follow a planned path needs a steering law that turns position errors into a smooth command. **Pure pursuit** (Coulter, 1992) is the simplest robust choice and still the default in many navigation stacks: pick the point one **look-ahead distance** ahead on the path, then drive the circular arc that reaches it. + +The output is a **curvature**, so the same tracker serves a unicycle ($\omega = v\kappa$) and a car-like vehicle ($\delta = \arctan(L\kappa)$). Its behaviour is governed by one intuitive parameter. A short look-ahead tracks tightly but oscillates at speed, and a long one is smooth but cuts corners. + +The tracker works on a bounded **planar polyline path**, stored with cumulative arc lengths, which supports closest-point projection and interpolation by arc length. + +## Mathematical Theory + +### Planar Path + +A path of waypoints $p_0, \ldots, p_{K}$ stores the cumulative arc lengths $s_0 = 0$, $s_{k+1} = s_k + \|p_{k+1} - p_k\|$. The projection of a query point onto segment $k$, with unit direction $u_k$, is + +$$t = \operatorname{clamp}\left((q - p_k)\cdot u_k,\; 0,\; \ell_k\right), \qquad c = p_k + t\,u_k$$ + +Its signed cross-track distance $u_{k,x}(q - p_k)_y - u_{k,y}(q - p_k)_x$ is positive when the query lies left of the path. The point at arc length $s$ is found by binary search over the cumulative lengths, then linear interpolation. + +### Monotone Progress + +The closest point is searched only over a short window of segments starting at the current one, and the progress along the path never decreases. A path that doubles back on itself, such as a U-turn or a parking loop, therefore cannot capture the robot onto its return leg. + +### Look-Ahead Distance + +The look-ahead is scheduled on speed: + +$$L = \operatorname{clamp}\left(L_{\min} + g\,|v|,\; L_{\min},\; L_{\max}\right)$$ + +### Goal Point + +The goal is the first point where the path **leaves** the circle $\|x - q\| = L$ beyond the current progress. On segment $p_k + t\,d$, this is the larger root of + +$$\|d\|^2 t^2 + 2\,(p_k - q)\cdot d\;t + \|p_k - q\|^2 - L^2 = 0, \qquad 0 \le t \le 1$$ + +If no segment crosses the circle, the goal falls back. It is the last waypoint when that lies within the look-ahead, which handles the end of the path. Otherwise it is the path point at $s = \min(\text{progress} + L,\; \text{length})$, which steers a robot that has strayed far from the path back towards it. + +### Steering Arc + +With the goal expressed in the body frame as $b = (b_x, b_y)$, the circle tangent to the current heading that passes through it has curvature + +$$\kappa = \frac{2\,b_y}{b_x^2 + b_y^2}$$ + +The arc does not depend on the direction of travel, so reversing along a path listed in driving order works with a negative speed. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------------|--------------|--------|----------------------------------------------------| +| Path assignment | $O(K)$ | $O(K)$ | Cumulative arc lengths for $K$ waypoints | +| Projection | $O(w)$ | $O(1)$ | $w$ segments in the search window | +| Point at length | $O(\log K)$ | $O(1)$ | Binary search and interpolation | +| Tracker step | $O(K)$ worst | $O(1)$ | Goal search usually ends after one or two segments | + +## Step-by-Step Walkthrough + +The path runs $(0,0) \to (2,0) \to (4,2) \to (4,5)$, with arc lengths $0$, $2$, $4.83$ and $7.83$ m. The tracker uses $L_{\min} = 1$ m, $L_{\max} = 3$ m, gain $0.5$ s and a two-segment search window. + +1. **Projection**: the point $(3.5, 0.5)$ projects onto $(3, 1)$ on the diagonal segment, $3.41$ m along the path, $0.71$ m to its right. +2. **Goal on the current segment**: at $(0.5, -0.2)$ heading east at $1$ m/s, $L = 1.5$ m. The goal is $(1.99, 0)$ and the curvature $2 \cdot 0.2/1.5^2 = 0.178$. +3. **Goal on the next segment**: from $(1.8, 0)$, the circle leaves the path on the diagonal at $(2.96, 0.96)$, which needs curvature $0.85$. +4. **Far off the path**: from $(1, -3)$, the circle does not reach the path, so the goal falls back to $(2, 0)$ with curvature $0.6$. +5. **U-turn**: on a path that runs out and back $1$ m apart, a robot at $(1, 0.6)$ stays on the outbound leg (goal $(1.8, 0)$). An unbounded search would have jumped to the closer return leg. +6. **Closed loop**: a unicycle starting $0.5$ m off the path at $0.8$ m/s reaches within $0.1$ m of the end in under $600$ steps of $20$ ms. + +## Pitfalls & Edge Cases + +- **Look-ahead tuning**: too short oscillates, too long cuts corners. Scale it with speed. +- **Curvature limits**: the commanded curvature may exceed what the vehicle can steer. Saturate it, and slow down on sharp curvature. +- **Steering lag**: a rate-limited steering actuator makes pure pursuit overshoot at speed. Increase the look-ahead or model the actuator. +- **Self-intersecting paths**: an unbounded closest-point search jumps between legs. Keep the search window small and progress monotone. +- **Degenerate paths**: fewer than two points, or repeated points, are rejected when the path is assigned. + +## Variants & Generalizations + +- **Regulated pure pursuit**: scales speed down with curvature and proximity to obstacles (Macenski et al., 2023). +- **Adaptive look-ahead**: look-ahead scheduled on curvature as well as speed. +- **Vector pursuit**: also uses the path heading at the goal point. +- **Stanley and model-predictive control**: alternatives for higher speeds and tighter tracking. + +## Applications + +- Global-path following in indoor and outdoor navigation stacks. +- Agricultural row following and autonomous-vehicle low-speed manoeuvring. +- Teaching geometric path tracking. + +## Connections to Other Algorithms + +- [Differential-Drive Kinematics](DifferentialDrive.md): turns the curvature into wheel speeds with $\omega = v\kappa$. +- [Car-Like Kinematics](CarLikeKinematics.md): turns the curvature into a steering angle, with steering limits. +- [Planar Odometry](PlanarOdometry.md): provides the pose the tracker uses. + +## References & Further Reading + +- Coulter, R.C. (1992). *Implementation of the Pure Pursuit Path Tracking Algorithm*. Technical Report CMU-RI-TR-92-01, Carnegie Mellon University. +- Snider, J.M. (2009). *Automatic Steering Methods for Autonomous Automobile Path Tracking*. Technical Report CMU-RI-TR-09-08, Carnegie Mellon University. +- Macenski, S., Singh, S., Martín, F. and Ginés, J. (2023). "Regulated pure pursuit for robot path tracking." *Autonomous Robots*, 47, 685–694. diff --git a/doc/mobile/README.md b/doc/mobile/README.md new file mode 100644 index 0000000..385bd2b --- /dev/null +++ b/doc/mobile/README.md @@ -0,0 +1,16 @@ +# Wheeled Mobile Robots + +Kinematics, odometry and path tracking for ground vehicles moving in the plane. + +## Algorithms + +| Algorithm | Description | +|------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Planar Odometry](PlanarOdometry.md) | Dead reckoning with the SE(2) exponential: exact arc integration of body twists, body-frame increments and increment noise — $O(1)$ | +| [Differential-Drive Kinematics](DifferentialDrive.md) | Two-wheel unicycle kinematics: forward and inverse maps, curvature-preserving wheel saturation and the speed envelope — $O(1)$ | +| [Car-Like Kinematics](CarLikeKinematics.md) | Kinematic bicycle twist at any reference point, steering for a curvature, angle and rate limits, and Ackermann wheel angles — $O(1)$ | +| [Omnidirectional Kinematics](OmnidirectionalKinematics.md) | Swedish-wheel and mecanum Jacobian for any layout, least-squares forward kinematics, slip residual and uniform wheel saturation — $O(N)$ | +| [Skid-Steer Kinematics](SkidSteerKinematics.md) | ICR model for skid-steered and tracked vehicles with lateral slip, plus closed-form least-squares identification of the ICRs — $O(1)$ | +| [Pure-Pursuit Path Tracking](PurePursuit.md) | Look-ahead goal on a bounded polyline path with monotone progress and a curvature command for unicycles and cars; includes the planar path type — $O(w)$ | +| [Kanayama Trajectory Tracking](KanayamaController.md) | Lyapunov-stable tracking of a reference pose and velocity for unicycle-type bases, with the error in the robot frame and critically damped gains — $O(1)$ | +| [Stanley Path Tracking](StanleyController.md) | Front-axle cross-track and heading-error steering law for car-like vehicles, with low-speed softening, steering angle and rate limits, and monotone path progress — $O(w)$ | diff --git a/doc/mobile/SkidSteerKinematics.md b/doc/mobile/SkidSteerKinematics.md new file mode 100644 index 0000000..c03aa2c --- /dev/null +++ b/doc/mobile/SkidSteerKinematics.md @@ -0,0 +1,88 @@ +# Skid-Steer Kinematics (Instantaneous Centres of Rotation) + +## Overview & Motivation + +Tracked vehicles and four-wheel skid-steered robots have no steering mechanism. They turn by driving the left and right sides at different speeds, which forces the treads or wheels to **skid** sideways. The ideal differential-drive model assumes rolling without slip, so it over-predicts the turn rate of these vehicles, often by a third or more. + +The **instantaneous centre of rotation (ICR)** model keeps the linear structure of the differential drive but replaces the geometric half-track with identified ICR positions for each tread. It also adds a longitudinal offset of the vehicle's ICR, which produces the lateral slip velocity seen in turns. +The three parameters can be identified from logged driving data with a closed-form least-squares fit. + +## Mathematical Theory + +### Tread ICRs + +A body point $(x, y)$ moves at $(v_x - \omega y,\; v_y + \omega x)$. A tread driven at ground speed $v_t$ slides at $v_x - \omega y - v_t$, which vanishes at the tread's ICR line $y_t = (v_x - v_t)/\omega$. With identified left and right ICR positions $y_L > y_R$, sprocket radius $r$ and tread speeds $v_L = r s_L$, $v_R = r s_R$: + +$$\omega = \frac{v_R - v_L}{y_L - y_R}, \qquad v_x = \frac{v_R\,y_L - v_L\,y_R}{y_L - y_R}, \qquad v_y = -x_{\text{ICR}}\,\omega$$ + +### Inverse Kinematics + +$$s_L = \frac{v_x - \omega\,y_L}{r}, \qquad s_R = \frac{v_x - \omega\,y_R}{r}$$ + +The lateral velocity follows from the ICR offset and cannot be commanded. + +### Ideal Case + +With $y_L = B/2$, $y_R = -B/2$ and $x_{\text{ICR}} = 0$, the model reduces exactly to differential drive. + +### Identification + +Each logged sample, with sprocket speeds and an externally measured twist, gives three linear equations in the three parameters: + +$$y_L\,\omega = v_x - v_L, \qquad y_R\,\omega = v_x - v_R, \qquad x_{\text{ICR}}\,\omega = -v_y$$ + +Their least-squares solution over all samples has a closed form, accumulated in four running sums: + +$$y_L = \frac{\sum \omega(v_x - v_L)}{\sum \omega^2}, \qquad y_R = \frac{\sum \omega(v_x - v_R)}{\sum \omega^2}, \qquad x_{\text{ICR}} = -\frac{\sum \omega\,v_y}{\sum \omega^2}$$ + +Straight driving ($\omega = 0$) carries no information, so the estimate is only produced once $\sum\omega^2$ exceeds a threshold. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-------------------|--------|--------|----------------------------------------| +| Forward / inverse | $O(1)$ | $O(1)$ | A few multiplications and one division | +| Add a sample | $O(1)$ | $O(1)$ | Four running sums | +| Estimate | $O(1)$ | $O(1)$ | Three divisions | +| State | — | $O(1)$ | Three ICR parameters, radius and limit | + +## Step-by-Step Walkthrough + +A tracked vehicle with sprocket radius $0.1$ m and identified ICRs $y_L = 0.30$, $y_R = -0.28$, $x_{\text{ICR}} = 0.05$ m: + +1. **Ideal model**: with a geometric track width of $0.4$ m, sprockets at $(5, 8)$ rad/s would give $(0.65, 0, 0.75)$, matching differential drive. +2. **Identified model**: the same sprockets give $v_x = 0.655$ m/s, $v_y = -0.026$ m/s and $\omega = 0.517$ rad/s. The vehicle turns 31 % slower than the ideal prediction and slips sideways. +3. **Inverse**: $(0.655, 0.517)$ maps back to sprockets at $(5, 8)$ rad/s. +4. **Identification**: five consistent samples covering both turn directions recover $(0.30, -0.28, 0.05)$ exactly. +5. **Straight driving**: samples with equal sprocket speeds leave $\sum\omega^2 = 0$, and no estimate is produced. + +## Pitfalls & Edge Cases + +- **Terrain dependence**: the ICR positions change with surface, load and speed. Identify per terrain and reset the estimator when the surface changes. +- **Measurement source**: the identification needs a twist measured independently of the wheels, from GNSS, motion capture, a total station or scan matching. +- **Excitation**: identification needs turns in both directions. Straight-line logs make the fit ill-posed. +- **Lateral slip in trackers**: path trackers that assume zero lateral velocity see the slip as a disturbance. Odometry should integrate the full twist. +- **Long accumulations**: single-precision sums lose accuracy over millions of samples. Accumulate in blocks or reset periodically. + +## Variants & Generalizations + +- **Speed-dependent ICRs**: parameters scheduled on speed and turn rate. +- **Dynamic skid-steer models**: track–soil interaction forces for heavy vehicles. +- **Online identification**: recursive least squares or a Kalman filter tracking the ICRs while driving. + +## Applications + +- Tracked robots, construction and agricultural machinery. +- Four-wheel skid-steer research platforms and planetary rovers. +- Odometry correction for slippery terrain. + +## Connections to Other Algorithms + +- [Differential-Drive Kinematics](DifferentialDrive.md): the no-slip special case, and the shared curvature-preserving saturation. +- [Planar Odometry](PlanarOdometry.md): integrates the full twist, including lateral slip. + +## References & Further Reading + +- Martínez, J.L., Mandow, A., Morales, J., Pedraza, S. and García-Cerezo, A. (2005). "Approximating kinematics for tracked mobile robots." *International Journal of Robotics Research*, 24(10), 867–878. +- Mandow, A., Martínez, J.L., Morales, J., Blanco, J.L., García-Cerezo, A. and González, J. (2007). "Experimental kinematics for wheeled skid-steer mobile robots." *IEEE/RSJ International Conference on Intelligent Robots and Systems*, 1222–1227. +- Siegwart, R., Nourbakhsh, I.R. and Scaramuzza, D. (2011). *Introduction to Autonomous Mobile Robots*, 2nd ed. MIT Press. Chapter 3. diff --git a/doc/mobile/StanleyController.md b/doc/mobile/StanleyController.md new file mode 100644 index 0000000..8b62630 --- /dev/null +++ b/doc/mobile/StanleyController.md @@ -0,0 +1,92 @@ +# Stanley Path Tracking + +## Overview & Motivation + +The **Stanley** steering law steered Stanford's autonomous car to victory in the 2005 DARPA Grand Challenge. It is a path tracker for front-steered vehicles that measures its error at the **front axle**, the point the steered wheels act on directly. +It then combines two intuitive corrections: turn the wheels parallel to the path, and turn them further towards the path by an amount that depends on the cross-track distance and the speed. + +Pure pursuit chases a look-ahead point and tends to cut corners. Stanley instead uses the path heading at the closest point, so it tracks tightly at moderate speed. For small errors the cross-track distance decays exponentially, at a rate set by one gain and independent of speed. + +## Mathematical Theory + +### Front-Axle Error + +With the rear-axle pose $T$ and wheel base $L$, the front axle is at $p_f = T\,(L, 0)$. Its projection onto the path gives the signed cross-track error $e$ (positive when the axle lies left of the path) and the path heading $\theta_p$. The heading error is + +$$\psi = \operatorname{wrap}(\theta_p - \theta)$$ + +### Steering Law + +With gain $k$ and softening speed $k_s$: + +$$\delta = \psi - \arctan\frac{k\,e}{k_s + v}$$ + +The first term aligns the wheels with the path. The second steers back towards it: an axle left of the path ($e > 0$) steers right. + +### Convergence + +With the wheels aligned to the path ($\psi = 0$), the front axle moves sideways at $\dot{e} = -v \sin\left(\arctan\frac{k e}{v}\right)$. For small errors this is + +$$\dot{e} \approx -k\,e$$ + +so the error decays exponentially at rate $k$, whatever the speed. + +### Softening and Actuator Limits + +Without $k_s$, the arctangent jumps to $\pm\pi/2$ at low speed for any error. A softening near $1$ m/s keeps low-speed steering gentle. The output is clamped to $\pm\delta_{\max}$ and then rate-limited to $\pm\dot{\delta}_{\max}\Delta t$ around the previously applied angle. + +### Monotone Progress + +The closest point is searched over a short window of path segments starting at the current one, and progress along the path never decreases. A path that doubles back on itself cannot capture the vehicle onto its return leg. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|------------------|--------|--------|--------------------------------------------| +| Front-axle error | $O(w)$ | $O(1)$ | Projection over $w$ segments in the window | +| Steering law | $O(1)$ | $O(1)$ | One arctangent, a clamp and a rate limit | +| State | — | $O(1)$ | Path and vehicle references, progress | + +## Step-by-Step Walkthrough + +A car with wheel base $2.5$ m and $\delta_{\max} = 0.6$ rad follows a straight road along the $x$ axis, with $k = 2.5$ s⁻¹, $k_s = 1$ m/s and $v = 5$ m/s: + +1. **Lateral offset**: $0.5$ m left of the road with zero heading error, $\delta = -\arctan(1.25/6) = -0.205$ rad. +2. **Heading error**: on the road but heading $0.2$ rad to the left, the front axle sits at $(2.450, 0.497)$. Both terms steer right, $\delta = -0.2 - \arctan(1.242/6) = -0.404$ rad. +3. **Saturation**: $3$ m right of the road, the law asks for $0.896$ rad, which is clamped to $0.6$ rad. +4. **Rate limit**: with $\dot{\delta}_{\max} = 0.5$ rad/s and a $0.1$ s cycle, the $-0.205$ rad request from rest only reaches $-0.05$ rad. +5. **Standstill**: at $v = 0$ the softening keeps the request finite, $-\arctan(1.25) = -0.896$ rad before the clamp. +6. **Closed loop**: starting $1$ m off the road at $5$ m/s, the cross-track error falls below $1$ mm within $4$ s. + +## Pitfalls & Edge Cases + +- **Rear-axle reference**: measuring the error at the rear axle makes the law unstable at speed. Always use the front axle. +- **Steering rate**: the actuator's slew rate dominates at speed. Feed back the applied steering angle every cycle. +- **Reversing**: the law assumes forward driving. Reverse with the rear axle as reference and a mirrored law, or use pure pursuit. +- **High speed**: the kinematic law ignores tyre slip and steering delay. The original controller adds yaw-rate damping and servo-delay terms. +- **Path discontinuities**: a corner in the polyline makes the heading term jump. Smooth sharp corners or limit the steering rate. + +## Variants & Generalizations + +- **Dynamic Stanley**: yaw-rate damping, steering-servo delay compensation and curvature feedforward for high speed. +- **Rear-axle Stanley**: the mirrored law for reversing. +- **Model-predictive control**: explicit handling of actuator limits and vehicle dynamics. + +## Applications + +- Autonomous-car lane keeping and route following at moderate speed. +- Agricultural vehicles following rows and headland turns. +- Outdoor delivery and inspection robots with car-like steering. + +## Connections to Other Algorithms + +- [Car-Like Kinematics](CarLikeKinematics.md): wheel base, steering angle and rate limits, and the bicycle twist used to simulate the vehicle. +- [Pure-Pursuit Path Tracking](PurePursuit.md): provides the planar path and its projection, and is the look-ahead alternative. +- [Planar Odometry](PlanarOdometry.md): provides the rear-axle pose. +- The angle wrapping comes from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Thrun, S. et al. (2006). "Stanley: The robot that won the DARPA Grand Challenge." *Journal of Field Robotics*, 23(9), 661–692. +- Hoffmann, G.M., Tomlin, C.J., Montemerlo, M. and Thrun, S. (2007). "Autonomous automobile trajectory tracking for off-road driving: controller design, experimental validation and racing." *American Control Conference*, 2296–2301. +- Snider, J.M. (2009). *Automatic Steering Methods for Autonomous Automobile Path Tracking*. Technical Report CMU-RI-TR-09-08, Carnegie Mellon University. diff --git a/doc/planning/DubinsPath.md b/doc/planning/DubinsPath.md new file mode 100644 index 0000000..71d3daa --- /dev/null +++ b/doc/planning/DubinsPath.md @@ -0,0 +1,100 @@ +# Dubins Shortest Paths + +## Overview & Motivation + +A car cannot turn on the spot. Its turning radius is bounded below, and without reversing it can only drive forward. Planning for such a vehicle needs the shortest path between two poses that respects this **curvature bound**, both as a local steering function inside sampling planners and as a distance metric between poses. + +Dubins (1957) proved a remarkable result: the shortest forward-only path between any two planar poses is one of just **six words** of at most three segments. Each segment is either an arc at the minimum turning radius ($C$, turning left $L$ or right $R$) or a straight line ($S$). Every word has a closed-form solution, so the planner evaluates all six and keeps the shortest. + +The same header defines a general **car path** type, a short list of steering segments with signed lengths, which the Reeds–Shepp planner reuses for paths that also reverse. + +## Mathematical Theory + +### Normalised Problem + +With turning radius $\rho$, the problem is scaled to unit radius and rotated so that the goal lies on the positive $x$ axis. With the displacement $\Delta$ between the two positions: + +$$d = \frac{\|\Delta\|}{\rho}, \qquad \theta = \operatorname{atan2}(\Delta_y, \Delta_x), \qquad \alpha = \operatorname{mod}_{2\pi}(\theta_0 - \theta), \qquad \beta = \operatorname{mod}_{2\pi}(\theta_1 - \theta)$$ + +When the two positions coincide, $\theta$ is taken as the start heading. + +### The Six Words + +Each word gives three normalised lengths $(t, p, q)$: two arcs and a straight segment ($CSC$), or three arcs ($CCC$). For LSL, for example: + +$$p = \left\|\left(d + \sin\alpha - \sin\beta,\; \cos\beta - \cos\alpha\right)\right\|, \qquad \tau = \operatorname{atan2}(\cos\beta - \cos\alpha,\; d + \sin\alpha - \sin\beta)$$ + +$$t = \operatorname{mod}_{2\pi}(\tau - \alpha), \qquad q = \operatorname{mod}_{2\pi}(\beta - \tau)$$ + +RSR follows by symmetry. The crossing words LSR and RSL need the two turning circles to be far enough apart: + +$$p^2 = d^2 - 2 + 2\cos(\alpha - \beta) \pm 2d(\sin\alpha + \sin\beta) \ge 0$$ + +The $CCC$ words RLR and LRL need the turning circles close enough for a third circle to touch both: + +$$\cos(2\pi - p) = \frac{6 - d^2 + 2\cos(\alpha - \beta) \pm 2d(\sin\alpha - \sin\beta)}{8}, \qquad |\cos(2\pi - p)| \le 1$$ + +The total length is $\rho\,(t + p + q)$, and the shortest valid word wins. On a tie, the first word in the order LSL, RSR, LSR, RSL, RLR, LRL is kept. + +### Single-Precision Boundary Cases + +Goals reachable by a single arc, such as a quarter turn to $(\rho, \rho, \pi/2)$, place the crossing discriminants exactly on zero. In single precision they come out slightly negative. A strict test would then reject the correct word and return a path five times too long. +The existence tests therefore accept a tolerance of $10^{-5}$ (in units of $\rho$), and angles within $10^{-5}$ of $2\pi$ snap to zero rather than adding a spurious full loop. + +### Sampling a Car Path + +A segment of signed length $\ell$ and curvature $\kappa \in \{1/\rho, 0, -1/\rho\}$ moves the pose by the SE(2) exponential of the twist $\operatorname{sign}(\ell)\,(1, 0, \kappa)$ over $|\ell|$. Reversing flips the yaw rate too. Sampling at arc length $s$ composes whole segments, then the remaining part of the current one. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|---------------|--------|--------|------------------------------------------------------| +| Shortest path | $O(1)$ | $O(1)$ | Six closed forms, about twenty trigonometric calls | +| Sample | $O(1)$ | $O(1)$ | At most one SE(2) exponential per segment | +| Car path | — | $O(1)$ | Up to five segments with a steering tag and a length | + +## Step-by-Step Walkthrough + +From the origin facing along $x$, with $\rho = 1$ m: + +1. **LSL**: to $(4, 4, \pi/2)$, a $45°$ left arc, $3\sqrt{2} = 4.243$ m straight and another $45°$ arc, $5.813$ m in total. +2. **RSL**: to $(3, -2, \pi/3)$, a right arc of $1.380$ rad, $0.897$ m straight and a left arc of $2.427$ rad, $4.703$ m in total. Mirroring the goal to $(3, 2, -\pi/3)$ gives LSR with the same lengths. +3. **RLR**: a goal close by at $(0.2, 0.6, -2.5)$ needs three arcs, $0.752$, $4.975$ and $0.439$, $6.167$ m in total. +4. **Turning on the spot**: a quarter turn without moving needs LRL, $0.424 + 5.560 + 0.424 = 6.409$ m. The best $CSC$ word, LSL, would take $9.268$ m. +5. **Float boundary**: the single quarter arc to $(1, 1, \pi/2)$ has length $\pi/2$. A strict existence test in single precision would return $7.854$ m instead. +6. **Radius scaling**: with $\rho = 2$ m, the goal $(8, 8, \pi/2)$ costs $11.627$ m, exactly twice the first case. +7. **Sampling**: halfway along the first path, the pose is $(2.207, 1.793)$ heading $\pi/4$, in the middle of the straight segment. + +## Pitfalls & Edge Cases + +- **Discontinuous metric**: a slightly rotated goal can switch the shortest word and add almost a full loop. Planners that need a continuous metric should allow reversing and use Reeds–Shepp paths. +- **Strict float comparisons**: exact zero tests drop valid words on boundary goals. Keep the tolerances. +- **Curvature discontinuity**: the path jumps between $\pm 1/\rho$ and $0$, which a real steering actuator cannot follow. Use clothoid transitions or a slightly larger radius for tracking. +- **Coincident poses**: start equal to goal gives a zero-length path. +- **Obstacles**: Dubins paths ignore obstacles. Use them as the local steering function inside a sampling planner. + +## Variants & Generalizations + +- **Reeds–Shepp paths**: shortest paths when reversing is allowed, with 48 words. +- **Continuous-curvature paths**: clothoid arcs that respect a bounded steering rate. +- **Dubins airplane**: an altitude dimension with a climb-rate bound. +- **Dubins traveling salesman**: tours through many poses for fixed-wing aircraft and survey vehicles. + +## Applications + +- Steering function and distance metric for RRT* and lattice planners of car-like robots. +- Path planning for fixed-wing aircraft, boats and agricultural vehicles that cannot reverse. +- Lower bounds and heuristics for planning with nonholonomic constraints. + +## Connections to Other Algorithms + +- [RRT-Connect](RrtConnect.md): a sampling planner where Dubins paths can replace straight-line steering. +- [Car-Like Kinematics](../mobile/CarLikeKinematics.md): the vehicle model with the minimum turning radius. +- [Pure-Pursuit Path Tracking](../mobile/PurePursuit.md): tracks a sampled Dubins path. +- The SE(2) exponential used for sampling comes from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Dubins, L.E. (1957). "On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents." *American Journal of Mathematics*, 79(3), 497–516. +- Shkel, A.M. and Lumelsky, V. (2001). "Classification of the Dubins set." *Robotics and Autonomous Systems*, 34(4), 179–202. +- LaValle, S.M. (2006). *Planning Algorithms*. Cambridge University Press. Chapter 15.3. diff --git a/doc/planning/DynamicWindowApproach.md b/doc/planning/DynamicWindowApproach.md new file mode 100644 index 0000000..d8c83d2 --- /dev/null +++ b/doc/planning/DynamicWindowApproach.md @@ -0,0 +1,103 @@ +# Dynamic Window Approach + +## Overview & Motivation + +A global planner gives a mobile robot a route through a known map, but people, carts and furniture that moved since the map was built must be avoided on the fly. The **dynamic window approach** (Fox, Burgard and Thrun, 1997) is a classic reactive local planner for this. It runs every control cycle and directly selects the next velocity command. + +It searches in **velocity space** rather than in position space. Only the speeds and turn rates the motors can reach within one control period are considered (the *dynamic window*), so every candidate is physically achievable. Each candidate is then checked against the obstacles along its circular arc. +The candidates the robot could still stop for are scored on progress towards the goal, distance to obstacles and speed, and the best one is commanded. + +## Mathematical Theory + +### Dynamic Window + +From the current speed $v_0$ and turn rate $\omega_0$, with acceleration limits $a$ and $\alpha$ and control period $\Delta t$, the reachable velocities are + +$$v \in \left[\max(v_{\min}, v_0 - a\Delta t),\; \min(v_{\max}, v_0 + a\Delta t)\right], \qquad \omega \in \left[\max(-\omega_{\max}, \omega_0 - \alpha\Delta t),\; \min(\omega_{\max}, \omega_0 + \alpha\Delta t)\right]$$ + +The window is sampled on a regular $N_v \times N_\omega$ grid that includes its corners. + +### Arc Clearance + +A constant command $(v, \omega)$ moves the robot centre on a circle of radius $R = v/\omega$ about $c = (0, R)$ in the body frame. For a disc robot of radius $r$ and an obstacle point $p$, the clearance is the arc length travelled before the disc first touches $p$. If $p$ stays farther than $r$ from that circle, the clearance is the cap $d_{\max}$. +Otherwise the contact interval about $c$ has half-angle + +$$\Delta = \arccos\frac{R^2 + \|p - c\|^2 - r^2}{2|R|\,\|p - c\|}$$ + +and the clearance is $|R|\,(\varphi - \Delta)$, where $\varphi$ is the angle from the start to the obstacle around $c$ in the direction of travel. A straight command uses $p_x - \sqrt{r^2 - p_y^2}$ for obstacles in its path, and rotation in place sweeps no new area. + +### Admissibility + +A command is admissible if the robot can brake to a stop before the nearest obstacle on its arc: + +$$v \le \sqrt{2\,a\,d(v, \omega)}$$ + +Standstill is always admissible, so no admissible command means the robot is moving too fast to stop. The caller then brakes at full deceleration. + +### Objective + +For each admissible sample, the pose after the prediction time $T_p$ comes from the SE(2) exponential of the twist $(v, 0, \omega)$. The heading term rewards facing the goal from there: + +$$h = \pi - \left|\operatorname{wrap}\left(\operatorname{atan2}(g - p_{T_p}) - \theta_{T_p}\right)\right|$$ + +Heading, clearance and speed are each normalised by their maximum over the admissible samples, and the command maximises + +$$G = w_h\,\hat{h} + w_c\,\hat{d} + w_v\,\hat{v}$$ + +On a tie, the first sample in grid order wins. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|---------------|---------------------|-------------------|-------------------------------------------------| +| Window | $O(1)$ | $O(1)$ | Four clamps | +| Arc clearance | $O(1)$ | $O(1)$ | At most one arccosine and two arctangents | +| Command | $O(N_v N_\omega M)$ | $O(N_v N_\omega)$ | $M$ obstacle points, one exponential per sample | + +An $11 \times 21$ grid against a 360-beam scan is about 83 000 arc tests per cycle. Decimating the scan to the nearest point per sector reduces this considerably. + +## Step-by-Step Walkthrough + +A disc robot of radius $0.2$ m with $v \le 1$ m/s, $|\omega| \le 2$ rad/s, $a = 1$ m/s², $\alpha = 3$ rad/s², a $0.25$ s period, a $1$ s prediction, a $2$ m clearance cap, weights $(1, 1, 0.5)$ and a $5 \times 11$ grid: + +1. **Straight clearance**: driving straight, an obstacle at $(2, 0.1)$ is touched after $2 - \sqrt{0.03} = 1.827$ m. One behind the robot never is. +2. **Turning clearance**: on the unit circle ($v = \omega = 1$), an obstacle at $(1, 1)$ is touched after $\pi/2 - \arccos 0.98 = 1.370$ m. +3. **Window**: at $(0.5, 0)$ the window is $[0.25, 0.75]$ m/s by $[-0.75, 0.75]$ rad/s. Near the limits it is clipped, to $[0.7, 1]$ by $[1.15, 2]$ at $(0.95, 1.9)$. +4. **Free space**: with the goal straight ahead, the robot accelerates to $0.75$ m/s without turning. +5. **Goal to the left**: with the goal at $(0, 3)$, it accelerates and turns left at the window's edge, $(0.75, 0.75)$. +6. **Obstacle slightly left**: a point at $(0.45, 0.05)$ leaves 44 of the 55 samples admissible. The robot slows to $0.375$ m/s and turns right at $0.75$ rad/s. Mirroring the obstacle mirrors the turn. +7. **Too fast to stop**: a wall $0.3$ m ahead admits no command at $0.75$ m/s. From standstill, the robot stays put. + +## Pitfalls & Edge Cases + +- **Local minima**: the planner is reactive and can stall in U-shaped traps. Feed it intermediate goals from a global planner or a path rather than the final goal. +- **Footprint**: a disc that is too tight lets corners scrape. Inflate the radius by a safety margin. +- **Body frame**: obstacle points must be in the body frame. Transform map obstacles with the current pose before each call. +- **Computation budget**: the cost grows with samples times obstacle points. Decimate the scan or shrink the grid on small targets. +- **Turn-rate admissibility**: the original paper also bounds $\omega$ by $\sqrt{2\alpha d}$, which mixes metres and radians. For a disc robot braking along its arc, the speed bound is the one that matters. + +## Variants & Generalizations + +- **Global dynamic window**: adds a navigation function to the objective to escape local minima. +- **Trajectory rollout**: simulates each candidate over the horizon instead of using a constant arc. +- **Velocity obstacles and ORCA**: reciprocal avoidance of moving agents. +- **Timed elastic band and model-predictive control**: optimisation-based local planners. + +## Applications + +- Local planners in indoor navigation stacks for service and logistics robots. +- Reactive avoidance of people in museums, hospitals and offices. +- Fallback obstacle avoidance under a global path tracker. + +## Connections to Other Algorithms + +- [Differential-Drive Kinematics](../mobile/DifferentialDrive.md): turns the selected command into wheel speeds. +- [Grid Search (A* and Dijkstra)](GridSearch.md): provides intermediate goals that avoid local minima. +- [Pure-Pursuit Path Tracking](../mobile/PurePursuit.md): provides a look-ahead point on a planned path to use as the goal. +- The SE(2) exponential and angle wrapping come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Fox, D., Burgard, W. and Thrun, S. (1997). "The dynamic window approach to collision avoidance." *IEEE Robotics & Automation Magazine*, 4(1), 23–33. +- Brock, O. and Khatib, O. (1999). "High-speed navigation using the global dynamic window approach." *IEEE International Conference on Robotics and Automation*, 341–346. +- Siegwart, R., Nourbakhsh, I.R. and Scaramuzza, D. (2011). *Introduction to Autonomous Mobile Robots*, 2nd ed. MIT Press. Chapter 6. diff --git a/doc/planning/README.md b/doc/planning/README.md index f55b416..baac87a 100644 --- a/doc/planning/README.md +++ b/doc/planning/README.md @@ -4,11 +4,14 @@ Geometric queries and planners for collision-free robot motion. ## Algorithms -| Algorithm | Description | -|------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Primitive Distance Queries](PrimitiveDistance.md) | Closed-form closest points and signed distances between points, segments, spheres, capsules and oriented boxes — $O(1)$ | -| [Artificial Potential Fields](PotentialField.md) | Quadratic-then-conic attraction plus Khatib repulsion as a clamped velocity, with local-minimum detection — $O(M)$ | -| [Self-Collision Checking](SelfCollision.md) | Joint-to-joint capsules per link, pairwise signed distances with an allowed-collision matrix and a safety margin — $O(n^2)$ | -| [Convex Collision (GJK and EPA)](ConvexCollision.md) | Distance, witness points, penetration depth and contact normal between spheres, capsules, oriented boxes and convex hulls from their support functions | -| [Grid Search (A* and Dijkstra)](GridSearch.md) | Optimal shortest paths on bounded 2-D/3-D grids with per-cell costs, an indexed binary heap and the admissible octile heuristic | -| [RRT-Connect](RrtConnect.md) | Bidirectional rapidly-exploring random trees in joint space with fixed node pools, injected sampler and validity test, and random shortcutting | +| Algorithm | Description | +|------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Primitive Distance Queries](PrimitiveDistance.md) | Closed-form closest points and signed distances between points, segments, spheres, capsules and oriented boxes — $O(1)$ | +| [Artificial Potential Fields](PotentialField.md) | Quadratic-then-conic attraction plus Khatib repulsion as a clamped velocity, with local-minimum detection — $O(M)$ | +| [Self-Collision Checking](SelfCollision.md) | Joint-to-joint capsules per link, pairwise signed distances with an allowed-collision matrix and a safety margin — $O(n^2)$ | +| [Convex Collision (GJK and EPA)](ConvexCollision.md) | Distance, witness points, penetration depth and contact normal between spheres, capsules, oriented boxes and convex hulls from their support functions | +| [Grid Search (A* and Dijkstra)](GridSearch.md) | Optimal shortest paths on bounded 2-D/3-D grids with per-cell costs, an indexed binary heap and the admissible octile heuristic | +| [RRT-Connect](RrtConnect.md) | Bidirectional rapidly-exploring random trees in joint space with fixed node pools, injected sampler and validity test, and random shortcutting | +| [Dubins Shortest Paths](DubinsPath.md) | Closed-form shortest forward-only paths for a bounded-curvature car from the six Dubins words, with float-safe existence tests and a reusable car path type — $O(1)$ | +| [Dynamic Window Approach](DynamicWindowApproach.md) | Reactive local obstacle avoidance: samples the reachable velocity window, keeps commands that can stop before the closed-form arc clearance and scores heading, clearance and speed — $O(N_v N_\omega M)$ | +| [Reeds–Shepp Shortest Paths](ReedsSheppPath.md) | Closed-form shortest paths for a bounded-curvature car that may reverse, from eight base formulas and their symmetries; continuous in the goal and never longer than Dubins — $O(1)$ | diff --git a/doc/planning/ReedsSheppPath.md b/doc/planning/ReedsSheppPath.md new file mode 100644 index 0000000..fce957c --- /dev/null +++ b/doc/planning/ReedsSheppPath.md @@ -0,0 +1,98 @@ +# Reeds–Shepp Shortest Paths + +## Overview & Motivation + +Dubins paths assume a car that only drives forward. Real cars, forklifts and tractors reverse: parallel parking, three-point turns and docking all rely on it. Reeds and Shepp (1990) extended Dubins' result to a car that may drive **both forwards and backwards** at a bounded curvature. +The shortest path is then one of **48 words** of at most five segments, arcs at the minimum turning radius or straight lines, with at most two **cusps** (changes of direction). + +The extension matters for planning beyond parking. Because the car can back up, a small change of the goal only changes the shortest path a little. The Reeds–Shepp length is therefore **continuous** in the goal pose, and it is never longer than the Dubins length. That makes it the preferred distance metric and local connector for sampling planners of car-like robots. + +## Mathematical Theory + +### Normalised Problem + +The goal is expressed in the start frame and scaled to unit turning radius: + +$$T_{\text{rel}} = T_0^{-1} T_1, \qquad (x, y) = \frac{t_{\text{rel}}}{\rho}, \qquad \varphi = \theta_{\text{rel}}$$ + +Each segment has a signed length: positive drives forwards, negative reverses. The total cost is $\rho \sum_i |\ell_i|$. + +### Base Formulas + +Eight closed-form base formulas cover the path families, named after the paper's equations: two $CSC$ words (8.1 LSL and 8.2 LSR), one $C|C|C$ word (8.3), two $CC|CC$ words (8.7 and 8.8), two $C|C_{\pi/2}SC$ words (8.9 and 8.10) and one $C|C_{\pi/2}SC_{\pi/2}|C$ word (8.11). Here $|$ marks a cusp and $C_{\pi/2}$ a quarter-turn arc. For LSL, for example: + +$$(u, t) = \operatorname{polar}(x - \sin\varphi,\; y - 1 + \cos\varphi), \qquad v = \operatorname{wrap}(\varphi - t)$$ + +The word exists if $t \ge 0$ and $v \ge 0$. Every other formula has a similar existence condition on its intermediate distances and angles. Formulas 8.3 and 8.11 contain typographical errors in the original paper. The corrected forms used here are those of OMPL's implementation. + +### Symmetries + +Each base formula, evaluated at a transformed goal, gives the remaining words: + +| Symmetry | Goal evaluated | Letters | Lengths | +|--------------------------|---------------------|----------|----------| +| Direct | $(x, y, \varphi)$ | as given | as given | +| Time flip | $(-x, y, -\varphi)$ | as given | negated | +| Reflection | $(x, -y, -\varphi)$ | L ↔ R | as given | +| Time flip and reflection | $(-x, -y, \varphi)$ | L ↔ R | negated | + +A time flip drives the same word backwards. A reflection mirrors it across the $x$ axis. The $C|C|C$ and $C|C_{\pi/2}SC$ families also have **backwards** variants: the goal is replaced by $(x\cos\varphi + y\sin\varphi,\; x\sin\varphi - y\cos\varphi,\; \varphi)$, and the letters and lengths are reversed. With all symmetries, 44 candidates are evaluated, and the shortest wins. +On a tie, the first candidate found is kept. + +### Tolerances + +In single precision, existence conditions that hold with equality on boundary goals come out slightly violated. Each condition accepts a tolerance of $10^{-5}$ in units of $\rho$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|---------------|--------|--------|----------------------------------------------------------| +| Shortest path | $O(1)$ | $O(1)$ | 44 closed-form candidates, about 150 trigonometric calls | +| Sample | $O(1)$ | $O(1)$ | At most five SE(2) exponentials | +| Car path | — | $O(1)$ | Up to five segments with signed lengths | + +## Step-by-Step Walkthrough + +From the origin facing along $x$, with $\rho = 1$ m: + +1. **Straight reverse**: to $(-3, 0, 0)$, a single straight segment of $-3$ m. +2. **Forward when shortest**: to $(3, 0.5, 0.2)$, an all-forward LSL of $0.170$, $2.842$ and $0.030$, $3.042$ m in total, as Dubins would give. +3. **Sideways shift**: to $(0, 1, 0)$, a path with forward and reversing arcs and two cusps, $2.636$ m. Dubins needs $7.283$ m. +4. **Reversed quarter turn**: to $(-2, 1, \pi/2)$, an LSRL that reverses $0.730$, $0.236$ and a quarter turn, then drives forward $0.730$, $3.266$ m in total. The best $C|C|C$ alternative is only $0.0006$ m longer. +5. **Three arcs with a cusp**: to $(-1, -1, 0.5)$, LRL with $0.166$ forward, then $0.966$ and $0.632$ in reverse, $1.764$ m in total. +6. **Turning around**: reversing the heading on the spot takes three arcs of $\pi/3$ (forward, reverse, forward), $\pi$ m in total. +7. **Sanity relations**: over 80 goals around a rotated start with $\rho = 1.5$ m, every path is no longer than its Dubins counterpart, has the same length in both directions and reaches its goal. + +## Pitfalls & Edge Cases + +- **Typos in the paper**: implementing equations 8.3 and 8.11 exactly as printed gives paths that miss their goals. Use the corrected forms. +- **Cusps**: every cusp stops the vehicle. The shortest path is not always the fastest, so weigh cusps when time matters. +- **Curvature discontinuity**: arcs and straights join with a jump in curvature. Smooth the path or track it with a margin. +- **Cost**: 44 candidates per query is cheap once, but expensive inside an inner loop that runs millions of times. +- **Obstacles**: the paths ignore obstacles. Check them for collision before use. + +## Variants & Generalizations + +- **Dubins paths**: the forward-only special case with six words. +- **Continuous-curvature Reeds–Shepp**: clothoid transitions that respect a steering-rate limit. +- **Weighted reversing**: penalising backward segments or cusps in the cost. +- **Hybrid A-star**: grid search over car motions with Reeds–Shepp paths as the analytic expansion to the goal. + +## Applications + +- Parking and docking for autonomous cars, forklifts and delivery robots. +- Distance metric and steering function for sampling planners of car-like vehicles. +- Heuristic and analytic goal connection in Hybrid A-star. + +## Connections to Other Algorithms + +- [Dubins Shortest Paths](DubinsPath.md): the forward-only planner, and the car path type both share. +- [RRT-Connect](RrtConnect.md): a sampling planner where Reeds–Shepp paths can replace straight-line steering. +- [Car-Like Kinematics](../mobile/CarLikeKinematics.md): the vehicle model with the minimum turning radius. +- The SE(2) transforms and angle wrapping come from [numerical-toolbox-cpp](https://github.com/embedded-pro/numerical-toolbox-cpp). + +## References & Further Reading + +- Reeds, J.A. and Shepp, L.A. (1990). "Optimal paths for a car that goes both forwards and backwards." *Pacific Journal of Mathematics*, 145(2), 367–393. +- Şucan, I.A., Moll, M. and Kavraki, L.E. (2012). "The Open Motion Planning Library." *IEEE Robotics & Automation Magazine*, 19(4), 72–82. +- LaValle, S.M. (2006). *Planning Algorithms*. Cambridge University Press. Chapter 15.3. diff --git a/roadmap/README.md b/roadmap/README.md index 1b8ebc5..5361f5c 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -23,11 +23,11 @@ All pseudocode respects the library constraints: **no heap**, bounded containers (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. The canonical worked example is -[mobile/PurePursuit](mobile/PurePursuit/implementation.md). +[localization/OdometryFusion](localization/OdometryFusion/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/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 +3-vectors `(vx, vy, ω)` by [math/SE2Transform](https://github.com/embedded-pro/numerical-toolbox-cpp/blob/main/doc/math/SE2Transform.md) (NT66); dynamics and Jacobians reach controllers through interfaces implemented by [dynamics/ChainDynamicsModel](../robotics/dynamics/ChainDynamicsModel.hpp) and [kinematics/JacobianProvider](../robotics/kinematics/JacobianProvider.hpp) (shipped). @@ -88,17 +88,8 @@ prerequisites. ## Index -### `kinematics` -`MobileManipulatorKinematics` (M24) - ### `trajectory` `TimeOptimalPathParameterization` (M27) -### `mobile` -`PlanarOdometry` (F1) · `DifferentialDrive` (W1) · `CarLikeKinematics` (W2) · `OmnidirectionalKinematics` (W3) · `SkidSteerKinematics` (W4) · `PurePursuit` (W5) · `StanleyController` (W6) · `KanayamaController` (W7) - -### `planning` -`DubinsPath` (W8) · `DynamicWindowApproach` (W9) · `ReedsSheppPath` (W10) - ### `localization` -`OdometryFusion` (E1) · `OccupancyGridMap` (E2) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) +`OdometryFusion` (E1) · `LandmarkLocalization` (E3) · `MonteCarloLocalization` (E4) · `ScanMatcher` (E5) diff --git a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md b/roadmap/kinematics/MobileManipulatorKinematics/explanation.md deleted file mode 100644 index 9eee4ec..0000000 --- a/roadmap/kinematics/MobileManipulatorKinematics/explanation.md +++ /dev/null @@ -1,36 +0,0 @@ -# Mobile-Manipulator Kinematics — Overview - -## What it is -Kinematics for an arm mounted on a differential-drive base — a robot that can both reach and roam. It -combines the arm's Jacobian with the base's motion into one map from "drive speed, turn rate and joint -rates" to the tool's linear and angular velocity in the world, respecting that wheels cannot slide -sideways. - -## Why it matters (embedded) -Warehouse, service and field robots must coordinate base and arm as one system. A unified Jacobian -lets a single controller command the whole platform, and the extra freedom of "drive there *and* -reach" gives these robots their large workspace. Computing it on board each cycle needs a compact, -frame-consistent model. - -## How it works (intuition) -The tool velocity is the sum of what the arm does on a stationary base and what the base does while -carrying a frozen arm. The arm's Jacobian is known in the arm's own base frame, so it is first rotated -into the world by the base heading and the mounting orientation. The base contributes two columns: -driving forward translates the tool along the heading, and turning swings it about the vertical axis -through the axle midpoint — the farther the tool from that axis, the faster it moves. Because a -differential drive cannot slip sideways, only these two base motions exist; there is no lateral -column to command. Base plus arm usually provides more freedom than the task needs, and null-space -resolution decides how much each contributes. - -## Key parameters -- **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. - -## Reference -Y. Yamamoto, X. Yun, "Coordinating Locomotion and Manipulation of a Mobile Manipulator," *IEEE Trans. -Automatic Control*, 39(6), 1994. - -## See also -`JacobianProvider` (M8, the arm block), `math::SE3Transform` (NT53, pose composition), `RedundancyResolution` -(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 deleted file mode 100644 index a6f2993..0000000 --- a/roadmap/kinematics/MobileManipulatorKinematics/implementation.md +++ /dev/null @@ -1,78 +0,0 @@ -# Mobile-Manipulator Kinematics (Differential-Drive Base) — Implementation Pseudocode - -> Roadmap ref: #M24 (Tier 4) · Target: `robotics/kinematics` · Namespace `kinematics` · Type: `float` (templated on `T`, instantiated for `float` only) - -## Data structures - -```cpp -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) -# base pose: math::SE2Transform of the axle midpoint in the world frame (NT66) -``` - -## Interface - -```cpp -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, ω) -``` - -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). Wheel speeds for the -`(v, ω)` part come from W1's `DifferentialDrive::Inverse`. - -## Algorithm (pseudocode) - -```cpp -function ToolPose(base, q): - 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 = 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.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 - return J - -function BaseConstraint(φ): # no side-slip: [−sin φ, cos φ, 0]·(ẋ, ẏ, φ̇) = 0 - return [[cos φ, 0], [sin φ, 0], [0, 1]] -``` - -## Complexity & memory - -- `Compute`: one arm `Jacobian` + `ToolPose`, `2·ArmDof` 3×3 rotations, two fixed base columns. -- Memory: one `6 × (2 + ArmDof)` matrix; stack only. - -## Numerical / embedded notes - -- **Frames:** the arm Jacobian is expressed in the arm-base frame; it must be rotated by - `R_wa = Rz(φ)·R_mount` before being stacked with the world-frame base columns (skipping it gives an - error of order ‖J_arm·q̇‖ — 0.52 in the test configuration). No lever-arm correction is needed for the - arm block: its linear rows already describe the tool point. -- **Nonholonomic base:** a differential drive has three planar coordinates but two controls; the base - columns are already the constrained ones, so no lateral-velocity column exists to command. -- Base plus arm is redundant for a 6-DOF task — split the motion with M14 (e.g. arm for fine motion, - base for reach); scale the base columns (`|r|` amplifies yaw) before resolving. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/kinematics/MobileManipulatorKinematics.hpp` — `#pragma once`, - `OPTIMIZE_FOR_SPEED` on `Compute`, and - `extern template class MobileManipulatorKinematics;` / `` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/kinematics/MobileManipulatorKinematics.cpp` → the same instantiations. -- Test: `robotics/kinematics/test/TestMobileManipulatorKinematics.cpp` -- 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`), 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 deleted file mode 100644 index 3d2bde7..0000000 --- a/roadmap/kinematics/MobileManipulatorKinematics/tests.md +++ /dev/null @@ -1,50 +0,0 @@ -# Mobile-Manipulator Kinematics — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -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) } } - 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, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -tool_pose_composes_base_mount_and_arm: - Assert: ToolPose(base, q) = T_wb·mount·arm.ToolPose(q); p ≈ (0.698536, 2.343297, 0.695513) -twist_matches_finite_difference: - Arrange: integrate base (x += εv cos φ, y += εv sin φ, φ += εω) and q += εq̇, ε = 1e-3 - Assert: Compute(base, q)·u ≈ (Δp; Log(ΔR)) / ε (tol 1e-3); - Compute·u ≈ (0.403993, 0.199541, −0.046890; −0.180886, −0.239333, −0.3) -arm_block_is_rotated_into_the_world: - Arrange: u = (0, 0, q̇) - Assert: columns 2..4 = blockdiag(R_wa, R_wa)·arm.Jacobian(q), R_wa = Rz(π/6)·Rz(π/2) -forward_drive_moves_tool_along_heading: - Arrange: base φ = 0, u = (1, 0, 0, 0, 0) - Assert: tool twist = ((1, 0, 0); 0) -yaw_moves_tool_by_lever_arm: - Arrange: base φ = 0, u = (0, 1, 0, 0, 0) - Assert: linear = ẑ × r, ‖linear‖ = ‖r_xy‖ = 0.456874; angular = ẑ -base_constraint_blocks_sideslip: - Assert: (−sin φ, cos φ, 0)·BaseConstraint(φ)·(v, ω) = 0 for φ ∈ {0, π/6, π} -``` - -## Reference vectors - -- Fixture: tool world `p = (0.698536, 2.343297, 0.695513)`, `r = (−0.301464, 0.343297, 0.695513)`. -- `J·u` agrees with the finite difference to `3.6e-8` (ε = 1e-6) / `3.6e-5` (ε = 1e-3) in double; - stacking the unrotated arm block instead gives an error of `0.516`. - -## Edge cases - -- Tool above the axle midpoint (`r_xy = 0`) ⇒ yaw adds no tool translation. -- Heading wrap `φ = ±π` ⇒ columns continuous. -- Redundancy split (base vs arm) is M14's job, not tested here. diff --git a/roadmap/localization/OccupancyGridMap/explanation.md b/roadmap/localization/OccupancyGridMap/explanation.md deleted file mode 100644 index 10ddbf5..0000000 --- a/roadmap/localization/OccupancyGridMap/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# Occupancy-Grid Mapping — Overview - -## What it is -A map of the robot's surroundings as a grid of small square cells, each holding how likely it is to be -occupied. Every laser or sonar beam updates the cells it passes through (probably free) and the cell where -it hit something (probably occupied). - -## Why it matters (embedded) -Occupancy grids are the standard map for indoor robots: they are what localization matches scans -against, what planners search, and what a robot builds when it explores. The update is pure integer -stepping and additions, so mapping keeps up with the sensor on a small processor. The fixed grid size -makes memory use known in advance. - -## How it works (intuition) -Each cell stores the log of the odds that it is occupied, so combining evidence is just adding. A beam is -traced from the sensor's cell to the cell where it ended with Bresenham's line algorithm. Every cell on -the way gets a small negative amount (the beam passed through, so it is likely free), and the end cell -gets a positive amount (something reflected it). Beams that reached the sensor's maximum range found -nothing, so they only clear. Values are clamped, so a cell that has been seen as free many times can -still become occupied when a door closes. - -## Key parameters -- **resolution** and **origin** — cell size and where the grid sits in the world. -- **occupied / free increments** — how much one beam changes a cell. -- **clamp limits** and **maximum range**. - -## Reference -A. Elfes, "Using occupancy grids for mobile robot perception and navigation," *Computer*, 22(6), 1989; -S. Thrun, W. Burgard, D. Fox, *Probabilistic Robotics* (2005), Ch. 9; J. E. Bresenham, "Algorithm for -computer control of a digital plotter," *IBM Systems Journal*, 4(1), 1965. - -## See also -`MonteCarloLocalization` (E4, uses the map), `math::SE2Transform` (NT66). diff --git a/roadmap/localization/OccupancyGridMap/implementation.md b/roadmap/localization/OccupancyGridMap/implementation.md deleted file mode 100644 index 11e68ed..0000000 --- a/roadmap/localization/OccupancyGridMap/implementation.md +++ /dev/null @@ -1,98 +0,0 @@ -# Occupancy-Grid Mapping (Log-Odds, Ray Casting) — Implementation Pseudocode - -> Roadmap ref: #E2 (★★★☆☆) · Target: `robotics/localization` · Namespace `localization` · Type: `float` (templated on `T`, instantiated for `float` only) - -Bounded 2-D occupancy grid updated from range scans with known sensor poses. Each beam marks the cells -it crosses as freer and its end cell as more occupied, adding clamped log-odds along a Bresenham ray. -Readings at maximum range only clear space. The grid also serves as the map for Monte Carlo -localization (E4) and as the obstacle source for grid planners (P3). - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct RangeBeam: - T range # metres - T bearing # radians, sensor frame - -template -struct OccupancyGridParameters: - T resolution # metres per cell - math::Vector2 origin # world position of the corner of cell (0, 0) - T logOddsOccupied # added at a hit, e.g. 0.85 (p = 0.70) - T logOddsFree # added on a pass-through, e.g. −0.4 (p = 0.40) - T logOddsMin, logOddsMax # clamp, e.g. [−2, 3.5]: keeps the map able to change - T maxRange # readings ≥ maxRange mark free space only - -struct GridCell { int x; int y; } - -template -class OccupancyGridMap: - std::array logOdds{} # row-major, 0 = unknown (p = 0.5) - OccupancyGridParameters parameters -``` - -## Interface - -```cpp -explicit OccupancyGridMap(const OccupancyGridParameters& parameters) -void Integrate(const math::SE2Transform& sensorPose, std::span> beams) # hot path -GridCell WorldToCell(const math::Vector2& point) const # floor((p − origin)/resolution) -math::Vector2 CellCenter(GridCell cell) const -bool Contains(GridCell cell) const -T LogOdds(GridCell cell) const; void SetLogOdds(GridCell cell, T value) # clamped; maps loaded from storage -T Probability(GridCell cell) const # 1 − 1/(1 + e^l) -bool IsOccupied(GridCell cell, T threshold) const # l > threshold (0 ⇔ p > 0.5) -const OccupancyGridParameters& Parameters() const -``` - -## Algorithm (pseudocode) - -```text -function Integrate(pose, beams): # OPTIMIZE_FOR_SPEED - start = WorldToCell(pose.translation) - for each (r, b) in beams: - hit = r < maxRange; ρ = min(r, maxRange) - end = WorldToCell(pose.Apply((ρ cos b, ρ sin b))) - for each cell c on Bresenham(start, end), in order: - if not Contains(c): break # leaves the map: stop the ray - Δ = (c == end and hit) ? logOddsOccupied : logOddsFree - logOdds[c] = clamp(logOdds[c] + Δ, logOddsMin, logOddsMax) - -function Bresenham(a, b): # all octants, integer only - dx = |b.x − a.x|; dy = −|b.y − a.y|; sx = sign(b.x − a.x); sy = sign(b.y − a.y); e = dx + dy - loop: emit a; if a == b: stop - e2 = 2e; if e2 ≥ dy: e += dy, a.x += sx; if e2 ≤ dx: e += dx, a.y += sy -``` - -## Complexity & memory - -- `Integrate`: `O(Σ beam length / resolution)` cell updates, integer stepping only. A 360-beam scan at - 4 m and 5 cm resolution is about 29 k additions. -- Memory: `Width·Height` scalars. 200 × 200 cells (10 m at 5 cm) take 160 KB in `float`; size the grid to - the target. No heap. - -## Numerical / embedded notes - -- **Log odds** turn the Bayesian cell update into an addition. The clamp keeps a cell that has been - observed many times able to change when the world changes (doors, people). -- **Max-range readings** have no reliable hit. Marking their end cell occupied would paint phantom walls, - so they only clear. -- The start cell is the sensor's own cell, which is also cleared: a sensor cannot sit inside an obstacle. -- Bresenham visits one cell per major-axis step; diagonal rays skip corner-touching cells. That is the - standard trade-off; an exact traversal (Amanatides–Woo) is a drop-in replacement for the cell walker. -- Rays that leave the grid stop at its border, and their outside end point is never written. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `robotics/localization/OccupancyGridMap.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `Integrate`, and `extern template class OccupancyGridMap;` under - `#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `robotics/localization/OccupancyGridMap.cpp` → `template class OccupancyGridMap;` -- Test: `robotics/localization/test/TestOccupancyGridMap.cpp` -- Doc: `doc/localization/OccupancyGridMap.md` (per `doc/TEMPLATE.md`) -- CMake: `.hpp` → `target_sources(robotics.localization ...)`; `.cpp` → `robotics_add_coverage_sources`; - test → `robotics.localization_test`. -- Depends on: NT66 (`SE2Transform`, `Vector2`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/localization/OccupancyGridMap/tests.md b/roadmap/localization/OccupancyGridMap/tests.md deleted file mode 100644 index b7212fc..0000000 --- a/roadmap/localization/OccupancyGridMap/tests.md +++ /dev/null @@ -1,52 +0,0 @@ -# Occupancy-Grid Mapping — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestOccupancyGridMap : public ::testing::Test: - OccupancyGridParameters parameters{ 0.1, (0, 0), 0.85, −0.4, −2.0, 3.5, 4.0 } - OccupancyGridMap map{ parameters } # 1 m × 1 m - SE2Transform sensor{ FromPose(0.05, 0.05, 0) } # centre of cell (0, 0) - # NonZeroCells() lists (x, y, logOdds) of every touched cell -# each case below is a TEST_F(TestOccupancyGridMap, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -axis_beam_clears_the_path_and_marks_the_hit: - Act: Integrate(sensor, { (0.5, 0) }) - Assert: cells (0..4, 0) = −0.4; cell (5, 0) = 0.85; nothing else touched -diagonal_and_shallow_beams_follow_bresenham: - Act: Integrate(sensor, { (0.5√2, π/4) }) on one map, Integrate(sensor, { (hypot(0.7, 0.3), atan2(0.3, 0.7)) }) on another - Assert: diagonal: (0,0), (1,1), (2,2), (3,3), (4,4) free, (5,5) occupied - shallow: (0,0), (1,0), (2,1), (3,1), (4,2), (5,2), (6,3) free, (7,3) occupied -max_range_reading_only_clears: - Arrange: parameters.maxRange = 0.5 - Act: Integrate(sensor, { (0.5, 0) }) - Assert: cells (0..5, 0) = −0.4; no occupied cell -ray_leaving_the_map_stops_at_the_border: - Act: Integrate(sensor, { (2.0, 0) }) - Assert: cells (0..9, 0) = −0.4; no occupied cell, no write outside the grid -repeated_hits_clamp_the_log_odds: - Act: 10 × Integrate(sensor, { (0.5, 0) }) - Assert: LogOdds((5, 0)) == 3.5; LogOdds((0, 0)) == −2.0 - Probability((5, 0)) ≈ 0.970688; Probability((0, 0)) ≈ 0.119203 -world_to_cell_respects_the_origin: - Arrange: parameters.origin = (−0.5, −0.5) - Assert: WorldToCell((0, 0)) == (5, 5); WorldToCell((−0.5, −0.5)) == (0, 0); WorldToCell((0.449, −0.051)) == (9, 4) - CellCenter((5, 5)) ≈ (0.05, 0.05) -``` - -## Reference vectors - -- `l = 0.85` ⇔ `p = 0.700567`; `l = −0.4` ⇔ `p = 0.401312`; clamp `[−2, 3.5]` ⇔ `p ∈ [0.119203, 0.970688]`. -- Bresenham `(0,0) → (7,3)`: `(0,0) (1,0) (2,1) (3,1) (4,2) (5,2) (6,3) (7,3)`, the same cells in reverse. - -## Edge cases - -- A beam of range 0 marks only the sensor cell (occupied if `0 < maxRange`, which the caller should - filter as an invalid reading). -- Negative-direction beams (bearing π) walk with `sx = −1` and stop at column 0. diff --git a/roadmap/mobile/CarLikeKinematics/explanation.md b/roadmap/mobile/CarLikeKinematics/explanation.md deleted file mode 100644 index 40c7e93..0000000 --- a/roadmap/mobile/CarLikeKinematics/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# 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 deleted file mode 100644 index bc29e60..0000000 --- a/roadmap/mobile/CarLikeKinematics/implementation.md +++ /dev/null @@ -1,92 +0,0 @@ -# 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 deleted file mode 100644 index 0ce8b6d..0000000 --- a/roadmap/mobile/CarLikeKinematics/tests.md +++ /dev/null @@ -1,44 +0,0 @@ -# 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 deleted file mode 100644 index 39a01c9..0000000 --- a/roadmap/mobile/DifferentialDrive/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# 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 deleted file mode 100644 index 70aba71..0000000 --- a/roadmap/mobile/DifferentialDrive/implementation.md +++ /dev/null @@ -1,85 +0,0 @@ -# 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 deleted file mode 100644 index 4958c15..0000000 --- a/roadmap/mobile/DifferentialDrive/tests.md +++ /dev/null @@ -1,40 +0,0 @@ -# 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 deleted file mode 100644 index e0681a5..0000000 --- a/roadmap/mobile/KanayamaController/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# 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 deleted file mode 100644 index ae32f41..0000000 --- a/roadmap/mobile/KanayamaController/implementation.md +++ /dev/null @@ -1,82 +0,0 @@ -# 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 deleted file mode 100644 index b3adbe6..0000000 --- a/roadmap/mobile/KanayamaController/tests.md +++ /dev/null @@ -1,44 +0,0 @@ -# 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 deleted file mode 100644 index 05dec8d..0000000 --- a/roadmap/mobile/OmnidirectionalKinematics/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# 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 deleted file mode 100644 index 230136b..0000000 --- a/roadmap/mobile/OmnidirectionalKinematics/implementation.md +++ /dev/null @@ -1,87 +0,0 @@ -# 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 deleted file mode 100644 index 7494fb5..0000000 --- a/roadmap/mobile/OmnidirectionalKinematics/tests.md +++ /dev/null @@ -1,45 +0,0 @@ -# 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 deleted file mode 100644 index be97779..0000000 --- a/roadmap/mobile/PlanarOdometry/explanation.md +++ /dev/null @@ -1,30 +0,0 @@ -# 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 deleted file mode 100644 index c91d046..0000000 --- a/roadmap/mobile/PlanarOdometry/implementation.md +++ /dev/null @@ -1,85 +0,0 @@ -# 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) - -template -struct OdometryNoise: # increment noise shared by the localization filters (E3, E4) - T rotationFromRotation, rotationFromTranslation # α₁, α₂: σ_θ² = α₁Δθ² + α₂Δs² - T translationFromTranslation, translationFromRotation # α₃, α₄: σ_xy² = α₃Δs² + α₄Δθ² -``` - -## 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) - -template -math::Vector3 IncrementDeviation(const math::SE2Transform& increment, const OdometryNoise& noise) - # (σ_xy, σ_xy, σ_θ) of the body-frame increment -``` - -## 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 - -function IncrementDeviation(Δ, α): # noise grows with distance and rotation; same for reverse - (dx, dy, dθ) = Δ.Pose(); Δs² = dx² + dy² - σ_xy = √(α₃Δs² + α₄dθ²); σ_θ = √(α₁dθ² + α₂Δs²) - return (σ_xy, σ_xy, σ_θ) -``` - -## 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. -- `IncrementDeviation` is Thrun's odometry noise model applied to the body-frame increment instead of - the rotate–translate–rotate split, whose first rotation jumps by π when the robot reverses. -- 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;` and - `template math::Vector3 IncrementDeviation(...);` -- 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 deleted file mode 100644 index ba78049..0000000 --- a/roadmap/mobile/PlanarOdometry/tests.md +++ /dev/null @@ -1,51 +0,0 @@ -# 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) -increment_deviation_grows_with_distance_and_rotation: - Arrange: α = (0.05, 0.01, 0.05, 0.01) - Assert: IncrementDeviation(FromPose(0.5, 0, 0.1), α) ≈ (0.112250, 0.112250, 0.054772) - IncrementDeviation(FromPose(−0.5, 0, 0), α) == IncrementDeviation(FromPose(0.5, 0, 0), α) -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 deleted file mode 100644 index c3a5629..0000000 --- a/roadmap/mobile/PurePursuit/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# 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 deleted file mode 100644 index cc536a1..0000000 --- a/roadmap/mobile/PurePursuit/implementation.md +++ /dev/null @@ -1,136 +0,0 @@ -# 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 deleted file mode 100644 index cf9ffe2..0000000 --- a/roadmap/mobile/PurePursuit/tests.md +++ /dev/null @@ -1,64 +0,0 @@ -# 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 deleted file mode 100644 index 2b67ca1..0000000 --- a/roadmap/mobile/SkidSteerKinematics/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# 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 deleted file mode 100644 index ca26481..0000000 --- a/roadmap/mobile/SkidSteerKinematics/implementation.md +++ /dev/null @@ -1,106 +0,0 @@ -# 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 deleted file mode 100644 index 7cc8f3d..0000000 --- a/roadmap/mobile/SkidSteerKinematics/tests.md +++ /dev/null @@ -1,43 +0,0 @@ -# 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 deleted file mode 100644 index 00f978a..0000000 --- a/roadmap/mobile/StanleyController/explanation.md +++ /dev/null @@ -1,32 +0,0 @@ -# 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 deleted file mode 100644 index 5449edb..0000000 --- a/roadmap/mobile/StanleyController/implementation.md +++ /dev/null @@ -1,85 +0,0 @@ -# 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 deleted file mode 100644 index 1bbf6ed..0000000 --- a/roadmap/mobile/StanleyController/tests.md +++ /dev/null @@ -1,44 +0,0 @@ -# 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 deleted file mode 100644 index 316d38d..0000000 --- a/roadmap/planning/DubinsPath/explanation.md +++ /dev/null @@ -1,33 +0,0 @@ -# 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 deleted file mode 100644 index a40d8d3..0000000 --- a/roadmap/planning/DubinsPath/implementation.md +++ /dev/null @@ -1,111 +0,0 @@ -# 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 deleted file mode 100644 index 7cbc4c5..0000000 --- a/roadmap/planning/DubinsPath/tests.md +++ /dev/null @@ -1,53 +0,0 @@ -# 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 deleted file mode 100644 index 7174161..0000000 --- a/roadmap/planning/DynamicWindowApproach/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# 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 deleted file mode 100644 index f7031cc..0000000 --- a/roadmap/planning/DynamicWindowApproach/implementation.md +++ /dev/null @@ -1,115 +0,0 @@ -# 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 deleted file mode 100644 index 663be34..0000000 --- a/roadmap/planning/DynamicWindowApproach/tests.md +++ /dev/null @@ -1,52 +0,0 @@ -# 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 deleted file mode 100644 index b35f727..0000000 --- a/roadmap/planning/ReedsSheppPath/explanation.md +++ /dev/null @@ -1,31 +0,0 @@ -# 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 deleted file mode 100644 index 9b726c4..0000000 --- a/roadmap/planning/ReedsSheppPath/implementation.md +++ /dev/null @@ -1,111 +0,0 @@ -# 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 deleted file mode 100644 index 4ea55c3..0000000 --- a/roadmap/planning/ReedsSheppPath/tests.md +++ /dev/null @@ -1,47 +0,0 @@ -# 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. diff --git a/robotics/CMakeLists.txt b/robotics/CMakeLists.txt index 202fc50..04b6cd7 100644 --- a/robotics/CMakeLists.txt +++ b/robotics/CMakeLists.txt @@ -1,5 +1,7 @@ add_subdirectory(controllers) add_subdirectory(dynamics) add_subdirectory(kinematics) +add_subdirectory(localization) +add_subdirectory(mobile) add_subdirectory(planning) add_subdirectory(trajectory) diff --git a/robotics/kinematics/CMakeLists.txt b/robotics/kinematics/CMakeLists.txt index 6ba9f30..7000904 100644 --- a/robotics/kinematics/CMakeLists.txt +++ b/robotics/kinematics/CMakeLists.txt @@ -25,6 +25,7 @@ target_sources(robotics.kinematics PRIVATE InverseKinematics.hpp JacobianProvider.hpp ManipulabilityIndex.hpp + MobileManipulatorKinematics.hpp PoseInverseKinematics.hpp ProductOfExponentials.hpp RedundancyResolution.hpp @@ -41,6 +42,7 @@ robotics_add_coverage_sources(robotics.kinematics GeometricJacobian.cpp InverseKinematics.cpp ManipulabilityIndex.cpp + MobileManipulatorKinematics.cpp PoseInverseKinematics.cpp ProductOfExponentials.cpp RedundancyResolution.cpp diff --git a/robotics/kinematics/MobileManipulatorKinematics.cpp b/robotics/kinematics/MobileManipulatorKinematics.cpp new file mode 100644 index 0000000..d3235f6 --- /dev/null +++ b/robotics/kinematics/MobileManipulatorKinematics.cpp @@ -0,0 +1,7 @@ +#include "robotics/kinematics/MobileManipulatorKinematics.hpp" + +namespace kinematics +{ + template class MobileManipulatorKinematics; + template class MobileManipulatorKinematics; +} diff --git a/robotics/kinematics/MobileManipulatorKinematics.hpp b/robotics/kinematics/MobileManipulatorKinematics.hpp new file mode 100644 index 0000000..282d9bc --- /dev/null +++ b/robotics/kinematics/MobileManipulatorKinematics.hpp @@ -0,0 +1,79 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/kinematics/JacobianProvider.hpp" +#include +#include + +namespace kinematics +{ + template + class MobileManipulatorKinematics + { + static_assert(std::is_floating_point_v, "MobileManipulatorKinematics supports floating-point types"); + static_assert(ArmDof > 0, "Arm must have at least one joint"); + + public: + using Arm = JacobianProvider; + using JointVector = math::Vector; + using Jacobian = math::Matrix; + using Constraint = math::Matrix; + + MobileManipulatorKinematics(const Arm& arm, const math::SE3Transform& mount); + + OPTIMIZE_FOR_SPEED Jacobian Compute(const math::SE2Transform& base, const JointVector& q) const; + math::SE3Transform ToolPose(const math::SE2Transform& base, const JointVector& q) const; + + static Constraint BaseConstraint(T phi); + + private: + const Arm& arm; + math::SE3Transform mount; + }; + + template + MobileManipulatorKinematics::MobileManipulatorKinematics(const Arm& arm, const math::SE3Transform& mount) + : arm{ arm } + , mount{ mount } + {} + + template + OPTIMIZE_FOR_SPEED auto MobileManipulatorKinematics::Compute(const math::SE2Transform& base, const JointVector& q) const -> Jacobian + { + const math::Vector3 up{ T{}, T{}, T{ 1 } }; + const math::Matrix3 armRotation{ base.ToSE3().rotation * mount.rotation }; + const auto armJacobian{ arm.Jacobian(q) }; + const auto tool{ ToolPose(base, q).translation }; + const math::Vector3 lever{ tool[0] - base.translation[0], tool[1] - base.translation[1], tool[2] }; + + Jacobian jacobian{}; + jacobian.SetBlock(math::Vector3{ base.cosine, base.sine, T{} }, 0, 0); + jacobian.SetBlock(math::CrossProduct(up, lever), 0, 1); + jacobian.SetBlock(up, 3, 1); + jacobian.SetBlock(armRotation * armJacobian.template GetBlock<3, ArmDof>(0, 0), 0, 2); + jacobian.SetBlock(armRotation * armJacobian.template GetBlock<3, ArmDof>(3, 0), 3, 2); + return jacobian; + } + + template + math::SE3Transform MobileManipulatorKinematics::ToolPose(const math::SE2Transform& base, const JointVector& q) const + { + return base.ToSE3() * mount * arm.ToolPose(q); + } + + template + auto MobileManipulatorKinematics::BaseConstraint(T phi) -> Constraint + { + return Constraint{ { math::Cos(phi), T{} }, { math::Sin(phi), T{} }, { T{}, T{ 1 } } }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class MobileManipulatorKinematics; + extern template class MobileManipulatorKinematics; +#endif +} diff --git a/robotics/kinematics/test/CMakeLists.txt b/robotics/kinematics/test/CMakeLists.txt index de57d26..4c563a3 100644 --- a/robotics/kinematics/test/CMakeLists.txt +++ b/robotics/kinematics/test/CMakeLists.txt @@ -18,6 +18,7 @@ target_sources(robotics.kinematics_test PRIVATE TestGeometricJacobian.cpp TestInverseKinematics.cpp TestManipulabilityIndex.cpp + TestMobileManipulatorKinematics.cpp TestPoseInverseKinematics.cpp TestProductOfExponentials.cpp TestRedundancyResolution.cpp diff --git a/robotics/kinematics/test/TestMobileManipulatorKinematics.cpp b/robotics/kinematics/test/TestMobileManipulatorKinematics.cpp new file mode 100644 index 0000000..c5f188b --- /dev/null +++ b/robotics/kinematics/test/TestMobileManipulatorKinematics.cpp @@ -0,0 +1,135 @@ +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "numerical/math/SE3Transform.hpp" +#include "robotics/kinematics/ChainTaskJacobian.hpp" +#include "robotics/kinematics/MobileManipulatorKinematics.hpp" +#include +#include +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Matrix3 = math::Matrix3; + using Link = dynamics::RevoluteJointLink; + using Pose = math::SE3Transform; + using Planar = math::SE2Transform; + using Kinematics = kinematics::MobileManipulatorKinematics; + using Command = math::Vector; + + constexpr float pi{ std::numbers::pi_v }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + + Link MakeLink(const Vector3& axis, const Vector3& parentToJoint) + { + return Link{ 1.0f, Matrix3::Identity(), axis, parentToJoint, Vector3{} }; + } + + class TestMobileManipulatorKinematics + : public ::testing::Test + { + protected: + static void ExpectNear(const Vector3& actual, const Vector3& expected, float tolerance) + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + static Vector3 Linear(const math::Vector& twist) + { + return Vector3{ twist[0], twist[1], twist[2] }; + } + + static Vector3 Angular(const math::Vector& twist) + { + return Vector3{ twist[3], twist[4], twist[5] }; + } + + std::array links{ MakeLink(zAxis, Vector3{ 0.0f, 0.0f, 0.1f }), MakeLink(yAxis, Vector3{ 0.0f, 0.0f, 0.2f }), MakeLink(yAxis, Vector3{ 0.3f, 0.0f, 0.0f }) }; + kinematics::ChainTaskJacobian arm{ links, Pose{ Matrix3::Identity(), Vector3{ 0.25f, 0.0f, 0.0f } } }; + Pose mount{ math::RotationAboutAxis(zAxis, pi / 2.0f), Vector3{ 0.1f, 0.0f, 0.3f } }; + Kinematics mobile{ arm, mount }; + Planar base{ Planar::FromPose(1.0f, 2.0f, pi / 6.0f) }; + Planar alongX{ Planar::FromPose(1.0f, 2.0f, 0.0f) }; + Kinematics::JointVector q{ 0.4f, -0.6f, 0.9f }; + Command u{ 0.3f, -0.5f, 0.2f, -0.1f, 0.4f }; + }; +} + +TEST_F(TestMobileManipulatorKinematics, tool_pose_composes_base_mount_and_arm) +{ + const auto tool{ mobile.ToolPose(base, q) }; + const auto composed{ base.ToSE3() * mount * arm.ToolPose(q) }; + + ExpectNear(tool.translation, Vector3{ 0.698536f, 2.343297f, 0.695513f }, 1e-5f); + for (std::size_t r = 0; r < 3; ++r) + for (std::size_t c = 0; c < 3; ++c) + EXPECT_NEAR(tool.rotation.at(r, c), composed.rotation.at(r, c), 1e-6f); +} + +TEST_F(TestMobileManipulatorKinematics, twist_matches_finite_difference) +{ + const float epsilon{ 1e-3f }; + const auto heading{ base.Heading() }; + const auto movedBase{ Planar::FromPose(base.translation[0] + epsilon * u[0] * std::cos(heading), base.translation[1] + epsilon * u[0] * std::sin(heading), heading + epsilon * u[1]) }; + const Kinematics::JointVector movedQ{ q[0] + epsilon * u[2], q[1] + epsilon * u[3], q[2] + epsilon * u[4] }; + const auto before{ mobile.ToolPose(base, q) }; + const auto after{ mobile.ToolPose(movedBase, movedQ) }; + + const auto twist{ mobile.Compute(base, q) * u }; + + ExpectNear(Linear(twist), Vector3{ after.translation - before.translation } * (1.0f / epsilon), 1e-3f); + ExpectNear(Angular(twist), Pose::LogRotation(after.rotation * before.rotation.Transpose()) * (1.0f / epsilon), 1e-3f); + ExpectNear(Linear(twist), Vector3{ 0.403993f, 0.199541f, -0.046890f }, 1e-5f); + ExpectNear(Angular(twist), Vector3{ -0.180886f, -0.239333f, -0.3f }, 1e-5f); +} + +TEST_F(TestMobileManipulatorKinematics, arm_block_is_rotated_into_the_world) +{ + const auto jacobian{ mobile.Compute(base, q) }; + const Matrix3 armRotation{ math::RotationAboutAxis(zAxis, pi / 6.0f) * math::RotationAboutAxis(zAxis, pi / 2.0f) }; + const auto armJacobian{ arm.Jacobian(q) }; + const auto linear{ armRotation * armJacobian.GetBlock<3, 3>(0, 0) }; + const auto angular{ armRotation * armJacobian.GetBlock<3, 3>(3, 0) }; + + for (std::size_t r = 0; r < 3; ++r) + for (std::size_t c = 0; c < 3; ++c) + { + EXPECT_NEAR(jacobian.at(r, c + 2), linear.at(r, c), 1e-5f); + EXPECT_NEAR(jacobian.at(r + 3, c + 2), angular.at(r, c), 1e-5f); + } +} + +TEST_F(TestMobileManipulatorKinematics, forward_drive_moves_tool_along_heading) +{ + const auto twist{ mobile.Compute(alongX, q) * Command{ 1.0f, 0.0f, 0.0f, 0.0f, 0.0f } }; + + ExpectNear(Linear(twist), Vector3{ 1.0f, 0.0f, 0.0f }, 1e-6f); + ExpectNear(Angular(twist), Vector3{}, 1e-6f); +} + +TEST_F(TestMobileManipulatorKinematics, yaw_moves_tool_by_lever_arm) +{ + const auto tool{ mobile.ToolPose(alongX, q).translation }; + const Vector3 lever{ tool[0] - alongX.translation[0], tool[1] - alongX.translation[1], tool[2] }; + + const auto twist{ mobile.Compute(alongX, q) * Command{ 0.0f, 1.0f, 0.0f, 0.0f, 0.0f } }; + + ExpectNear(Linear(twist), math::CrossProduct(zAxis, lever), 1e-6f); + EXPECT_NEAR(std::hypot(twist[0], twist[1], twist[2]), 0.456874f, 1e-5f); + ExpectNear(Angular(twist), zAxis, 1e-6f); +} + +TEST_F(TestMobileManipulatorKinematics, base_constraint_blocks_sideslip) +{ + const math::Vector command{ 0.7f, -0.3f }; + + for (const float phi : { 0.0f, pi / 6.0f, pi }) + { + const auto planar{ Kinematics::BaseConstraint(phi) * command }; + EXPECT_NEAR(-std::sin(phi) * planar[0] + std::cos(phi) * planar[1], 0.0f, 1e-6f); + } +} diff --git a/robotics/localization/CMakeLists.txt b/robotics/localization/CMakeLists.txt new file mode 100644 index 0000000..7731246 --- /dev/null +++ b/robotics/localization/CMakeLists.txt @@ -0,0 +1,21 @@ +robotics_add_header_library(robotics.localization) + +target_include_directories(robotics.localization ${ROBOTICS_VISIBILITY} + "$" + "$" +) + +target_link_libraries(robotics.localization ${ROBOTICS_VISIBILITY} + infra.util + numerical.math +) + +target_sources(robotics.localization PRIVATE + OccupancyGridMap.hpp +) + +robotics_add_coverage_sources(robotics.localization + OccupancyGridMap.cpp +) + +add_subdirectory(test) diff --git a/robotics/localization/OccupancyGridMap.cpp b/robotics/localization/OccupancyGridMap.cpp new file mode 100644 index 0000000..782a7eb --- /dev/null +++ b/robotics/localization/OccupancyGridMap.cpp @@ -0,0 +1,7 @@ +#include "robotics/localization/OccupancyGridMap.hpp" + +namespace localization +{ + template class OccupancyGridMap; + template class OccupancyGridMap; +} diff --git a/robotics/localization/OccupancyGridMap.hpp b/robotics/localization/OccupancyGridMap.hpp new file mode 100644 index 0000000..8be6d18 --- /dev/null +++ b/robotics/localization/OccupancyGridMap.hpp @@ -0,0 +1,186 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include +#include +#include +#include +#include +#include + +namespace localization +{ + template + struct RangeBeam + { + static_assert(std::is_floating_point_v, "RangeBeam supports floating-point types"); + + T range{}; + T bearing{}; + }; + + template + struct OccupancyGridParameters + { + static_assert(std::is_floating_point_v, "OccupancyGridParameters supports floating-point types"); + + T resolution{}; + math::Vector2 origin{}; + T logOddsOccupied{}; + T logOddsFree{}; + T logOddsMin{}; + T logOddsMax{}; + T maxRange{}; + }; + + struct GridCell + { + int x{}; + int y{}; + + friend bool operator==(const GridCell&, const GridCell&) = default; + }; + + template + class OccupancyGridMap + { + static_assert(std::is_floating_point_v, "OccupancyGridMap supports floating-point types"); + static_assert(Width > 0 && Height > 0, "OccupancyGridMap needs at least one cell"); + + public: + explicit OccupancyGridMap(const OccupancyGridParameters& parameters); + + OPTIMIZE_FOR_SPEED void Integrate(const math::SE2Transform& sensorPose, std::span> beams); + GridCell WorldToCell(const math::Vector2& point) const; + math::Vector2 CellCenter(GridCell cell) const; + bool Contains(GridCell cell) const; + T LogOdds(GridCell cell) const; + void SetLogOdds(GridCell cell, T value); + T Probability(GridCell cell) const; + bool IsOccupied(GridCell cell, T threshold) const; + const OccupancyGridParameters& Parameters() const; + + private: + std::size_t Index(GridCell cell) const; + void TraceRay(GridCell cell, GridCell end, bool hit); + + std::array logOdds{}; + OccupancyGridParameters parameters; + }; + + template + OccupancyGridMap::OccupancyGridMap(const OccupancyGridParameters& parameters) + : parameters{ parameters } + { + really_assert(parameters.resolution > T{} && parameters.maxRange > T{}); + really_assert(parameters.logOddsMin <= T{} && parameters.logOddsMax >= T{}); + } + + template + OPTIMIZE_FOR_SPEED void OccupancyGridMap::Integrate(const math::SE2Transform& sensorPose, std::span> beams) + { + const auto start{ WorldToCell(sensorPose.translation) }; + for (const auto& beam : beams) + { + const bool hit{ beam.range < parameters.maxRange }; + const T range{ std::min(beam.range, parameters.maxRange) }; + const auto end{ WorldToCell(sensorPose.Apply(math::Vector2{ range * math::Cos(beam.bearing), range * math::Sin(beam.bearing) })) }; + TraceRay(start, end, hit); + } + } + + template + GridCell OccupancyGridMap::WorldToCell(const math::Vector2& point) const + { + return GridCell{ static_cast(math::Floor((point[0] - parameters.origin[0]) / parameters.resolution)), + static_cast(math::Floor((point[1] - parameters.origin[1]) / parameters.resolution)) }; + } + + template + math::Vector2 OccupancyGridMap::CellCenter(GridCell cell) const + { + return math::Vector2{ parameters.origin[0] + (static_cast(cell.x) + T{ 0.5f }) * parameters.resolution, + parameters.origin[1] + (static_cast(cell.y) + T{ 0.5f }) * parameters.resolution }; + } + + template + bool OccupancyGridMap::Contains(GridCell cell) const + { + return cell.x >= 0 && cell.y >= 0 && static_cast(cell.x) < Width && static_cast(cell.y) < Height; + } + + template + T OccupancyGridMap::LogOdds(GridCell cell) const + { + return logOdds[Index(cell)]; + } + + template + void OccupancyGridMap::SetLogOdds(GridCell cell, T value) + { + logOdds[Index(cell)] = std::clamp(value, parameters.logOddsMin, parameters.logOddsMax); + } + + template + T OccupancyGridMap::Probability(GridCell cell) const + { + return T{ 1 } - T{ 1 } / (T{ 1 } + math::Exp(LogOdds(cell))); + } + + template + bool OccupancyGridMap::IsOccupied(GridCell cell, T threshold) const + { + return LogOdds(cell) > threshold; + } + + template + const OccupancyGridParameters& OccupancyGridMap::Parameters() const + { + return parameters; + } + + template + std::size_t OccupancyGridMap::Index(GridCell cell) const + { + really_assert(Contains(cell)); + return static_cast(cell.y) * Width + static_cast(cell.x); + } + + template + void OccupancyGridMap::TraceRay(GridCell cell, GridCell end, bool hit) + { + const int dx{ std::abs(end.x - cell.x) }; + const int dy{ -std::abs(end.y - cell.y) }; + const int sx{ cell.x < end.x ? 1 : -1 }; + const int sy{ cell.y < end.y ? 1 : -1 }; + int error{ dx + dy }; + + while (Contains(cell)) + { + const bool last{ cell == end }; + SetLogOdds(cell, LogOdds(cell) + (last && hit ? parameters.logOddsOccupied : parameters.logOddsFree)); + if (last) + return; + + const int doubled{ 2 * error }; + if (doubled >= dy) + { + error += dy; + cell.x += sx; + } + if (doubled <= dx) + { + error += dx; + cell.y += sy; + } + } + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class OccupancyGridMap; + extern template class OccupancyGridMap; +#endif +} diff --git a/robotics/localization/test/CMakeLists.txt b/robotics/localization/test/CMakeLists.txt new file mode 100644 index 0000000..12ab4f3 --- /dev/null +++ b/robotics/localization/test/CMakeLists.txt @@ -0,0 +1,12 @@ +add_executable(robotics.localization_test) +emil_build_for(robotics.localization_test BOOL ROBOTICS_TOOLBOX_BUILD_TESTS) +emil_add_test(robotics.localization_test) + +target_link_libraries(robotics.localization_test PUBLIC + gmock_main + robotics.localization +) + +target_sources(robotics.localization_test PRIVATE + TestOccupancyGridMap.cpp +) diff --git a/robotics/localization/test/TestOccupancyGridMap.cpp b/robotics/localization/test/TestOccupancyGridMap.cpp new file mode 100644 index 0000000..a616754 --- /dev/null +++ b/robotics/localization/test/TestOccupancyGridMap.cpp @@ -0,0 +1,118 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/localization/OccupancyGridMap.hpp" +#include +#include +#include +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Point = math::Vector2; + using Beam = localization::RangeBeam; + using Cell = localization::GridCell; + using Map = localization::OccupancyGridMap; + + constexpr float pi{ std::numbers::pi_v }; + + struct ExpectedCell + { + int x; + int y; + float logOdds; + }; + + class TestOccupancyGridMap + : public ::testing::Test + { + protected: + static void Integrate(Map& target, const Transform& pose, Beam beam) + { + const std::array beams{ beam }; + target.Integrate(pose, beams); + } + + static float Expected(std::initializer_list touched, int x, int y) + { + for (const auto& cell : touched) + if (cell.x == x && cell.y == y) + return cell.logOdds; + return 0.0f; + } + + static void ExpectCells(const Map& target, std::initializer_list touched) + { + for (int y = 0; y < 10; ++y) + for (int x = 0; x < 10; ++x) + EXPECT_FLOAT_EQ(target.LogOdds(Cell{ x, y }), Expected(touched, x, y)) << "cell (" << x << ", " << y << ")"; + } + + localization::OccupancyGridParameters parameters{ 0.1f, Point{ 0.0f, 0.0f }, 0.85f, -0.4f, -2.0f, 3.5f, 4.0f }; + Map map{ parameters }; + Transform sensor{ Transform::FromPose(0.05f, 0.05f, 0.0f) }; + }; +} + +TEST_F(TestOccupancyGridMap, axis_beam_clears_the_path_and_marks_the_hit) +{ + Integrate(map, sensor, Beam{ 0.5f, 0.0f }); + + ExpectCells(map, { { 0, 0, -0.4f }, { 1, 0, -0.4f }, { 2, 0, -0.4f }, { 3, 0, -0.4f }, { 4, 0, -0.4f }, { 5, 0, 0.85f } }); +} + +TEST_F(TestOccupancyGridMap, diagonal_and_shallow_beams_follow_bresenham) +{ + Map shallow{ parameters }; + + Integrate(map, sensor, Beam{ 0.5f * std::numbers::sqrt2_v, pi / 4.0f }); + Integrate(shallow, sensor, Beam{ std::hypot(0.7f, 0.3f), std::atan2(0.3f, 0.7f) }); + + ExpectCells(map, { { 0, 0, -0.4f }, { 1, 1, -0.4f }, { 2, 2, -0.4f }, { 3, 3, -0.4f }, { 4, 4, -0.4f }, { 5, 5, 0.85f } }); + ExpectCells(shallow, { { 0, 0, -0.4f }, { 1, 0, -0.4f }, { 2, 1, -0.4f }, { 3, 1, -0.4f }, { 4, 2, -0.4f }, { 5, 2, -0.4f }, { 6, 3, -0.4f }, { 7, 3, 0.85f } }); +} + +TEST_F(TestOccupancyGridMap, max_range_reading_only_clears) +{ + parameters.maxRange = 0.5f; + Map shortRange{ parameters }; + + Integrate(shortRange, sensor, Beam{ 0.5f, 0.0f }); + + ExpectCells(shortRange, { { 0, 0, -0.4f }, { 1, 0, -0.4f }, { 2, 0, -0.4f }, { 3, 0, -0.4f }, { 4, 0, -0.4f }, { 5, 0, -0.4f } }); +} + +TEST_F(TestOccupancyGridMap, ray_leaving_the_map_stops_at_the_border) +{ + Integrate(map, sensor, Beam{ 2.0f, 0.0f }); + + ExpectCells(map, { { 0, 0, -0.4f }, { 1, 0, -0.4f }, { 2, 0, -0.4f }, { 3, 0, -0.4f }, { 4, 0, -0.4f }, { 5, 0, -0.4f }, { 6, 0, -0.4f }, { 7, 0, -0.4f }, { 8, 0, -0.4f }, { 9, 0, -0.4f } }); +} + +TEST_F(TestOccupancyGridMap, repeated_hits_clamp_the_log_odds) +{ + for (std::size_t k = 0; k < 10; ++k) + Integrate(map, sensor, Beam{ 0.5f, 0.0f }); + + EXPECT_EQ(map.LogOdds(Cell{ 5, 0 }), 3.5f); + EXPECT_EQ(map.LogOdds(Cell{ 0, 0 }), -2.0f); + EXPECT_NEAR(map.Probability(Cell{ 5, 0 }), 0.970688f, 1e-6f); + EXPECT_NEAR(map.Probability(Cell{ 0, 0 }), 0.119203f, 1e-6f); + EXPECT_TRUE(map.IsOccupied(Cell{ 5, 0 }, 0.0f)); + EXPECT_FALSE(map.IsOccupied(Cell{ 0, 0 }, 0.0f)); +} + +TEST_F(TestOccupancyGridMap, world_to_cell_respects_the_origin) +{ + parameters.origin = Point{ -0.5f, -0.5f }; + const Map centred{ parameters }; + + EXPECT_EQ(centred.WorldToCell(Point{ 0.0f, 0.0f }), (Cell{ 5, 5 })); + EXPECT_EQ(centred.WorldToCell(Point{ -0.5f, -0.5f }), (Cell{ 0, 0 })); + EXPECT_EQ(centred.WorldToCell(Point{ 0.449f, -0.051f }), (Cell{ 9, 4 })); + const auto centre{ centred.CellCenter(Cell{ 5, 5 }) }; + EXPECT_NEAR(centre[0], 0.05f, 1e-6f); + EXPECT_NEAR(centre[1], 0.05f, 1e-6f); + EXPECT_EQ(centred.Parameters().origin[0], -0.5f); +} diff --git a/robotics/mobile/CMakeLists.txt b/robotics/mobile/CMakeLists.txt new file mode 100644 index 0000000..11b5fbc --- /dev/null +++ b/robotics/mobile/CMakeLists.txt @@ -0,0 +1,37 @@ +robotics_add_header_library(robotics.mobile) + +target_include_directories(robotics.mobile ${ROBOTICS_VISIBILITY} + "$" + "$" +) + +target_link_libraries(robotics.mobile ${ROBOTICS_VISIBILITY} + infra.util + numerical.math +) + +target_sources(robotics.mobile PRIVATE + CarLikeKinematics.hpp + DifferentialDrive.hpp + KanayamaController.hpp + OmnidirectionalKinematics.hpp + PlanarOdometry.hpp + PlanarPath.hpp + PurePursuit.hpp + SkidSteerKinematics.hpp + StanleyController.hpp +) + +robotics_add_coverage_sources(robotics.mobile + CarLikeKinematics.cpp + DifferentialDrive.cpp + KanayamaController.cpp + OmnidirectionalKinematics.cpp + PlanarOdometry.cpp + PlanarPath.cpp + PurePursuit.cpp + SkidSteerKinematics.cpp + StanleyController.cpp +) + +add_subdirectory(test) diff --git a/robotics/mobile/CarLikeKinematics.cpp b/robotics/mobile/CarLikeKinematics.cpp new file mode 100644 index 0000000..1dbb194 --- /dev/null +++ b/robotics/mobile/CarLikeKinematics.cpp @@ -0,0 +1,6 @@ +#include "robotics/mobile/CarLikeKinematics.hpp" + +namespace mobile +{ + template class CarLikeKinematics; +} diff --git a/robotics/mobile/CarLikeKinematics.hpp b/robotics/mobile/CarLikeKinematics.hpp new file mode 100644 index 0000000..7af1638 --- /dev/null +++ b/robotics/mobile/CarLikeKinematics.hpp @@ -0,0 +1,117 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include +#include + +namespace mobile +{ + template + struct CarLikeParameters + { + static_assert(std::is_floating_point_v, "CarLikeParameters supports floating-point types"); + + T wheelBase{}; + T trackWidth{}; + T maxSteer{}; + T maxSteerRate{}; + }; + + template + struct AckermannAngles + { + static_assert(std::is_floating_point_v, "AckermannAngles supports floating-point types"); + + T left{}; + T right{}; + }; + + template + class CarLikeKinematics + { + static_assert(std::is_floating_point_v, "CarLikeKinematics supports floating-point types"); + + public: + explicit CarLikeKinematics(const CarLikeParameters& parameters); + + OPTIMIZE_FOR_SPEED math::Vector3 Forward(T v, T steer) const; + math::Vector3 TwistAt(T v, T steer, T offset) const; + T Curvature(T steer) const; + T SteerForCurvature(T curvature) const; + T LimitSteer(T requested, T previous, T dt) const; + AckermannAngles Ackermann(T steer) const; + T MinTurningRadius() const; + const CarLikeParameters& Parameters() const; + + private: + CarLikeParameters parameters; + }; + + template + CarLikeKinematics::CarLikeKinematics(const CarLikeParameters& parameters) + : parameters{ parameters } + { + really_assert(parameters.wheelBase > T{} && parameters.trackWidth >= T{} && parameters.maxSteer > T{} && parameters.maxSteerRate > T{}); + really_assert(parameters.maxSteer < math::Atan2(T{ 2 } * parameters.wheelBase, parameters.trackWidth)); + } + + template + OPTIMIZE_FOR_SPEED math::Vector3 CarLikeKinematics::Forward(T v, T steer) const + { + return math::Vector3{ v, T{}, v * Curvature(steer) }; + } + + template + math::Vector3 CarLikeKinematics::TwistAt(T v, T steer, T offset) const + { + const T omega{ v * Curvature(steer) }; + return math::Vector3{ v, offset * omega, omega }; + } + + template + T CarLikeKinematics::Curvature(T steer) const + { + return math::Tan(steer) / parameters.wheelBase; + } + + template + T CarLikeKinematics::SteerForCurvature(T curvature) const + { + return std::clamp(math::Atan(parameters.wheelBase * curvature), -parameters.maxSteer, parameters.maxSteer); + } + + template + T CarLikeKinematics::LimitSteer(T requested, T previous, T dt) const + { + const T bounded{ std::clamp(requested, -parameters.maxSteer, parameters.maxSteer) }; + const T step{ parameters.maxSteerRate * dt }; + return std::clamp(bounded, previous - step, previous + step); + } + + template + AckermannAngles CarLikeKinematics::Ackermann(T steer) const + { + const T lateral{ parameters.wheelBase * math::Tan(steer) }; + const T halfTrack{ parameters.trackWidth / T{ 2 } * math::Tan(steer) }; + return AckermannAngles{ math::Atan(lateral / (parameters.wheelBase - halfTrack)), math::Atan(lateral / (parameters.wheelBase + halfTrack)) }; + } + + template + T CarLikeKinematics::MinTurningRadius() const + { + return parameters.wheelBase / math::Tan(parameters.maxSteer); + } + + template + const CarLikeParameters& CarLikeKinematics::Parameters() const + { + return parameters; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class CarLikeKinematics; +#endif +} diff --git a/robotics/mobile/DifferentialDrive.cpp b/robotics/mobile/DifferentialDrive.cpp new file mode 100644 index 0000000..662e07f --- /dev/null +++ b/robotics/mobile/DifferentialDrive.cpp @@ -0,0 +1,7 @@ +#include "robotics/mobile/DifferentialDrive.hpp" + +namespace mobile +{ + template class DifferentialDrive; + template WheelSpeeds ScaleToLimit(const WheelSpeeds&, float); +} diff --git a/robotics/mobile/DifferentialDrive.hpp b/robotics/mobile/DifferentialDrive.hpp new file mode 100644 index 0000000..8d79acc --- /dev/null +++ b/robotics/mobile/DifferentialDrive.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include +#include + +namespace mobile +{ + template + struct WheelSpeeds + { + static_assert(std::is_floating_point_v, "WheelSpeeds supports floating-point types"); + + T left{}; + T right{}; + }; + + template + WheelSpeeds ScaleToLimit(const WheelSpeeds& wheels, T maxWheelSpeed) + { + static_assert(std::is_floating_point_v, "ScaleToLimit supports floating-point types"); + + const T largest{ std::max(math::Abs(wheels.left), math::Abs(wheels.right)) }; + if (largest <= maxWheelSpeed) + return wheels; + + const T scale{ maxWheelSpeed / largest }; + return WheelSpeeds{ wheels.left * scale, wheels.right * scale }; + } + + template + class DifferentialDrive + { + static_assert(std::is_floating_point_v, "DifferentialDrive supports floating-point types"); + + public: + DifferentialDrive(T wheelRadius, T trackWidth, T maxWheelSpeed); + + OPTIMIZE_FOR_SPEED math::Vector3 Forward(const WheelSpeeds& wheels) const; + OPTIMIZE_FOR_SPEED WheelSpeeds Inverse(T v, T omega) const; + WheelSpeeds Limit(const WheelSpeeds& wheels) const; + math::Vector3 Achievable(T v, T omega) const; + T MaxLinearSpeed() const; + T MaxAngularSpeed() const; + + private: + T wheelRadius; + T trackWidth; + T maxWheelSpeed; + }; + + template + DifferentialDrive::DifferentialDrive(T wheelRadius, T trackWidth, T maxWheelSpeed) + : wheelRadius{ wheelRadius } + , trackWidth{ trackWidth } + , maxWheelSpeed{ maxWheelSpeed } + { + really_assert(wheelRadius > T{} && trackWidth > T{} && maxWheelSpeed > T{}); + } + + template + OPTIMIZE_FOR_SPEED math::Vector3 DifferentialDrive::Forward(const WheelSpeeds& wheels) const + { + return math::Vector3{ wheelRadius * (wheels.left + wheels.right) / T{ 2 }, T{}, wheelRadius * (wheels.right - wheels.left) / trackWidth }; + } + + template + OPTIMIZE_FOR_SPEED WheelSpeeds DifferentialDrive::Inverse(T v, T omega) const + { + const T halfTrack{ omega * trackWidth / T{ 2 } }; + return WheelSpeeds{ (v - halfTrack) / wheelRadius, (v + halfTrack) / wheelRadius }; + } + + template + WheelSpeeds DifferentialDrive::Limit(const WheelSpeeds& wheels) const + { + return ScaleToLimit(wheels, maxWheelSpeed); + } + + template + math::Vector3 DifferentialDrive::Achievable(T v, T omega) const + { + return Forward(Limit(Inverse(v, omega))); + } + + template + T DifferentialDrive::MaxLinearSpeed() const + { + return wheelRadius * maxWheelSpeed; + } + + template + T DifferentialDrive::MaxAngularSpeed() const + { + return T{ 2 } * wheelRadius * maxWheelSpeed / trackWidth; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class DifferentialDrive; + extern template WheelSpeeds ScaleToLimit(const WheelSpeeds&, float); +#endif +} diff --git a/robotics/mobile/KanayamaController.cpp b/robotics/mobile/KanayamaController.cpp new file mode 100644 index 0000000..4d1ea45 --- /dev/null +++ b/robotics/mobile/KanayamaController.cpp @@ -0,0 +1,6 @@ +#include "robotics/mobile/KanayamaController.hpp" + +namespace mobile +{ + template class KanayamaController; +} diff --git a/robotics/mobile/KanayamaController.hpp b/robotics/mobile/KanayamaController.hpp new file mode 100644 index 0000000..334f455 --- /dev/null +++ b/robotics/mobile/KanayamaController.hpp @@ -0,0 +1,79 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include + +namespace mobile +{ + template + struct KanayamaGains + { + static_assert(std::is_floating_point_v, "KanayamaGains supports floating-point types"); + + T kx{}; + T ky{}; + T ktheta{}; + }; + + template + struct UnicycleCommand + { + static_assert(std::is_floating_point_v, "UnicycleCommand supports floating-point types"); + + T v{}; + T omega{}; + }; + + template + class KanayamaController + { + static_assert(std::is_floating_point_v, "KanayamaController supports floating-point types"); + + public: + explicit KanayamaController(const KanayamaGains& gains); + + OPTIMIZE_FOR_SPEED UnicycleCommand Compute(const math::SE2Transform& pose, const math::SE2Transform& reference, T referenceSpeed, T referenceTurnRate) const; + + static math::Vector3 TrackingError(const math::SE2Transform& pose, const math::SE2Transform& reference); + static KanayamaGains CriticallyDamped(T kx, T ky); + + private: + KanayamaGains gains; + }; + + template + KanayamaController::KanayamaController(const KanayamaGains& gains) + : gains{ gains } + { + really_assert(gains.kx > T{} && gains.ky > T{} && gains.ktheta > T{}); + } + + template + OPTIMIZE_FOR_SPEED UnicycleCommand KanayamaController::Compute(const math::SE2Transform& pose, const math::SE2Transform& reference, T referenceSpeed, T referenceTurnRate) const + { + const auto error{ TrackingError(pose, reference) }; + const T v{ referenceSpeed * math::Cos(error[2]) + gains.kx * error[0] }; + const T omega{ referenceTurnRate + referenceSpeed * (gains.ky * error[1] + gains.ktheta * math::Sin(error[2])) }; + return UnicycleCommand{ v, omega }; + } + + template + math::Vector3 KanayamaController::TrackingError(const math::SE2Transform& pose, const math::SE2Transform& reference) + { + const auto error{ pose.Inverse() * reference }; + return math::Vector3{ error.translation[0], error.translation[1], error.Heading() }; + } + + template + KanayamaGains KanayamaController::CriticallyDamped(T kx, T ky) + { + return KanayamaGains{ kx, ky, T{ 2 } * math::Sqrt(ky) }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class KanayamaController; +#endif +} diff --git a/robotics/mobile/OmnidirectionalKinematics.cpp b/robotics/mobile/OmnidirectionalKinematics.cpp new file mode 100644 index 0000000..cbe5ded --- /dev/null +++ b/robotics/mobile/OmnidirectionalKinematics.cpp @@ -0,0 +1,7 @@ +#include "robotics/mobile/OmnidirectionalKinematics.hpp" + +namespace mobile +{ + template class OmnidirectionalKinematics; + template class OmnidirectionalKinematics; +} diff --git a/robotics/mobile/OmnidirectionalKinematics.hpp b/robotics/mobile/OmnidirectionalKinematics.hpp new file mode 100644 index 0000000..2767aea --- /dev/null +++ b/robotics/mobile/OmnidirectionalKinematics.hpp @@ -0,0 +1,144 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CholeskyDecomposition.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "numerical/math/TriangularSolve.hpp" +#include +#include +#include +#include + +namespace mobile +{ + template + struct SwedishWheel + { + static_assert(std::is_floating_point_v, "SwedishWheel supports floating-point types"); + + math::Vector2 position{}; + T driveAngle{}; + T rollerAngle{}; + T radius{}; + }; + + template + class OmnidirectionalKinematics + { + static_assert(std::is_floating_point_v, "OmnidirectionalKinematics supports floating-point types"); + static_assert(NumWheels >= 3, "At least three wheels are needed to span the planar twist"); + + public: + using WheelVector = math::Vector; + using Jacobian = math::Matrix; + using Wheels = std::array, NumWheels>; + + OmnidirectionalKinematics(const Wheels& wheels, T maxWheelSpeed); + + OPTIMIZE_FOR_SPEED WheelVector Inverse(const math::Vector3& twist) const; + OPTIMIZE_FOR_SPEED math::Vector3 Forward(const WheelVector& speeds) const; + WheelVector Limit(const WheelVector& speeds) const; + T SlipResidual(const WheelVector& speeds) const; + const Jacobian& WheelJacobian() const; + + private: + static Jacobian BuildJacobian(const Wheels& wheels); + static math::Matrix BuildPseudoInverse(const Jacobian& jacobian); + + Jacobian jacobian; + math::Matrix pseudoInverse; + T maxWheelSpeed; + }; + + template + OmnidirectionalKinematics::OmnidirectionalKinematics(const Wheels& wheels, T maxWheelSpeed) + : jacobian{ BuildJacobian(wheels) } + , pseudoInverse{ BuildPseudoInverse(jacobian) } + , maxWheelSpeed{ maxWheelSpeed } + {} + + template + OPTIMIZE_FOR_SPEED auto OmnidirectionalKinematics::Inverse(const math::Vector3& twist) const -> WheelVector + { + return jacobian * twist; + } + + template + OPTIMIZE_FOR_SPEED math::Vector3 OmnidirectionalKinematics::Forward(const WheelVector& speeds) const + { + return pseudoInverse * speeds; + } + + template + auto OmnidirectionalKinematics::Limit(const WheelVector& speeds) const -> WheelVector + { + T largest{}; + for (std::size_t i = 0; i < NumWheels; ++i) + largest = std::max(largest, math::Abs(speeds[i])); + + if (largest <= maxWheelSpeed) + return speeds; + return speeds * (maxWheelSpeed / largest); + } + + template + T OmnidirectionalKinematics::SlipResidual(const WheelVector& speeds) const + { + const WheelVector residual{ speeds - jacobian * Forward(speeds) }; + T sum{}; + for (std::size_t i = 0; i < NumWheels; ++i) + sum += residual[i] * residual[i]; + return math::Sqrt(sum); + } + + template + auto OmnidirectionalKinematics::WheelJacobian() const -> const Jacobian& + { + return jacobian; + } + + template + auto OmnidirectionalKinematics::BuildJacobian(const Wheels& wheels) -> Jacobian + { + Jacobian result{}; + for (std::size_t i = 0; i < NumWheels; ++i) + { + const auto& wheel{ wheels[i] }; + const T rollerCosine{ math::Cos(wheel.rollerAngle) }; + really_assert(math::Abs(rollerCosine) > T{ 1e-3f }); + + const T angle{ wheel.driveAngle + wheel.rollerAngle }; + const T scale{ T{ 1 } / (wheel.radius * rollerCosine) }; + result.at(i, 0) = math::Cos(angle) * scale; + result.at(i, 1) = math::Sin(angle) * scale; + result.at(i, 2) = (wheel.position[0] * math::Sin(angle) - wheel.position[1] * math::Cos(angle)) * scale; + } + return result; + } + + template + math::Matrix OmnidirectionalKinematics::BuildPseudoInverse(const Jacobian& jacobian) + { + const auto transposed{ jacobian.Transpose() }; + const auto factor{ math::CholeskyDecomposition::TryFactor(math::SquareMatrix{ transposed * jacobian }) }; + really_assert(factor.has_value()); + + math::Matrix result{}; + for (std::size_t c = 0; c < NumWheels; ++c) + { + const math::Vector3 column{ transposed.at(0, c), transposed.at(1, c), transposed.at(2, c) }; + const auto solved{ math::SolveUpperTriangular(factor->Transpose(), math::SolveLowerTriangular(*factor, column)) }; + for (std::size_t r = 0; r < 3; ++r) + result.at(r, c) = solved[r]; + } + return result; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class OmnidirectionalKinematics; + extern template class OmnidirectionalKinematics; +#endif +} diff --git a/robotics/mobile/PlanarOdometry.cpp b/robotics/mobile/PlanarOdometry.cpp new file mode 100644 index 0000000..8de869a --- /dev/null +++ b/robotics/mobile/PlanarOdometry.cpp @@ -0,0 +1,7 @@ +#include "robotics/mobile/PlanarOdometry.hpp" + +namespace mobile +{ + template class PlanarOdometry; + template math::Vector3 IncrementDeviation(const math::SE2Transform&, const OdometryNoise&); +} diff --git a/robotics/mobile/PlanarOdometry.hpp b/robotics/mobile/PlanarOdometry.hpp new file mode 100644 index 0000000..341bfca --- /dev/null +++ b/robotics/mobile/PlanarOdometry.hpp @@ -0,0 +1,86 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include + +namespace mobile +{ + template + struct OdometryNoise + { + static_assert(std::is_floating_point_v, "OdometryNoise supports floating-point types"); + + T rotationFromRotation{}; + T rotationFromTranslation{}; + T translationFromTranslation{}; + T translationFromRotation{}; + }; + + template + class PlanarOdometry + { + static_assert(std::is_floating_point_v, "PlanarOdometry supports floating-point types"); + + public: + PlanarOdometry() = default; + explicit PlanarOdometry(const math::SE2Transform& initial); + + OPTIMIZE_FOR_SPEED void Integrate(const math::Vector3& twist, T dt); + void Reset(const math::SE2Transform& initial); + const math::SE2Transform& Pose() const; + + static math::Vector3 Increment(const math::SE2Transform& from, const math::SE2Transform& to); + + private: + math::SE2Transform pose{}; + }; + + template + math::Vector3 IncrementDeviation(const math::SE2Transform& increment, const OdometryNoise& noise) + { + static_assert(std::is_floating_point_v, "IncrementDeviation supports floating-point types"); + + const auto delta{ increment.Pose() }; + const T distanceSquared{ delta[0] * delta[0] + delta[1] * delta[1] }; + const T rotationSquared{ delta[2] * delta[2] }; + const T translation{ math::Sqrt(noise.translationFromTranslation * distanceSquared + noise.translationFromRotation * rotationSquared) }; + const T rotation{ math::Sqrt(noise.rotationFromRotation * rotationSquared + noise.rotationFromTranslation * distanceSquared) }; + return math::Vector3{ translation, translation, rotation }; + } + + template + PlanarOdometry::PlanarOdometry(const math::SE2Transform& initial) + : pose{ initial } + {} + + template + OPTIMIZE_FOR_SPEED void PlanarOdometry::Integrate(const math::Vector3& twist, T dt) + { + pose = (pose * math::SE2Transform::Exp(twist, dt)).Normalized(); + } + + template + void PlanarOdometry::Reset(const math::SE2Transform& initial) + { + pose = initial; + } + + template + const math::SE2Transform& PlanarOdometry::Pose() const + { + return pose; + } + + template + math::Vector3 PlanarOdometry::Increment(const math::SE2Transform& from, const math::SE2Transform& to) + { + return (from.Inverse() * to).Log(); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class PlanarOdometry; + extern template math::Vector3 IncrementDeviation(const math::SE2Transform&, const OdometryNoise&); +#endif +} diff --git a/robotics/mobile/PlanarPath.cpp b/robotics/mobile/PlanarPath.cpp new file mode 100644 index 0000000..cfb33bd --- /dev/null +++ b/robotics/mobile/PlanarPath.cpp @@ -0,0 +1,7 @@ +#include "robotics/mobile/PlanarPath.hpp" + +namespace mobile +{ + template class PlanarPath; + template class PlanarPath; +} diff --git a/robotics/mobile/PlanarPath.hpp b/robotics/mobile/PlanarPath.hpp new file mode 100644 index 0000000..2c84ce3 --- /dev/null +++ b/robotics/mobile/PlanarPath.hpp @@ -0,0 +1,171 @@ +#pragma once + +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace mobile +{ + template + struct PathProjection + { + static_assert(std::is_floating_point_v, "PathProjection supports floating-point types"); + + T arcLength{}; + math::Vector2 point{}; + T heading{}; + T crossTrack{}; + std::size_t segment{ 0 }; + }; + + template + class PlanarPath + { + static_assert(std::is_floating_point_v, "PlanarPath supports floating-point types"); + static_assert(MaxPoints >= 2, "A path needs at least two points"); + + public: + using Point2 = math::Vector2; + + bool Assign(std::span waypoints); + std::size_t Size() const; + std::size_t Segments() const; + T Length() const; + const Point2& Point(std::size_t index) const; + T ArcLength(std::size_t index) const; + PathProjection Project(const Point2& query, std::size_t firstSegment, std::size_t segmentWindow) const; + Point2 PointAt(T arcLength) const; + T HeadingAt(T arcLength) const; + + private: + static constexpr T minimumSegment{ T{ 1e-6f } }; + + static T Distance(const Point2& from, const Point2& to); + std::size_t SegmentAt(T arcLength) const; + + std::array points{}; + std::array cumulative{}; + std::size_t count{ 0 }; + }; + + template + bool PlanarPath::Assign(std::span waypoints) + { + if (waypoints.size() < 2 || waypoints.size() > MaxPoints) + return false; + for (std::size_t i = 1; i < waypoints.size(); ++i) + if (Distance(waypoints[i - 1], waypoints[i]) < minimumSegment) + return false; + + count = waypoints.size(); + cumulative[0] = T{}; + for (std::size_t i = 0; i < count; ++i) + { + points[i] = waypoints[i]; + if (i > 0) + cumulative[i] = cumulative[i - 1] + Distance(points[i - 1], points[i]); + } + return true; + } + + template + std::size_t PlanarPath::Size() const + { + return count; + } + + template + std::size_t PlanarPath::Segments() const + { + return count - 1; + } + + template + T PlanarPath::Length() const + { + return cumulative[count - 1]; + } + + template + auto PlanarPath::Point(std::size_t index) const -> const Point2& + { + return points[index]; + } + + template + T PlanarPath::ArcLength(std::size_t index) const + { + return cumulative[index]; + } + + template + PathProjection PlanarPath::Project(const Point2& query, std::size_t firstSegment, std::size_t segmentWindow) const + { + const std::size_t first{ std::min(firstSegment, Segments() - 1) }; + const std::size_t last{ std::min(first + std::max(segmentWindow, 1), Segments()) }; + + PathProjection best{}; + T bestDistance{ std::numeric_limits::infinity() }; + for (std::size_t k = first; k < last; ++k) + { + const Point2 delta{ points[k + 1] - points[k] }; + const T length{ cumulative[k + 1] - cumulative[k] }; + const Point2 direction{ delta * (T{ 1 } / length) }; + const Point2 offset{ query - points[k] }; + const T along{ std::clamp(offset[0] * direction[0] + offset[1] * direction[1], T{}, length) }; + const Point2 closest{ points[k] + direction * along }; + const T distance{ Distance(query, closest) }; + if (distance < bestDistance) + { + bestDistance = distance; + best = PathProjection{ cumulative[k] + along, closest, math::Atan2(direction[1], direction[0]), direction[0] * offset[1] - direction[1] * offset[0], k }; + } + } + return best; + } + + template + auto PlanarPath::PointAt(T arcLength) const -> Point2 + { + if (arcLength <= T{}) + return points[0]; + if (arcLength >= Length()) + return points[count - 1]; + + const std::size_t k{ SegmentAt(arcLength) }; + const T fraction{ (arcLength - cumulative[k]) / (cumulative[k + 1] - cumulative[k]) }; + return Point2{ points[k] + Point2{ points[k + 1] - points[k] } * fraction }; + } + + template + T PlanarPath::HeadingAt(T arcLength) const + { + const std::size_t k{ SegmentAt(std::clamp(arcLength, T{}, Length())) }; + return math::Atan2(points[k + 1][1] - points[k][1], points[k + 1][0] - points[k][0]); + } + + template + T PlanarPath::Distance(const Point2& from, const Point2& to) + { + return math::Hypot(to[0] - from[0], to[1] - from[1]); + } + + template + std::size_t PlanarPath::SegmentAt(T arcLength) const + { + const auto upper{ std::upper_bound(cumulative.begin(), cumulative.begin() + static_cast(count), arcLength) }; + const auto index{ static_cast(std::distance(cumulative.begin(), upper)) }; + return std::min(index == 0 ? 0 : index - 1, Segments() - 1); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class PlanarPath; + extern template class PlanarPath; +#endif +} diff --git a/robotics/mobile/PurePursuit.cpp b/robotics/mobile/PurePursuit.cpp new file mode 100644 index 0000000..5a612ad --- /dev/null +++ b/robotics/mobile/PurePursuit.cpp @@ -0,0 +1,7 @@ +#include "robotics/mobile/PurePursuit.hpp" + +namespace mobile +{ + template class PurePursuit; + template class PurePursuit; +} diff --git a/robotics/mobile/PurePursuit.hpp b/robotics/mobile/PurePursuit.hpp new file mode 100644 index 0000000..8fa6b81 --- /dev/null +++ b/robotics/mobile/PurePursuit.hpp @@ -0,0 +1,141 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "robotics/mobile/PlanarPath.hpp" +#include +#include +#include +#include + +namespace mobile +{ + template + struct PurePursuitParameters + { + static_assert(std::is_floating_point_v, "PurePursuitParameters supports floating-point types"); + + T minLookahead{}; + T maxLookahead{}; + T lookaheadGain{}; + T goalTolerance{}; + std::size_t searchWindow{ 1 }; + }; + + template + struct PursuitCommand + { + static_assert(std::is_floating_point_v, "PursuitCommand supports floating-point types"); + + T curvature{}; + math::Vector2 goal{}; + bool reached{ false }; + }; + + template + class PurePursuit + { + static_assert(std::is_floating_point_v, "PurePursuit supports floating-point types"); + + public: + using Path = PlanarPath; + + PurePursuit(const Path& path, const PurePursuitParameters& parameters); + + OPTIMIZE_FOR_SPEED PursuitCommand Compute(const math::SE2Transform& pose, T speed); + void Restart(); + T Progress() const; + + private: + std::optional> CircleExit(const math::Vector2& centre, T radius) const; + math::Vector2 Goal(const math::Vector2& position, T lookahead) const; + + const Path& path; + PurePursuitParameters parameters; + T progress{}; + std::size_t segment{ 0 }; + }; + + template + PurePursuit::PurePursuit(const Path& path, const PurePursuitParameters& parameters) + : path{ path } + , parameters{ parameters } + {} + + template + OPTIMIZE_FOR_SPEED PursuitCommand PurePursuit::Compute(const math::SE2Transform& pose, T speed) + { + really_assert(path.Size() >= 2); + + const T lookahead{ std::clamp(parameters.minLookahead + parameters.lookaheadGain * math::Abs(speed), parameters.minLookahead, parameters.maxLookahead) }; + const auto& position{ pose.translation }; + const auto projection{ path.Project(position, segment, parameters.searchWindow) }; + if (projection.arcLength >= progress) + { + progress = projection.arcLength; + segment = projection.segment; + } + + const auto goal{ Goal(position, lookahead) }; + const auto body{ pose.Inverse().Apply(goal) }; + const T squared{ body[0] * body[0] + body[1] * body[1] }; + const auto& last{ path.Point(path.Size() - 1) }; + const bool reached{ math::Hypot(position[0] - last[0], position[1] - last[1]) <= parameters.goalTolerance }; + return PursuitCommand{ squared < T{ 1e-12f } ? T{} : T{ 2 } * body[1] / squared, goal, reached }; + } + + template + void PurePursuit::Restart() + { + progress = T{}; + segment = 0; + } + + template + T PurePursuit::Progress() const + { + return progress; + } + + template + std::optional> PurePursuit::CircleExit(const math::Vector2& centre, T radius) const + { + for (std::size_t k = segment; k < path.Segments(); ++k) + { + const auto& start{ path.Point(k) }; + const math::Vector2 delta{ path.Point(k + 1) - start }; + const math::Vector2 offset{ start - centre }; + const T a{ delta[0] * delta[0] + delta[1] * delta[1] }; + const T b{ T{ 2 } * (offset[0] * delta[0] + offset[1] * delta[1]) }; + const T c{ offset[0] * offset[0] + offset[1] * offset[1] - radius * radius }; + const T discriminant{ b * b - T{ 4 } * a * c }; + if (discriminant < T{}) + continue; + + const T t{ (-b + math::Sqrt(discriminant)) / (T{ 2 } * a) }; + if (t >= T{} && t <= T{ 1 } && path.ArcLength(k) + t * math::Sqrt(a) >= progress) + return math::Vector2{ start + delta * t }; + } + return std::nullopt; + } + + template + math::Vector2 PurePursuit::Goal(const math::Vector2& position, T lookahead) const + { + const auto exit{ CircleExit(position, lookahead) }; + if (exit) + return *exit; + + const auto& last{ path.Point(path.Size() - 1) }; + if (math::Hypot(position[0] - last[0], position[1] - last[1]) <= lookahead) + return last; + return path.PointAt(std::min(progress + lookahead, path.Length())); + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class PurePursuit; + extern template class PurePursuit; +#endif +} diff --git a/robotics/mobile/SkidSteerKinematics.cpp b/robotics/mobile/SkidSteerKinematics.cpp new file mode 100644 index 0000000..d0fb4f6 --- /dev/null +++ b/robotics/mobile/SkidSteerKinematics.cpp @@ -0,0 +1,7 @@ +#include "robotics/mobile/SkidSteerKinematics.hpp" + +namespace mobile +{ + template class SkidSteerKinematics; + template class IcrEstimator; +} diff --git a/robotics/mobile/SkidSteerKinematics.hpp b/robotics/mobile/SkidSteerKinematics.hpp new file mode 100644 index 0000000..fe42cc5 --- /dev/null +++ b/robotics/mobile/SkidSteerKinematics.hpp @@ -0,0 +1,147 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "robotics/mobile/DifferentialDrive.hpp" +#include +#include +#include + +namespace mobile +{ + template + struct IcrParameters + { + static_assert(std::is_floating_point_v, "IcrParameters supports floating-point types"); + + T yLeft{}; + T yRight{}; + T xIcr{}; + }; + + template + class SkidSteerKinematics + { + static_assert(std::is_floating_point_v, "SkidSteerKinematics supports floating-point types"); + + public: + static IcrParameters IdealIcr(T trackWidth); + + SkidSteerKinematics(const IcrParameters& icr, T sprocketRadius, T maxSprocketSpeed); + + OPTIMIZE_FOR_SPEED math::Vector3 Forward(const WheelSpeeds& sprockets) const; + OPTIMIZE_FOR_SPEED WheelSpeeds Inverse(T vx, T omega) const; + WheelSpeeds Limit(const WheelSpeeds& sprockets) const; + + private: + IcrParameters icr; + T sprocketRadius; + T maxSprocketSpeed; + }; + + template + class IcrEstimator + { + static_assert(std::is_floating_point_v, "IcrEstimator supports floating-point types"); + + public: + explicit IcrEstimator(T sprocketRadius); + + void Add(const WheelSpeeds& sprockets, const math::Vector3& measuredTwist); + std::optional> Estimate(T minExcitation) const; + void Reset(); + std::size_t Samples() const; + + private: + T sprocketRadius; + T sumOmegaSquared{}; + T sumLeft{}; + T sumRight{}; + T sumLateral{}; + std::size_t samples{ 0 }; + }; + + template + IcrParameters SkidSteerKinematics::IdealIcr(T trackWidth) + { + return IcrParameters{ trackWidth / T{ 2 }, -trackWidth / T{ 2 }, T{} }; + } + + template + SkidSteerKinematics::SkidSteerKinematics(const IcrParameters& icr, T sprocketRadius, T maxSprocketSpeed) + : icr{ icr } + , sprocketRadius{ sprocketRadius } + , maxSprocketSpeed{ maxSprocketSpeed } + { + really_assert(icr.yLeft > icr.yRight && sprocketRadius > T{} && maxSprocketSpeed > T{}); + } + + template + OPTIMIZE_FOR_SPEED math::Vector3 SkidSteerKinematics::Forward(const WheelSpeeds& sprockets) const + { + const T left{ sprocketRadius * sprockets.left }; + const T right{ sprocketRadius * sprockets.right }; + const T spread{ icr.yLeft - icr.yRight }; + const T omega{ (right - left) / spread }; + return math::Vector3{ (right * icr.yLeft - left * icr.yRight) / spread, -icr.xIcr * omega, omega }; + } + + template + OPTIMIZE_FOR_SPEED WheelSpeeds SkidSteerKinematics::Inverse(T vx, T omega) const + { + return WheelSpeeds{ (vx - omega * icr.yLeft) / sprocketRadius, (vx - omega * icr.yRight) / sprocketRadius }; + } + + template + WheelSpeeds SkidSteerKinematics::Limit(const WheelSpeeds& sprockets) const + { + return ScaleToLimit(sprockets, maxSprocketSpeed); + } + + template + IcrEstimator::IcrEstimator(T sprocketRadius) + : sprocketRadius{ sprocketRadius } + {} + + template + void IcrEstimator::Add(const WheelSpeeds& sprockets, const math::Vector3& measuredTwist) + { + const T omega{ measuredTwist[2] }; + sumOmegaSquared += omega * omega; + sumLeft += omega * (measuredTwist[0] - sprocketRadius * sprockets.left); + sumRight += omega * (measuredTwist[0] - sprocketRadius * sprockets.right); + sumLateral += omega * measuredTwist[1]; + ++samples; + } + + template + std::optional> IcrEstimator::Estimate(T minExcitation) const + { + if (sumOmegaSquared < minExcitation) + return std::nullopt; + + return IcrParameters{ sumLeft / sumOmegaSquared, sumRight / sumOmegaSquared, -sumLateral / sumOmegaSquared }; + } + + template + void IcrEstimator::Reset() + { + sumOmegaSquared = T{}; + sumLeft = T{}; + sumRight = T{}; + sumLateral = T{}; + samples = 0; + } + + template + std::size_t IcrEstimator::Samples() const + { + return samples; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class SkidSteerKinematics; + extern template class IcrEstimator; +#endif +} diff --git a/robotics/mobile/StanleyController.cpp b/robotics/mobile/StanleyController.cpp new file mode 100644 index 0000000..3c270d3 --- /dev/null +++ b/robotics/mobile/StanleyController.cpp @@ -0,0 +1,7 @@ +#include "robotics/mobile/StanleyController.hpp" + +namespace mobile +{ + template class StanleyController; + template class StanleyController; +} diff --git a/robotics/mobile/StanleyController.hpp b/robotics/mobile/StanleyController.hpp new file mode 100644 index 0000000..aa35f2e --- /dev/null +++ b/robotics/mobile/StanleyController.hpp @@ -0,0 +1,100 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/AngleUtilities.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "robotics/mobile/CarLikeKinematics.hpp" +#include "robotics/mobile/PlanarPath.hpp" +#include +#include + +namespace mobile +{ + template + struct StanleyParameters + { + static_assert(std::is_floating_point_v, "StanleyParameters supports floating-point types"); + + T gain{}; + T softening{}; + std::size_t searchWindow{ 1 }; + }; + + template + struct StanleyError + { + static_assert(std::is_floating_point_v, "StanleyError supports floating-point types"); + + T crossTrack{}; + T headingError{}; + }; + + template + class StanleyController + { + static_assert(std::is_floating_point_v, "StanleyController supports floating-point types"); + + public: + using Path = PlanarPath; + + StanleyController(const Path& path, const CarLikeKinematics& vehicle, const StanleyParameters& parameters); + + OPTIMIZE_FOR_SPEED T Compute(const math::SE2Transform& rearAxlePose, T speed, T previousSteer, T dt); + StanleyError Error(const math::SE2Transform& rearAxlePose); + void Restart(); + + private: + const Path& path; + const CarLikeKinematics& vehicle; + StanleyParameters parameters; + T progress{}; + std::size_t segment{ 0 }; + }; + + template + StanleyController::StanleyController(const Path& path, const CarLikeKinematics& vehicle, const StanleyParameters& parameters) + : path{ path } + , vehicle{ vehicle } + , parameters{ parameters } + { + really_assert(parameters.gain > T{} && parameters.softening > T{}); + } + + template + OPTIMIZE_FOR_SPEED T StanleyController::Compute(const math::SE2Transform& rearAxlePose, T speed, T previousSteer, T dt) + { + const auto error{ Error(rearAxlePose) }; + const T steer{ error.headingError - math::Atan2(parameters.gain * error.crossTrack, parameters.softening + speed) }; + return vehicle.LimitSteer(steer, previousSteer, dt); + } + + template + StanleyError StanleyController::Error(const math::SE2Transform& rearAxlePose) + { + really_assert(path.Size() >= 2); + + const auto front{ rearAxlePose.Apply(math::Vector2{ vehicle.Parameters().wheelBase, T{} }) }; + const auto projection{ path.Project(front, segment, parameters.searchWindow) }; + if (projection.arcLength >= progress) + { + progress = projection.arcLength; + segment = projection.segment; + } + + return StanleyError{ projection.crossTrack, math::AngleDifference(projection.heading, rearAxlePose.Heading()) }; + } + + template + void StanleyController::Restart() + { + progress = T{}; + segment = 0; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class StanleyController; + extern template class StanleyController; +#endif +} diff --git a/robotics/mobile/test/CMakeLists.txt b/robotics/mobile/test/CMakeLists.txt new file mode 100644 index 0000000..63aa821 --- /dev/null +++ b/robotics/mobile/test/CMakeLists.txt @@ -0,0 +1,20 @@ +add_executable(robotics.mobile_test) +emil_build_for(robotics.mobile_test BOOL ROBOTICS_TOOLBOX_BUILD_TESTS) +emil_add_test(robotics.mobile_test) + +target_link_libraries(robotics.mobile_test PUBLIC + gmock_main + robotics.mobile +) + +target_sources(robotics.mobile_test PRIVATE + TestCarLikeKinematics.cpp + TestDifferentialDrive.cpp + TestKanayamaController.cpp + TestOmnidirectionalKinematics.cpp + TestPlanarOdometry.cpp + TestPlanarPath.cpp + TestPurePursuit.cpp + TestSkidSteerKinematics.cpp + TestStanleyController.cpp +) diff --git a/robotics/mobile/test/TestCarLikeKinematics.cpp b/robotics/mobile/test/TestCarLikeKinematics.cpp new file mode 100644 index 0000000..da4ec05 --- /dev/null +++ b/robotics/mobile/test/TestCarLikeKinematics.cpp @@ -0,0 +1,71 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/mobile/CarLikeKinematics.hpp" +#include +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + + class TestCarLikeKinematics + : public ::testing::Test + { + protected: + static void ExpectNear(const Vector3& actual, const Vector3& expected) + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], 1e-5f); + } + + mobile::CarLikeKinematics car{ { 2.5f, 1.5f, 0.6f, 0.5f } }; + }; +} + +TEST_F(TestCarLikeKinematics, forward_gives_the_rear_axle_twist) +{ + ExpectNear(car.Forward(10.0f, 0.1f), Vector3{ 10.0f, 0.0f, 0.401339f }); +} + +TEST_F(TestCarLikeKinematics, twist_at_offset_matches_the_se2_adjoint) +{ + const auto twist{ car.TwistAt(10.0f, 0.1f, 1.2f) }; + + ExpectNear(twist, Vector3{ 10.0f, 0.481606f, 0.401339f }); + ExpectNear(twist, math::SE2Transform::FromPose(-1.2f, 0.0f, 0.0f).TransformTwist(car.Forward(10.0f, 0.1f))); +} + +TEST_F(TestCarLikeKinematics, steer_for_curvature_inverts_curvature_and_saturates) +{ + EXPECT_NEAR(car.SteerForCurvature(0.1f), 0.244979f, 1e-5f); + EXPECT_NEAR(car.Curvature(0.244979f), 0.1f, 1e-5f); + EXPECT_EQ(car.SteerForCurvature(0.5f), 0.6f); +} + +TEST_F(TestCarLikeKinematics, limit_steer_applies_angle_then_rate) +{ + EXPECT_NEAR(car.LimitSteer(0.5f, 0.1f, 0.2f), 0.2f, 1e-6f); + EXPECT_NEAR(car.LimitSteer(0.9f, 0.55f, 1.0f), 0.6f, 1e-6f); + EXPECT_NEAR(car.LimitSteer(-0.9f, 0.55f, 1.0f), 0.05f, 1e-6f); +} + +TEST_F(TestCarLikeKinematics, ackermann_wheels_share_one_turning_centre) +{ + const auto angles{ car.Ackermann(0.3f) }; + const float radius{ 2.5f / std::tan(0.3f) }; + + EXPECT_NEAR(angles.left, 0.328616f, 1e-5f); + EXPECT_NEAR(angles.right, 0.275851f, 1e-5f); + EXPECT_NEAR(std::tan(angles.left) * (radius - 0.75f), 2.5f, 1e-4f); + EXPECT_NEAR(std::tan(angles.right) * (radius + 0.75f), 2.5f, 1e-4f); + EXPECT_NEAR(car.Ackermann(-0.3f).left, -0.275851f, 1e-5f); + EXPECT_NEAR(car.Ackermann(-0.3f).right, -0.328616f, 1e-5f); + EXPECT_EQ(car.Ackermann(0.0f).left, 0.0f); + EXPECT_EQ(car.Ackermann(0.0f).right, 0.0f); +} + +TEST_F(TestCarLikeKinematics, min_turning_radius_uses_max_steer) +{ + EXPECT_NEAR(car.MinTurningRadius(), 3.654240f, 1e-5f); + EXPECT_EQ(car.Parameters().wheelBase, 2.5f); +} diff --git a/robotics/mobile/test/TestDifferentialDrive.cpp b/robotics/mobile/test/TestDifferentialDrive.cpp new file mode 100644 index 0000000..3ffc397 --- /dev/null +++ b/robotics/mobile/test/TestDifferentialDrive.cpp @@ -0,0 +1,67 @@ +#include "robotics/mobile/DifferentialDrive.hpp" +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Wheels = mobile::WheelSpeeds; + + class TestDifferentialDrive + : public ::testing::Test + { + protected: + static void ExpectNear(const Vector3& actual, const Vector3& expected) + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], 1e-5f); + } + + static void ExpectNear(const Wheels& actual, const Wheels& expected) + { + EXPECT_NEAR(actual.left, expected.left, 1e-4f); + EXPECT_NEAR(actual.right, expected.right, 1e-4f); + } + + mobile::DifferentialDrive drive{ 0.05f, 0.3f, 20.0f }; + }; +} + +TEST_F(TestDifferentialDrive, forward_maps_wheel_speeds_to_a_unicycle_twist) +{ + ExpectNear(drive.Forward(Wheels{ 10.0f, 14.0f }), Vector3{ 0.6f, 0.0f, 0.666667f }); +} + +TEST_F(TestDifferentialDrive, inverse_inverts_forward) +{ + ExpectNear(drive.Inverse(0.6f, 0.666667f), Wheels{ 10.0f, 14.0f }); +} + +TEST_F(TestDifferentialDrive, spin_in_place_counter_rotates_the_wheels) +{ + ExpectNear(drive.Inverse(0.0f, 2.0f), Wheels{ -6.0f, 6.0f }); + ExpectNear(drive.Forward(Wheels{ -6.0f, 6.0f }), Vector3{ 0.0f, 0.0f, 2.0f }); +} + +TEST_F(TestDifferentialDrive, limit_preserves_curvature) +{ + ExpectNear(drive.Inverse(1.2f, 2.0f), Wheels{ 18.0f, 30.0f }); + ExpectNear(drive.Limit(Wheels{ 18.0f, 30.0f }), Wheels{ 12.0f, 20.0f }); + ExpectNear(drive.Achievable(1.2f, 2.0f), Vector3{ 0.8f, 0.0f, 1.333333f }); +} + +TEST_F(TestDifferentialDrive, limit_leaves_feasible_speeds_unchanged) +{ + const auto limited{ drive.Limit(Wheels{ 10.0f, -14.0f }) }; + + EXPECT_EQ(limited.left, 10.0f); + EXPECT_EQ(limited.right, -14.0f); +} + +TEST_F(TestDifferentialDrive, speed_envelope_corners) +{ + EXPECT_NEAR(drive.MaxLinearSpeed(), 1.0f, 1e-5f); + EXPECT_NEAR(drive.MaxAngularSpeed(), 6.666667f, 1e-5f); + ExpectNear(drive.Achievable(1.0f, 0.0f), Vector3{ 1.0f, 0.0f, 0.0f }); + ExpectNear(drive.Achievable(0.0f, 6.666667f), Vector3{ 0.0f, 0.0f, 6.666667f }); +} diff --git a/robotics/mobile/test/TestKanayamaController.cpp b/robotics/mobile/test/TestKanayamaController.cpp new file mode 100644 index 0000000..7afe011 --- /dev/null +++ b/robotics/mobile/test/TestKanayamaController.cpp @@ -0,0 +1,97 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/mobile/KanayamaController.hpp" +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Vector3 = math::Vector3; + using Command = mobile::UnicycleCommand; + + constexpr float pi{ std::numbers::pi_v }; + + class TestKanayamaController + : public ::testing::Test + { + protected: + static void ExpectNear(const Command& actual, const Command& expected) + { + EXPECT_NEAR(actual.v, expected.v, 1e-5f); + EXPECT_NEAR(actual.omega, expected.omega, 1e-5f); + } + + static float Lyapunov(const Vector3& error, float ky) + { + return error[0] * error[0] + error[1] * error[1] + 2.0f * (1.0f - math::Cos(error[2])) / ky; + } + + mobile::KanayamaController controller{ { 1.0f, 4.0f, 4.0f } }; + float vr{ 0.5f }; + }; +} + +TEST_F(TestKanayamaController, zero_error_returns_the_feedforward) +{ + ExpectNear(controller.Compute(Transform::Identity(), Transform::Identity(), vr, 0.1f), Command{ 0.5f, 0.1f }); +} + +TEST_F(TestKanayamaController, longitudinal_error_changes_speed) +{ + ExpectNear(controller.Compute(Transform::Identity(), Transform::FromPose(0.2f, 0.0f, 0.0f), vr, 0.0f), Command{ 0.7f, 0.0f }); +} + +TEST_F(TestKanayamaController, lateral_error_turns_towards_the_reference) +{ + ExpectNear(controller.Compute(Transform::Identity(), Transform::FromPose(0.0f, 0.1f, 0.0f), vr, 0.0f), Command{ 0.5f, 0.2f }); +} + +TEST_F(TestKanayamaController, heading_error_enters_through_sine_and_cosine) +{ + ExpectNear(controller.Compute(Transform::Identity(), Transform::FromPose(0.0f, 0.0f, 0.3f), vr, 0.0f), Command{ 0.477668f, 0.591040f }); +} + +TEST_F(TestKanayamaController, error_is_expressed_in_the_robot_frame) +{ + const auto pose{ Transform::FromPose(1.0f, 1.0f, pi / 2.0f) }; + const auto reference{ Transform::FromPose(1.0f, 2.0f, pi / 2.0f) }; + const auto error{ mobile::KanayamaController::TrackingError(pose, reference) }; + + EXPECT_NEAR(error[0], 1.0f, 1e-5f); + EXPECT_NEAR(error[1], 0.0f, 1e-5f); + EXPECT_NEAR(error[2], 0.0f, 1e-5f); + ExpectNear(controller.Compute(pose, reference, vr, 0.0f), Command{ 1.5f, 0.0f }); +} + +TEST_F(TestKanayamaController, critically_damped_gains) +{ + const auto gains{ mobile::KanayamaController::CriticallyDamped(1.0f, 4.0f) }; + + EXPECT_EQ(gains.kx, 1.0f); + EXPECT_EQ(gains.ky, 4.0f); + EXPECT_EQ(gains.ktheta, 4.0f); +} + +TEST_F(TestKanayamaController, closed_loop_converges_with_a_non_increasing_lyapunov_function) +{ + const float dt{ 0.01f }; + auto pose{ Transform::FromPose(-0.3f, 0.5f, -0.4f) }; + auto reference{ Transform::Identity() }; + float lyapunov{ Lyapunov(mobile::KanayamaController::TrackingError(pose, reference), 4.0f) }; + EXPECT_NEAR(lyapunov, 0.379470f, 1e-5f); + + for (std::size_t k = 0; k < 2000; ++k) + { + const auto command{ controller.Compute(pose, reference, vr, 0.0f) }; + pose = (pose * Transform::Exp(Vector3{ command.v, 0.0f, command.omega }, dt)).Normalized(); + reference = (reference * Transform::Exp(Vector3{ vr, 0.0f, 0.0f }, dt)).Normalized(); + + const float next{ Lyapunov(mobile::KanayamaController::TrackingError(pose, reference), 4.0f) }; + EXPECT_LE(next, lyapunov + 1e-6f); + lyapunov = next; + } + + const auto error{ mobile::KanayamaController::TrackingError(pose, reference) }; + EXPECT_LT(math::Hypot(math::Hypot(error[0], error[1]), error[2]), 1e-4f); +} diff --git a/robotics/mobile/test/TestOmnidirectionalKinematics.cpp b/robotics/mobile/test/TestOmnidirectionalKinematics.cpp new file mode 100644 index 0000000..dd34eee --- /dev/null +++ b/robotics/mobile/test/TestOmnidirectionalKinematics.cpp @@ -0,0 +1,78 @@ +#include "robotics/mobile/OmnidirectionalKinematics.hpp" +#include +#include +#include +#include + +namespace +{ + using Vector2 = math::Vector2; + using Vector3 = math::Vector3; + using Vector4 = math::Vector; + using Wheel = mobile::SwedishWheel; + + constexpr float pi{ std::numbers::pi_v }; + + Wheel KiwiWheel(float degrees) + { + const float angle{ degrees * pi / 180.0f }; + return Wheel{ Vector2{ 0.2f * std::cos(angle), 0.2f * std::sin(angle) }, angle + pi / 2.0f, 0.0f, 0.05f }; + } + + class TestOmnidirectionalKinematics + : public ::testing::Test + { + protected: + template + static void ExpectNear(const math::Matrix& actual, const math::Matrix& expected, float tolerance = 1e-4f) + { + for (std::size_t r = 0; r < Rows; ++r) + for (std::size_t c = 0; c < Cols; ++c) + EXPECT_NEAR(actual.at(r, c), expected.at(r, c), tolerance); + } + + mobile::OmnidirectionalKinematics mecanum{ { Wheel{ Vector2{ 0.2f, 0.15f }, 0.0f, -pi / 4.0f, 0.05f }, Wheel{ Vector2{ 0.2f, -0.15f }, 0.0f, pi / 4.0f, 0.05f }, + Wheel{ Vector2{ -0.2f, 0.15f }, 0.0f, pi / 4.0f, 0.05f }, Wheel{ Vector2{ -0.2f, -0.15f }, 0.0f, -pi / 4.0f, 0.05f } }, + 20.0f }; + mobile::OmnidirectionalKinematics kiwi{ { KiwiWheel(90.0f), KiwiWheel(210.0f), KiwiWheel(330.0f) }, 20.0f }; + }; +} + +TEST_F(TestOmnidirectionalKinematics, mecanum_jacobian_matches_the_closed_form) +{ + ExpectNear(mecanum.WheelJacobian(), math::Matrix{ { 20.0f, -20.0f, -7.0f }, { 20.0f, 20.0f, 7.0f }, { 20.0f, 20.0f, -7.0f }, { 20.0f, -20.0f, 7.0f } }); +} + +TEST_F(TestOmnidirectionalKinematics, inverse_then_forward_recovers_the_twist) +{ + const Vector4 speeds{ 6.5f, 5.5f, -1.5f, 13.5f }; + + ExpectNear(mecanum.Inverse(Vector3{ 0.3f, -0.2f, 0.5f }), speeds); + ExpectNear(mecanum.Forward(speeds), Vector3{ 0.3f, -0.2f, 0.5f }, 1e-5f); + EXPECT_NEAR(mecanum.SlipResidual(speeds), 0.0f, 1e-4f); +} + +TEST_F(TestOmnidirectionalKinematics, forward_is_least_squares_and_flags_slip) +{ + const Vector4 speeds{ 10.0f, 0.0f, 0.0f, 0.0f }; + + ExpectNear(mecanum.Forward(speeds), Vector3{ 0.125f, -0.125f, -0.357143f }, 1e-5f); + EXPECT_NEAR(mecanum.SlipResidual(speeds), 5.0f, 1e-4f); +} + +TEST_F(TestOmnidirectionalKinematics, kiwi_is_exactly_determined) +{ + ExpectNear(kiwi.WheelJacobian(), math::Matrix{ { -20.0f, 0.0f, 4.0f }, { 10.0f, -17.320508f, 4.0f }, { 10.0f, 17.320508f, 4.0f } }); + ExpectNear(kiwi.Forward(Vector3{ 5.0f, 5.0f, 5.0f }), Vector3{ 0.0f, 0.0f, 1.25f }, 1e-5f); + EXPECT_NEAR(kiwi.SlipResidual(Vector3{ 3.0f, -1.0f, 7.0f }), 0.0f, 1e-4f); +} + +TEST_F(TestOmnidirectionalKinematics, limit_scales_all_wheels_uniformly) +{ + const auto speeds{ mecanum.Inverse(Vector3{ 1.0f, 0.5f, 1.0f }) }; + + ExpectNear(speeds, Vector4{ 3.0f, 37.0f, 23.0f, 17.0f }); + ExpectNear(mecanum.Limit(speeds), Vector4{ speeds * (20.0f / 37.0f) }); + ExpectNear(mecanum.Forward(mecanum.Limit(speeds)), Vector3{ 0.540541f, 0.270270f, 0.540541f }, 1e-5f); + ExpectNear(mecanum.Limit(Vector4{ 1.0f, -2.0f, 3.0f, -4.0f }), Vector4{ 1.0f, -2.0f, 3.0f, -4.0f }, 0.0f); +} diff --git a/robotics/mobile/test/TestPlanarOdometry.cpp b/robotics/mobile/test/TestPlanarOdometry.cpp new file mode 100644 index 0000000..aae8399 --- /dev/null +++ b/robotics/mobile/test/TestPlanarOdometry.cpp @@ -0,0 +1,92 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/mobile/PlanarOdometry.hpp" +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Vector3 = math::Vector3; + using Odometry = mobile::PlanarOdometry; + + constexpr float pi{ std::numbers::pi_v }; + + class TestPlanarOdometry + : public ::testing::Test + { + protected: + static void ExpectNear(const Vector3& actual, const Vector3& expected, float tolerance = 1e-5f) + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], tolerance); + } + + Odometry odometry; + Vector3 arc{ 0.5f, 0.0f, 0.25f }; + }; +} + +TEST_F(TestPlanarOdometry, straight_line_accumulates_distance) +{ + for (std::size_t k = 0; k < 4; ++k) + odometry.Integrate(Vector3{ 1.0f, 0.0f, 0.0f }, 0.25f); + + ExpectNear(odometry.Pose().Pose(), Vector3{ 1.0f, 0.0f, 0.0f }); +} + +TEST_F(TestPlanarOdometry, quarter_arc_is_exact_in_one_step) +{ + odometry.Integrate(arc, 2.0f * pi); + + ExpectNear(odometry.Pose().Pose(), Vector3{ 2.0f, 2.0f, pi / 2.0f }); +} + +TEST_F(TestPlanarOdometry, full_circle_closes_independent_of_step_count) +{ + Odometry fine; + + for (std::size_t k = 0; k < 7; ++k) + odometry.Integrate(arc, 8.0f * pi / 7.0f); + for (std::size_t k = 0; k < 1000; ++k) + fine.Integrate(arc, 8.0f * pi / 1000.0f); + + ExpectNear(odometry.Pose().Pose(), Vector3{}, 1e-5f); + ExpectNear(fine.Pose().Pose(), Vector3{}, 1e-4f); +} + +TEST_F(TestPlanarOdometry, increment_recovers_the_integrated_twist) +{ + const auto start{ Transform::FromPose(1.0f, 2.0f, 0.3f) }; + Odometry moving{ start }; + + moving.Integrate(Vector3{ 0.4f, 0.1f, -0.6f }, 0.5f); + + ExpectNear(Odometry::Increment(start, moving.Pose()), Vector3{ 0.2f, 0.05f, -0.3f }); +} + +TEST_F(TestPlanarOdometry, reset_replaces_the_pose) +{ + odometry.Integrate(arc, 1.0f); + + odometry.Reset(Transform::FromPose(-1.0f, 3.0f, 2.0f)); + + ExpectNear(odometry.Pose().Pose(), Vector3{ -1.0f, 3.0f, 2.0f }); +} + +TEST_F(TestPlanarOdometry, increment_deviation_grows_with_distance_and_rotation) +{ + const mobile::OdometryNoise noise{ 0.05f, 0.01f, 0.05f, 0.01f }; + + ExpectNear(mobile::IncrementDeviation(Transform::FromPose(0.5f, 0.0f, 0.1f), noise), Vector3{ 0.112250f, 0.112250f, 0.054772f }); + ExpectNear(mobile::IncrementDeviation(Transform::FromPose(-0.5f, 0.0f, 0.0f), noise), mobile::IncrementDeviation(Transform::FromPose(0.5f, 0.0f, 0.0f), noise), 0.0f); +} + +TEST_F(TestPlanarOdometry, long_runs_keep_a_unit_rotation) +{ + for (std::size_t k = 0; k < 100000; ++k) + odometry.Integrate(arc, 1e-3f); + + const auto& pose{ odometry.Pose() }; + EXPECT_NEAR(pose.cosine * pose.cosine + pose.sine * pose.sine, 1.0f, 1e-6f); +} diff --git a/robotics/mobile/test/TestPlanarPath.cpp b/robotics/mobile/test/TestPlanarPath.cpp new file mode 100644 index 0000000..822c504 --- /dev/null +++ b/robotics/mobile/test/TestPlanarPath.cpp @@ -0,0 +1,73 @@ +#include "robotics/mobile/PlanarPath.hpp" +#include +#include +#include + +namespace +{ + using Point = math::Vector2; + using Path = mobile::PlanarPath; + + constexpr float pi{ std::numbers::pi_v }; + + class TestPlanarPath + : public ::testing::Test + { + protected: + TestPlanarPath() + { + path.Assign(waypoints); + } + + static void ExpectNear(const Point& actual, const Point& expected) + { + EXPECT_NEAR(actual[0], expected[0], 1e-5f); + EXPECT_NEAR(actual[1], expected[1], 1e-5f); + } + + std::array waypoints{ Point{ 0.0f, 0.0f }, Point{ 2.0f, 0.0f }, Point{ 4.0f, 2.0f }, Point{ 4.0f, 5.0f } }; + Path path; + }; +} + +TEST_F(TestPlanarPath, assign_computes_arc_lengths_and_rejects_bad_input) +{ + const std::array single{ Point{ 1.0f, 1.0f } }; + const std::array tooMany{}; + const std::array repeated{ Point{ 0.0f, 0.0f }, Point{ 1.0f, 0.0f }, Point{ 1.0f, 0.0f } }; + + EXPECT_NEAR(path.Length(), 7.828427f, 1e-5f); + EXPECT_NEAR(path.ArcLength(2), 4.828427f, 1e-5f); + EXPECT_FALSE(path.Assign(single)); + EXPECT_FALSE(path.Assign(tooMany)); + EXPECT_FALSE(path.Assign(repeated)); + EXPECT_EQ(path.Size(), std::size_t{ 4 }); + EXPECT_NEAR(path.Length(), 7.828427f, 1e-5f); +} + +TEST_F(TestPlanarPath, project_reports_the_signed_cross_track) +{ + const auto first{ path.Project(Point{ 1.0f, 0.5f }, 0, 3) }; + const auto second{ path.Project(Point{ 3.5f, 0.5f }, 0, 3) }; + + EXPECT_NEAR(first.arcLength, 1.0f, 1e-5f); + ExpectNear(first.point, Point{ 1.0f, 0.0f }); + EXPECT_NEAR(first.heading, 0.0f, 1e-6f); + EXPECT_NEAR(first.crossTrack, 0.5f, 1e-6f); + EXPECT_EQ(first.segment, std::size_t{ 0 }); + EXPECT_NEAR(second.arcLength, 3.414214f, 1e-5f); + ExpectNear(second.point, Point{ 3.0f, 1.0f }); + EXPECT_NEAR(second.heading, pi / 4.0f, 1e-6f); + EXPECT_NEAR(second.crossTrack, -0.707107f, 1e-5f); + EXPECT_EQ(second.segment, std::size_t{ 1 }); +} + +TEST_F(TestPlanarPath, point_at_clamps_to_the_path) +{ + ExpectNear(path.PointAt(5.5f), Point{ 4.0f, 2.671573f }); + EXPECT_NEAR(path.HeadingAt(5.5f), pi / 2.0f, 1e-6f); + EXPECT_EQ(path.PointAt(-1.0f)[0], 0.0f); + EXPECT_EQ(path.PointAt(-1.0f)[1], 0.0f); + EXPECT_EQ(path.PointAt(99.0f)[0], 4.0f); + EXPECT_EQ(path.PointAt(99.0f)[1], 5.0f); +} diff --git a/robotics/mobile/test/TestPurePursuit.cpp b/robotics/mobile/test/TestPurePursuit.cpp new file mode 100644 index 0000000..9ef058c --- /dev/null +++ b/robotics/mobile/test/TestPurePursuit.cpp @@ -0,0 +1,116 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/mobile/PurePursuit.hpp" +#include +#include +#include +#include +#include + +namespace +{ + using Point = math::Vector2; + using Transform = math::SE2Transform; + using Path = mobile::PlanarPath; + using Pursuit = mobile::PurePursuit; + + constexpr float pi{ std::numbers::pi_v }; + + class TestPurePursuit + : public ::testing::Test + { + protected: + TestPurePursuit() + { + path.Assign(waypoints); + uTurn.Assign(uTurnWaypoints); + } + + static void ExpectNear(const Point& actual, const Point& expected, float tolerance = 1e-5f) + { + EXPECT_NEAR(actual[0], expected[0], tolerance); + EXPECT_NEAR(actual[1], expected[1], tolerance); + } + + static float Distance(const Point& a, const Point& b) + { + return std::hypot(a[0] - b[0], a[1] - b[1]); + } + + std::array waypoints{ Point{ 0.0f, 0.0f }, Point{ 2.0f, 0.0f }, Point{ 4.0f, 2.0f }, Point{ 4.0f, 5.0f } }; + std::array uTurnWaypoints{ Point{ 0.0f, 0.0f }, Point{ 4.0f, 0.0f }, Point{ 4.0f, 1.0f }, Point{ 0.0f, 1.0f } }; + Path path; + Path uTurn; + mobile::PurePursuitParameters parameters{ 1.0f, 3.0f, 0.5f, 0.1f, 2 }; + Pursuit pursuit{ path, parameters }; + }; +} + +TEST_F(TestPurePursuit, goal_on_the_current_segment) +{ + const auto command{ pursuit.Compute(Transform::FromPose(0.5f, -0.2f, 0.0f), 1.0f) }; + + ExpectNear(command.goal, Point{ 1.986607f, 0.0f }); + EXPECT_NEAR(command.curvature, 0.177778f, 1e-5f); +} + +TEST_F(TestPurePursuit, goal_on_the_next_segment) +{ + const auto command{ pursuit.Compute(Transform::FromPose(1.8f, 0.0f, 0.0f), 1.0f) }; + + ExpectNear(command.goal, Point{ 2.955936f, 0.955936f }); + EXPECT_NEAR(command.curvature, 0.849721f, 1e-5f); +} + +TEST_F(TestPurePursuit, lookahead_is_speed_scheduled_and_clamped) +{ + const auto start{ Transform::FromPose(0.5f, 0.0f, 0.0f) }; + + EXPECT_NEAR(Distance(pursuit.Compute(start, 10.0f).goal, start.translation), 3.0f, 1e-5f); + EXPECT_NEAR(Distance(pursuit.Compute(start, 0.0f).goal, start.translation), 1.0f, 1e-5f); +} + +TEST_F(TestPurePursuit, falls_back_to_a_path_point_when_far_off_the_path) +{ + const auto command{ pursuit.Compute(Transform::FromPose(1.0f, -3.0f, 0.0f), 0.0f) }; + + ExpectNear(command.goal, Point{ 2.0f, 0.0f }); + EXPECT_NEAR(command.curvature, 0.6f, 1e-5f); +} + +TEST_F(TestPurePursuit, end_of_path_targets_the_last_point_and_reports_reached) +{ + const auto approaching{ pursuit.Compute(Transform::FromPose(4.0f, 4.5f, pi / 2.0f), 1.0f) }; + const auto arrived{ pursuit.Compute(Transform::FromPose(4.0f, 4.95f, pi / 2.0f), 1.0f) }; + + ExpectNear(approaching.goal, Point{ 4.0f, 5.0f }); + EXPECT_NEAR(approaching.curvature, 0.0f, 1e-5f); + EXPECT_FALSE(approaching.reached); + EXPECT_TRUE(arrived.reached); + pursuit.Restart(); + EXPECT_EQ(pursuit.Progress(), 0.0f); +} + +TEST_F(TestPurePursuit, search_window_keeps_progress_on_the_current_leg) +{ + Pursuit onLoop{ uTurn, parameters }; + + const auto command{ onLoop.Compute(Transform::FromPose(1.0f, 0.6f, 0.0f), 0.0f) }; + + ExpectNear(command.goal, Point{ 1.8f, 0.0f }); + EXPECT_NEAR(command.curvature, -1.2f, 1e-5f); +} + +TEST_F(TestPurePursuit, closed_loop_unicycle_reaches_the_goal) +{ + Pursuit tracker{ path, mobile::PurePursuitParameters{ 0.6f, 2.0f, 0.5f, 0.1f, 2 } }; + auto pose{ Transform::FromPose(0.0f, -0.5f, 0.3f) }; + constexpr float speed{ 0.8f }; + constexpr float dt{ 0.02f }; + + std::size_t steps{ 0 }; + for (auto command{ tracker.Compute(pose, speed) }; !command.reached && steps < 600; command = tracker.Compute(pose, speed), ++steps) + pose = pose * Transform::Exp(math::Vector3{ speed, 0.0f, speed * command.curvature }, dt); + + EXPECT_LT(steps, std::size_t{ 600 }); + EXPECT_LT(Distance(pose.translation, Point{ 4.0f, 5.0f }), 0.1f); +} diff --git a/robotics/mobile/test/TestSkidSteerKinematics.cpp b/robotics/mobile/test/TestSkidSteerKinematics.cpp new file mode 100644 index 0000000..594d26b --- /dev/null +++ b/robotics/mobile/test/TestSkidSteerKinematics.cpp @@ -0,0 +1,76 @@ +#include "robotics/mobile/DifferentialDrive.hpp" +#include "robotics/mobile/SkidSteerKinematics.hpp" +#include +#include + +namespace +{ + using Vector3 = math::Vector3; + using Wheels = mobile::WheelSpeeds; + using SkidSteer = mobile::SkidSteerKinematics; + + class TestSkidSteerKinematics + : public ::testing::Test + { + protected: + static void ExpectNear(const Vector3& actual, const Vector3& expected) + { + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(actual[i], expected[i], 1e-5f); + } + + static void ExpectNear(const Wheels& actual, const Wheels& expected) + { + EXPECT_NEAR(actual.left, expected.left, 1e-4f); + EXPECT_NEAR(actual.right, expected.right, 1e-4f); + } + + mobile::IcrParameters identified{ 0.30f, -0.28f, 0.05f }; + SkidSteer vehicle{ identified, 0.1f, 20.0f }; + mobile::IcrEstimator estimator{ 0.1f }; + }; +} + +TEST_F(TestSkidSteerKinematics, ideal_icr_reduces_to_differential_drive) +{ + const SkidSteer ideal{ SkidSteer::IdealIcr(0.4f), 0.1f, 20.0f }; + const mobile::DifferentialDrive drive{ 0.1f, 0.4f, 20.0f }; + + ExpectNear(ideal.Forward(Wheels{ 5.0f, 8.0f }), drive.Forward(Wheels{ 5.0f, 8.0f })); + ExpectNear(ideal.Forward(Wheels{ 5.0f, 8.0f }), Vector3{ 0.65f, 0.0f, 0.75f }); +} + +TEST_F(TestSkidSteerKinematics, forward_with_identified_icr_turns_slower_and_slips) +{ + ExpectNear(vehicle.Forward(Wheels{ 5.0f, 8.0f }), Vector3{ 0.655172f, -0.025862f, 0.517241f }); +} + +TEST_F(TestSkidSteerKinematics, inverse_inverts_forward) +{ + ExpectNear(vehicle.Inverse(0.655172f, 0.517241f), Wheels{ 5.0f, 8.0f }); + ExpectNear(vehicle.Limit(Wheels{ 30.0f, 10.0f }), Wheels{ 20.0f, 6.666667f }); +} + +TEST_F(TestSkidSteerKinematics, estimator_recovers_the_icr_from_consistent_samples) +{ + for (const auto& sprockets : { Wheels{ 5.0f, 8.0f }, Wheels{ 8.0f, 5.0f }, Wheels{ -3.0f, 4.0f }, Wheels{ 6.0f, 6.0f }, Wheels{ 2.0f, -2.0f } }) + estimator.Add(sprockets, vehicle.Forward(sprockets)); + + const auto estimate{ estimator.Estimate(0.01f) }; + + EXPECT_EQ(estimator.Samples(), std::size_t{ 5 }); + ASSERT_TRUE(estimate.has_value()); + EXPECT_NEAR(estimate->yLeft, 0.30f, 1e-5f); + EXPECT_NEAR(estimate->yRight, -0.28f, 1e-5f); + EXPECT_NEAR(estimate->xIcr, 0.05f, 1e-5f); +} + +TEST_F(TestSkidSteerKinematics, estimator_rejects_straight_driving) +{ + estimator.Add(Wheels{ 5.0f, 5.0f }, vehicle.Forward(Wheels{ 5.0f, 5.0f })); + estimator.Add(Wheels{ 8.0f, 8.0f }, vehicle.Forward(Wheels{ 8.0f, 8.0f })); + + EXPECT_FALSE(estimator.Estimate(0.01f).has_value()); + estimator.Reset(); + EXPECT_EQ(estimator.Samples(), std::size_t{ 0 }); +} diff --git a/robotics/mobile/test/TestStanleyController.cpp b/robotics/mobile/test/TestStanleyController.cpp new file mode 100644 index 0000000..909b340 --- /dev/null +++ b/robotics/mobile/test/TestStanleyController.cpp @@ -0,0 +1,82 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/mobile/CarLikeKinematics.hpp" +#include "robotics/mobile/PlanarPath.hpp" +#include "robotics/mobile/StanleyController.hpp" +#include +#include +#include +#include + +namespace +{ + using Point = math::Vector2; + using Transform = math::SE2Transform; + using Car = mobile::CarLikeKinematics; + using Stanley = mobile::StanleyController; + + class TestStanleyController + : public ::testing::Test + { + protected: + TestStanleyController() + { + road.Assign(waypoints); + } + + std::array waypoints{ Point{ 0.0f, 0.0f }, Point{ 50.0f, 0.0f } }; + mobile::PlanarPath road; + Car car{ { 2.5f, 1.5f, 0.6f, 10.0f } }; + mobile::StanleyParameters parameters{ 2.5f, 1.0f, 2 }; + Stanley stanley{ road, car, parameters }; + }; +} + +TEST_F(TestStanleyController, lateral_offset_steers_back_towards_the_path) +{ + EXPECT_NEAR(stanley.Compute(Transform::FromPose(0.0f, 0.5f, 0.0f), 5.0f, 0.0f, 1.0f), -0.205395f, 1e-5f); +} + +TEST_F(TestStanleyController, heading_error_adds_to_the_correction) +{ + EXPECT_NEAR(stanley.Compute(Transform::FromPose(0.0f, 0.0f, 0.2f), 5.0f, 0.0f, 1.0f), -0.404067f, 1e-5f); +} + +TEST_F(TestStanleyController, error_is_measured_at_the_front_axle) +{ + const auto error{ stanley.Error(Transform::FromPose(0.0f, 0.0f, 0.2f)) }; + + EXPECT_NEAR(error.crossTrack, 0.496673f, 1e-5f); + EXPECT_NEAR(error.headingError, -0.2f, 1e-5f); +} + +TEST_F(TestStanleyController, output_saturates_at_max_steer) +{ + EXPECT_EQ(stanley.Compute(Transform::FromPose(0.0f, -3.0f, 0.0f), 5.0f, 0.0f, 1.0f), 0.6f); +} + +TEST_F(TestStanleyController, steering_rate_is_limited) +{ + const Car slowSteering{ { 2.5f, 1.5f, 0.6f, 0.5f } }; + Stanley limited{ road, slowSteering, parameters }; + + EXPECT_NEAR(limited.Compute(Transform::FromPose(0.0f, 0.5f, 0.0f), 5.0f, 0.0f, 0.1f), -0.05f, 1e-6f); +} + +TEST_F(TestStanleyController, closed_loop_converges) +{ + const float speed{ 5.0f }; + const float dt{ 0.01f }; + auto pose{ Transform::FromPose(0.0f, 1.0f, 0.0f) }; + float steer{ 0.0f }; + + for (std::size_t k = 0; k < 400; ++k) + { + steer = stanley.Compute(pose, speed, steer, dt); + pose = (pose * Transform::Exp(car.Forward(speed, steer), dt)).Normalized(); + } + + EXPECT_LT(std::abs(stanley.Error(pose).crossTrack), 1e-3f); + + stanley.Restart(); + EXPECT_NEAR(stanley.Compute(Transform::FromPose(0.0f, 0.5f, 0.0f), speed, 0.0f, 1.0f), -0.205395f, 1e-5f); +} diff --git a/robotics/planning/CMakeLists.txt b/robotics/planning/CMakeLists.txt index d2379eb..df827a7 100644 --- a/robotics/planning/CMakeLists.txt +++ b/robotics/planning/CMakeLists.txt @@ -12,19 +12,27 @@ target_link_libraries(robotics.planning ${ROBOTICS_VISIBILITY} ) target_sources(robotics.planning PRIVATE + CarPath.hpp ConvexCollision.hpp + DubinsPath.hpp + DynamicWindowApproach.hpp GridSearch.hpp PotentialField.hpp PrimitiveDistance.hpp + ReedsSheppPath.hpp RrtConnect.hpp SelfCollision.hpp ) robotics_add_coverage_sources(robotics.planning + CarPath.cpp ConvexCollision.cpp + DubinsPath.cpp + DynamicWindowApproach.cpp GridSearch.cpp PotentialField.cpp PrimitiveDistance.cpp + ReedsSheppPath.cpp RrtConnect.cpp SelfCollision.cpp ) diff --git a/robotics/planning/CarPath.cpp b/robotics/planning/CarPath.cpp new file mode 100644 index 0000000..6fe7705 --- /dev/null +++ b/robotics/planning/CarPath.cpp @@ -0,0 +1,6 @@ +#include "robotics/planning/CarPath.hpp" + +namespace planning +{ + template struct CarPath; +} diff --git a/robotics/planning/CarPath.hpp b/robotics/planning/CarPath.hpp new file mode 100644 index 0000000..db34fe9 --- /dev/null +++ b/robotics/planning/CarPath.hpp @@ -0,0 +1,77 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include +#include +#include +#include +#include + +namespace planning +{ + enum class Steering : std::uint8_t + { + Left, + Straight, + Right + }; + + template + struct CarPathSegment + { + static_assert(std::is_floating_point_v, "CarPathSegment supports floating-point types"); + + Steering steering{ Steering::Straight }; + T length{}; + }; + + template + struct CarPath + { + static_assert(std::is_floating_point_v, "CarPath supports floating-point types"); + + T Length() const; + math::SE2Transform Sample(const math::SE2Transform& start, T s) const; + + std::array, 5> segments{}; + std::size_t count{ 0 }; + T turningRadius{}; + }; + + template + T CarPath::Length() const + { + T total{}; + for (std::size_t k = 0; k < count; ++k) + total += math::Abs(segments[k].length); + return total; + } + + template + math::SE2Transform CarPath::Sample(const math::SE2Transform& start, T s) const + { + auto pose{ start }; + T remaining{ std::clamp(s, T{}, Length()) }; + for (std::size_t k = 0; k < count; ++k) + { + const auto& segment{ segments[k] }; + const T take{ std::min(remaining, math::Abs(segment.length)) }; + const T direction{ segment.length < T{} ? T{ -1 } : T{ 1 } }; + T curvature{}; + if (segment.steering == Steering::Left) + curvature = T{ 1 } / turningRadius; + else if (segment.steering == Steering::Right) + curvature = -T{ 1 } / turningRadius; + + pose = pose * math::SE2Transform::Exp(math::Vector3{ direction, T{}, direction * curvature }, take); + remaining -= take; + } + return pose; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template struct CarPath; +#endif +} diff --git a/robotics/planning/DubinsPath.cpp b/robotics/planning/DubinsPath.cpp new file mode 100644 index 0000000..b4cacad --- /dev/null +++ b/robotics/planning/DubinsPath.cpp @@ -0,0 +1,6 @@ +#include "robotics/planning/DubinsPath.hpp" + +namespace planning +{ + template class DubinsPlanner; +} diff --git a/robotics/planning/DubinsPath.hpp b/robotics/planning/DubinsPath.hpp new file mode 100644 index 0000000..eafe5fa --- /dev/null +++ b/robotics/planning/DubinsPath.hpp @@ -0,0 +1,196 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "robotics/planning/CarPath.hpp" +#include +#include +#include +#include +#include +#include + +namespace planning +{ + template + class DubinsPlanner + { + static_assert(std::is_floating_point_v, "DubinsPlanner supports floating-point types"); + + public: + explicit DubinsPlanner(T turningRadius); + + CarPath Shortest(const math::SE2Transform& start, const math::SE2Transform& goal) const; + + private: + struct Geometry + { + T d; + T alpha; + T beta; + T sa; + T ca; + T sb; + T cb; + T cab; + }; + + using Lengths = std::array; + using Word = std::array; + + static constexpr T twoPi{ T{ 2 } * std::numbers::pi_v }; + static constexpr T tolerance{ T{ 1e-5f } }; + static constexpr std::array words{ { { Steering::Left, Steering::Straight, Steering::Left }, + { Steering::Right, Steering::Straight, Steering::Right }, + { Steering::Left, Steering::Straight, Steering::Right }, + { Steering::Right, Steering::Straight, Steering::Left }, + { Steering::Right, Steering::Left, Steering::Right }, + { Steering::Left, Steering::Right, Steering::Left } } }; + + static T Mod2Pi(T angle); + static std::optional Straight(T squared); + static std::optional Middle(T cosine); + static Lengths Lsl(const Geometry& g); + static Lengths Rsr(const Geometry& g); + static std::optional Lsr(const Geometry& g); + static std::optional Rsl(const Geometry& g); + static std::optional Rlr(const Geometry& g); + static std::optional Lrl(const Geometry& g); + + T turningRadius; + }; + + template + DubinsPlanner::DubinsPlanner(T turningRadius) + : turningRadius{ turningRadius } + { + really_assert(turningRadius > T{}); + } + + template + CarPath DubinsPlanner::Shortest(const math::SE2Transform& start, const math::SE2Transform& goal) const + { + const T dx{ goal.translation[0] - start.translation[0] }; + const T dy{ goal.translation[1] - start.translation[1] }; + const T distance{ math::Hypot(dx, dy) }; + const T theta{ distance > T{} ? math::Atan2(dy, dx) : start.Heading() }; + const T alpha{ Mod2Pi(start.Heading() - theta) }; + const T beta{ Mod2Pi(goal.Heading() - theta) }; + const Geometry geometry{ distance / turningRadius, alpha, beta, math::Sin(alpha), math::Cos(alpha), math::Sin(beta), math::Cos(beta), math::Cos(alpha - beta) }; + + const std::array, 6> candidates{ Lsl(geometry), Rsr(geometry), Lsr(geometry), Rsl(geometry), Rlr(geometry), Lrl(geometry) }; + + std::size_t best{ candidates.size() }; + T bestLength{}; + for (std::size_t k = 0; k < candidates.size(); ++k) + { + if (!candidates[k]) + continue; + + const auto& lengths{ *candidates[k] }; + const T total{ lengths[0] + lengths[1] + lengths[2] }; + if (best == candidates.size() || total < bestLength) + { + best = k; + bestLength = total; + } + } + + really_assert(best < candidates.size()); + + CarPath path{}; + path.count = 3; + path.turningRadius = turningRadius; + for (std::size_t k = 0; k < 3; ++k) + path.segments[k] = CarPathSegment{ words[best][k], (*candidates[best])[k] * turningRadius }; + return path; + } + + template + T DubinsPlanner::Mod2Pi(T angle) + { + const T wrapped{ angle - twoPi * math::Floor(angle / twoPi) }; + return wrapped > twoPi - tolerance ? T{} : wrapped; + } + + template + std::optional DubinsPlanner::Straight(T squared) + { + if (squared < -tolerance) + return std::nullopt; + return math::Sqrt(std::max(squared, T{})); + } + + template + std::optional DubinsPlanner::Middle(T cosine) + { + if (math::Abs(cosine) > T{ 1 } + tolerance) + return std::nullopt; + return Mod2Pi(twoPi - math::Acos(std::clamp(cosine, T{ -1 }, T{ 1 }))); + } + + template + auto DubinsPlanner::Lsl(const Geometry& g) -> Lengths + { + const T x{ g.d + g.sa - g.sb }; + const T y{ g.cb - g.ca }; + const T tau{ math::Atan2(y, x) }; + return Lengths{ Mod2Pi(tau - g.alpha), math::Hypot(x, y), Mod2Pi(g.beta - tau) }; + } + + template + auto DubinsPlanner::Rsr(const Geometry& g) -> Lengths + { + const T x{ g.d - g.sa + g.sb }; + const T y{ g.ca - g.cb }; + const T tau{ math::Atan2(y, x) }; + return Lengths{ Mod2Pi(g.alpha - tau), math::Hypot(x, y), Mod2Pi(tau - g.beta) }; + } + + template + auto DubinsPlanner::Lsr(const Geometry& g) -> std::optional + { + const auto p{ Straight(T{ -2 } + g.d * g.d + T{ 2 } * g.cab + T{ 2 } * g.d * (g.sa + g.sb)) }; + if (!p) + return std::nullopt; + const T tau{ math::Atan2(-g.ca - g.cb, g.d + g.sa + g.sb) - math::Atan2(T{ -2 }, *p) }; + return Lengths{ Mod2Pi(tau - g.alpha), *p, Mod2Pi(tau - g.beta) }; + } + + template + auto DubinsPlanner::Rsl(const Geometry& g) -> std::optional + { + const auto p{ Straight(T{ -2 } + g.d * g.d + T{ 2 } * g.cab - T{ 2 } * g.d * (g.sa + g.sb)) }; + if (!p) + return std::nullopt; + const T tau{ math::Atan2(g.ca + g.cb, g.d - g.sa - g.sb) - math::Atan2(T{ 2 }, *p) }; + return Lengths{ Mod2Pi(g.alpha - tau), *p, Mod2Pi(g.beta - tau) }; + } + + template + auto DubinsPlanner::Rlr(const Geometry& g) -> std::optional + { + const auto p{ Middle((T{ 6 } - g.d * g.d + T{ 2 } * g.cab + T{ 2 } * g.d * (g.sa - g.sb)) / T{ 8 }) }; + if (!p) + return std::nullopt; + const T phi{ math::Atan2(g.ca - g.cb, g.d - g.sa + g.sb) }; + const T t{ Mod2Pi(g.alpha - phi + *p / T{ 2 }) }; + return Lengths{ t, *p, Mod2Pi(g.alpha - g.beta - t + *p) }; + } + + template + auto DubinsPlanner::Lrl(const Geometry& g) -> std::optional + { + const auto p{ Middle((T{ 6 } - g.d * g.d + T{ 2 } * g.cab + T{ 2 } * g.d * (g.sb - g.sa)) / T{ 8 }) }; + if (!p) + return std::nullopt; + const T phi{ math::Atan2(g.ca - g.cb, g.d + g.sa - g.sb) }; + const T t{ Mod2Pi(-g.alpha - phi + *p / T{ 2 }) }; + return Lengths{ t, *p, Mod2Pi(g.beta - g.alpha - t + *p) }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class DubinsPlanner; +#endif +} diff --git a/robotics/planning/DynamicWindowApproach.cpp b/robotics/planning/DynamicWindowApproach.cpp new file mode 100644 index 0000000..c5f4178 --- /dev/null +++ b/robotics/planning/DynamicWindowApproach.cpp @@ -0,0 +1,7 @@ +#include "robotics/planning/DynamicWindowApproach.hpp" + +namespace planning +{ + template class DynamicWindowApproach; + template class DynamicWindowApproach; +} diff --git a/robotics/planning/DynamicWindowApproach.hpp b/robotics/planning/DynamicWindowApproach.hpp new file mode 100644 index 0000000..c8cb1e9 --- /dev/null +++ b/robotics/planning/DynamicWindowApproach.hpp @@ -0,0 +1,233 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/AngleUtilities.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace planning +{ + template + struct DwaParameters + { + static_assert(std::is_floating_point_v, "DwaParameters supports floating-point types"); + + T minSpeed{}; + T maxSpeed{}; + T maxTurnRate{}; + T maxAcceleration{}; + T maxTurnAcceleration{}; + T controlPeriod{}; + T predictionTime{}; + T robotRadius{}; + T maxClearance{}; + T headingWeight{}; + T clearanceWeight{}; + T speedWeight{}; + }; + + template + struct VelocityWindow + { + static_assert(std::is_floating_point_v, "VelocityWindow supports floating-point types"); + + T vMin{}; + T vMax{}; + T omegaMin{}; + T omegaMax{}; + }; + + template + struct DwaCommand + { + static_assert(std::is_floating_point_v, "DwaCommand supports floating-point types"); + + T v{}; + T omega{}; + T clearance{}; + }; + + template + class DynamicWindowApproach + { + static_assert(std::is_floating_point_v, "DynamicWindowApproach supports floating-point types"); + static_assert(NumV >= 2 && NumW >= 2, "DynamicWindowApproach needs at least two samples per axis"); + + public: + explicit DynamicWindowApproach(const DwaParameters& parameters); + + VelocityWindow Window(T v, T omega) const; + OPTIMIZE_FOR_SPEED std::optional> Compute(T v, T omega, const math::Vector2& goal, std::span> obstacles) const; + + static T ArcClearance(T v, T omega, const math::Vector2& obstacle, T robotRadius, T maxClearance); + + private: + struct Sample + { + T v; + T omega; + T heading; + T clearance; + bool admissible; + }; + + struct Maxima + { + T heading; + T clearance; + T speed; + }; + + static constexpr std::size_t sampleCount{ NumV * NumW }; + + using Samples = std::array; + + static constexpr T pi{ std::numbers::pi_v }; + static constexpr T straightCurvature{ T{ 1e-6f } }; + + static T Mod2Pi(T angle); + static T Interpolate(T low, T high, std::size_t index, std::size_t count); + static T Normalise(T value, T maximum); + Sample Evaluate(T v, T omega, const math::Vector2& goal, std::span> obstacles) const; + DwaCommand Select(const Samples& samples, const Maxima& maxima) const; + + DwaParameters parameters; + }; + + template + DynamicWindowApproach::DynamicWindowApproach(const DwaParameters& parameters) + : parameters{ parameters } + { + really_assert(parameters.minSpeed >= T{} && parameters.minSpeed <= parameters.maxSpeed); + really_assert(parameters.maxTurnRate > T{} && parameters.maxAcceleration > T{} && parameters.maxTurnAcceleration > T{}); + really_assert(parameters.controlPeriod > T{} && parameters.predictionTime > T{} && parameters.robotRadius > T{} && parameters.maxClearance > T{}); + } + + template + VelocityWindow DynamicWindowApproach::Window(T v, T omega) const + { + const T speedStep{ parameters.maxAcceleration * parameters.controlPeriod }; + const T turnStep{ parameters.maxTurnAcceleration * parameters.controlPeriod }; + return VelocityWindow{ std::max(parameters.minSpeed, v - speedStep), std::min(parameters.maxSpeed, v + speedStep), + std::max(-parameters.maxTurnRate, omega - turnStep), std::min(parameters.maxTurnRate, omega + turnStep) }; + } + + template + OPTIMIZE_FOR_SPEED std::optional> DynamicWindowApproach::Compute(T v, T omega, const math::Vector2& goal, std::span> obstacles) const + { + const auto window{ Window(v, omega) }; + Samples samples{}; + Maxima maxima{}; + bool anyAdmissible{ false }; + + for (std::size_t k = 0; k < sampleCount; ++k) + { + samples[k] = Evaluate(Interpolate(window.vMin, window.vMax, k / NumW, NumV), Interpolate(window.omegaMin, window.omegaMax, k % NumW, NumW), goal, obstacles); + const auto& sample{ samples[k] }; + if (sample.admissible) + { + anyAdmissible = true; + maxima = Maxima{ std::max(maxima.heading, sample.heading), std::max(maxima.clearance, sample.clearance), std::max(maxima.speed, sample.v) }; + } + } + + if (!anyAdmissible) + return std::nullopt; + + return Select(samples, maxima); + } + + template + T DynamicWindowApproach::ArcClearance(T v, T omega, const math::Vector2& obstacle, T robotRadius, T maxClearance) + { + if (math::Hypot(obstacle[0], obstacle[1]) <= robotRadius) + return T{}; + if (v <= T{}) + return maxClearance; + + const T curvature{ omega / v }; + if (math::Abs(curvature) < straightCurvature) + { + const bool ahead{ math::Abs(obstacle[1]) <= robotRadius && obstacle[0] > T{} }; + return ahead ? std::min(maxClearance, obstacle[0] - math::Sqrt(robotRadius * robotRadius - obstacle[1] * obstacle[1])) : maxClearance; + } + + const T radius{ T{ 1 } / curvature }; + const T distance{ math::Hypot(obstacle[0], obstacle[1] - radius) }; + if (math::Abs(distance - math::Abs(radius)) > robotRadius) + return maxClearance; + + const T halfContact{ math::Acos(std::clamp((radius * radius + distance * distance - robotRadius * robotRadius) / (T{ 2 } * math::Abs(radius) * distance), T{ -1 }, T{ 1 })) }; + const T startBearing{ math::Atan2(-radius, T{}) }; + const T obstacleBearing{ math::Atan2(obstacle[1] - radius, obstacle[0]) }; + const T sweep{ Mod2Pi(curvature > T{} ? obstacleBearing - startBearing : startBearing - obstacleBearing) }; + return std::min(maxClearance, math::Abs(radius) * Mod2Pi(sweep - halfContact)); + } + + template + T DynamicWindowApproach::Mod2Pi(T angle) + { + const T twoPi{ T{ 2 } * pi }; + return angle - twoPi * math::Floor(angle / twoPi); + } + + template + T DynamicWindowApproach::Interpolate(T low, T high, std::size_t index, std::size_t count) + { + return low + static_cast(index) * (high - low) / static_cast(count - 1); + } + + template + T DynamicWindowApproach::Normalise(T value, T maximum) + { + return maximum > T{} ? value / maximum : value; + } + + template + auto DynamicWindowApproach::Evaluate(T v, T omega, const math::Vector2& goal, std::span> obstacles) const -> Sample + { + T clearance{ parameters.maxClearance }; + for (const auto& obstacle : obstacles) + clearance = std::min(clearance, ArcClearance(v, omega, obstacle, parameters.robotRadius, parameters.maxClearance)); + + const auto predicted{ math::SE2Transform::Exp(math::Vector3{ v, T{}, omega }, parameters.predictionTime) }; + const T bearing{ math::Atan2(goal[1] - predicted.translation[1], goal[0] - predicted.translation[0]) }; + const T heading{ pi - math::Abs(math::AngleDifference(bearing, predicted.Heading())) }; + return Sample{ v, omega, heading, clearance, v <= math::Sqrt(T{ 2 } * parameters.maxAcceleration * clearance) }; + } + + template + DwaCommand DynamicWindowApproach::Select(const Samples& samples, const Maxima& maxima) const + { + const Sample* best{ &samples.front() }; + T bestScore{ -std::numeric_limits::infinity() }; + for (const auto& sample : samples) + { + if (!sample.admissible) + continue; + + const T score{ parameters.headingWeight * Normalise(sample.heading, maxima.heading) + parameters.clearanceWeight * Normalise(sample.clearance, maxima.clearance) + parameters.speedWeight * Normalise(sample.v, maxima.speed) }; + if (score > bestScore) + { + best = &sample; + bestScore = score; + } + } + + return DwaCommand{ best->v, best->omega, best->clearance }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class DynamicWindowApproach; + extern template class DynamicWindowApproach; +#endif +} diff --git a/robotics/planning/ReedsSheppPath.cpp b/robotics/planning/ReedsSheppPath.cpp new file mode 100644 index 0000000..38803fd --- /dev/null +++ b/robotics/planning/ReedsSheppPath.cpp @@ -0,0 +1,6 @@ +#include "robotics/planning/ReedsSheppPath.hpp" + +namespace planning +{ + template class ReedsSheppPlanner; +} diff --git a/robotics/planning/ReedsSheppPath.hpp b/robotics/planning/ReedsSheppPath.hpp new file mode 100644 index 0000000..467b72a --- /dev/null +++ b/robotics/planning/ReedsSheppPath.hpp @@ -0,0 +1,334 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/AngleUtilities.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/SE2Transform.hpp" +#include "robotics/planning/CarPath.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace planning +{ + template + class ReedsSheppPlanner + { + static_assert(std::is_floating_point_v, "ReedsSheppPlanner supports floating-point types"); + + public: + explicit ReedsSheppPlanner(T turningRadius); + + CarPath Shortest(const math::SE2Transform& start, const math::SE2Transform& goal) const; + + private: + using Triple = std::array; + using Word = std::array; + + struct Lengths + { + std::array values; + std::size_t count; + }; + + struct Best + { + CarPath path{}; + T length{ std::numeric_limits::infinity() }; + }; + + struct Pose + { + T x; + T y; + T phi; + }; + + static constexpr T pi{ std::numbers::pi_v }; + static constexpr T halfPi{ std::numbers::pi_v / T{ 2 } }; + static constexpr T tolerance{ T{ 1e-5f } }; + static constexpr Word lsl{ Steering::Left, Steering::Straight, Steering::Left, Steering::Straight, Steering::Straight }; + static constexpr Word lsr{ Steering::Left, Steering::Straight, Steering::Right, Steering::Straight, Steering::Straight }; + static constexpr Word lrl{ Steering::Left, Steering::Right, Steering::Left, Steering::Straight, Steering::Straight }; + static constexpr Word lrlr{ Steering::Left, Steering::Right, Steering::Left, Steering::Right, Steering::Straight }; + static constexpr Word lrsl{ Steering::Left, Steering::Right, Steering::Straight, Steering::Left, Steering::Straight }; + static constexpr Word lrsr{ Steering::Left, Steering::Right, Steering::Straight, Steering::Right, Steering::Straight }; + static constexpr Word lrslr{ Steering::Left, Steering::Right, Steering::Straight, Steering::Left, Steering::Right }; + + template + static void Consider(Best& best, Formula formula, Builder builder, const Word& word, const Pose& pose, bool backwards); + static Steering Mirror(Steering steering); + static Triple TauOmega(T u, T v, T xi, T eta, T phi); + + static Lengths Plain(const Triple& r); + static Lengths OppositeTurns(const Triple& r); + static Lengths EqualTurns(const Triple& r); + static Lengths QuarterTurnFirst(const Triple& r); + static Lengths QuarterTurnsAround(const Triple& r); + + static std::optional LpSpLp(T x, T y, T phi); + static std::optional LpSpRp(T x, T y, T phi); + static std::optional LpRmL(T x, T y, T phi); + static std::optional LpRupLumRm(T x, T y, T phi); + static std::optional LpRumLumRp(T x, T y, T phi); + static std::optional LpRmSmLm(T x, T y, T phi); + static std::optional LpRmSmRm(T x, T y, T phi); + static std::optional LpRmSLmRp(T x, T y, T phi); + + T turningRadius; + }; + + template + ReedsSheppPlanner::ReedsSheppPlanner(T turningRadius) + : turningRadius{ turningRadius } + { + really_assert(turningRadius > T{}); + } + + template + CarPath ReedsSheppPlanner::Shortest(const math::SE2Transform& start, const math::SE2Transform& goal) const + { + const auto relative{ start.Inverse() * goal }; + const Pose pose{ relative.translation[0] / turningRadius, relative.translation[1] / turningRadius, relative.Heading() }; + const T cosine{ math::Cos(pose.phi) }; + const T sine{ math::Sin(pose.phi) }; + const Pose reversed{ pose.x * cosine + pose.y * sine, pose.x * sine - pose.y * cosine, pose.phi }; + + Best best{}; + Consider(best, &LpSpLp, &Plain, lsl, pose, false); + Consider(best, &LpSpRp, &Plain, lsr, pose, false); + Consider(best, &LpRmL, &Plain, lrl, pose, false); + Consider(best, &LpRmL, &Plain, lrl, reversed, true); + Consider(best, &LpRupLumRm, &OppositeTurns, lrlr, pose, false); + Consider(best, &LpRumLumRp, &EqualTurns, lrlr, pose, false); + Consider(best, &LpRmSmLm, &QuarterTurnFirst, lrsl, pose, false); + Consider(best, &LpRmSmRm, &QuarterTurnFirst, lrsr, pose, false); + Consider(best, &LpRmSmLm, &QuarterTurnFirst, lrsl, reversed, true); + Consider(best, &LpRmSmRm, &QuarterTurnFirst, lrsr, reversed, true); + Consider(best, &LpRmSLmRp, &QuarterTurnsAround, lrslr, pose, false); + + auto path{ best.path }; + path.turningRadius = turningRadius; + for (auto& segment : path.segments) + segment.length *= turningRadius; + return path; + } + + template + template + void ReedsSheppPlanner::Consider(Best& best, Formula formula, Builder builder, const Word& word, const Pose& pose, bool backwards) + { + for (std::size_t symmetry = 0; symmetry < 4; ++symmetry) + { + const bool timeFlip{ (symmetry & 1U) != 0 }; + const bool reflect{ (symmetry & 2U) != 0 }; + const auto solution{ formula(timeFlip ? -pose.x : pose.x, reflect ? -pose.y : pose.y, timeFlip != reflect ? -pose.phi : pose.phi) }; + if (!solution) + continue; + + const auto lengths{ builder(*solution) }; + CarPath candidate{}; + candidate.count = lengths.count; + for (std::size_t k = 0; k < lengths.count; ++k) + { + const std::size_t source{ backwards ? lengths.count - 1 - k : k }; + candidate.segments[k] = CarPathSegment{ reflect ? Mirror(word[source]) : word[source], timeFlip ? -lengths.values[source] : lengths.values[source] }; + } + + const T length{ candidate.Length() }; + if (length < best.length) + best = Best{ candidate, length }; + } + } + + template + Steering ReedsSheppPlanner::Mirror(Steering steering) + { + using enum Steering; + + if (steering == Left) + return Right; + if (steering == Right) + return Left; + return Straight; + } + + template + auto ReedsSheppPlanner::TauOmega(T u, T v, T xi, T eta, T phi) -> Triple + { + const T delta{ math::WrapToPi(u - v) }; + const T a{ math::Sin(u) - math::Sin(delta) }; + const T b{ math::Cos(u) - math::Cos(delta) - T{ 1 } }; + const T t1{ math::Atan2(eta * a - xi * b, xi * a + eta * b) }; + const T t2{ T{ 2 } * (math::Cos(delta) - math::Cos(v) - math::Cos(u)) + T{ 3 } }; + const T tau{ t2 < T{} ? math::WrapToPi(t1 + pi) : math::WrapToPi(t1) }; + return Triple{ tau, math::WrapToPi(tau - u + v - phi), T{} }; + } + + template + auto ReedsSheppPlanner::Plain(const Triple& r) -> Lengths + { + return Lengths{ { r[0], r[1], r[2], T{}, T{} }, 3 }; + } + + template + auto ReedsSheppPlanner::OppositeTurns(const Triple& r) -> Lengths + { + return Lengths{ { r[0], r[1], -r[1], r[2], T{} }, 4 }; + } + + template + auto ReedsSheppPlanner::EqualTurns(const Triple& r) -> Lengths + { + return Lengths{ { r[0], r[1], r[1], r[2], T{} }, 4 }; + } + + template + auto ReedsSheppPlanner::QuarterTurnFirst(const Triple& r) -> Lengths + { + return Lengths{ { r[0], -halfPi, r[1], r[2], T{} }, 4 }; + } + + template + auto ReedsSheppPlanner::QuarterTurnsAround(const Triple& r) -> Lengths + { + return Lengths{ { r[0], -halfPi, r[1], -halfPi, r[2] }, 5 }; + } + + template + auto ReedsSheppPlanner::LpSpLp(T x, T y, T phi) -> std::optional + { + const T xi{ x - math::Sin(phi) }; + const T eta{ y - T{ 1 } + math::Cos(phi) }; + const T t{ math::Atan2(eta, xi) }; + const T v{ math::WrapToPi(phi - t) }; + if (t < -tolerance || v < -tolerance) + return std::nullopt; + return Triple{ t, math::Hypot(xi, eta), v }; + } + + template + auto ReedsSheppPlanner::LpSpRp(T x, T y, T phi) -> std::optional + { + const T xi{ x + math::Sin(phi) }; + const T eta{ y - T{ 1 } - math::Cos(phi) }; + const T squared{ xi * xi + eta * eta }; + if (squared < T{ 4 }) + return std::nullopt; + + const T u{ math::Sqrt(squared - T{ 4 }) }; + const T t{ math::WrapToPi(math::Atan2(eta, xi) + math::Atan2(T{ 2 }, u)) }; + const T v{ math::WrapToPi(t - phi) }; + if (t < -tolerance || v < -tolerance) + return std::nullopt; + return Triple{ t, u, v }; + } + + template + auto ReedsSheppPlanner::LpRmL(T x, T y, T phi) -> std::optional + { + const T xi{ x - math::Sin(phi) }; + const T eta{ y - T{ 1 } + math::Cos(phi) }; + const T distance{ math::Hypot(xi, eta) }; + if (distance > T{ 4 }) + return std::nullopt; + + const T u{ T{ -2 } * math::Asin(distance / T{ 4 }) }; + const T t{ math::WrapToPi(math::Atan2(eta, xi) + u / T{ 2 } + pi) }; + if (t < -tolerance || u > tolerance) + return std::nullopt; + return Triple{ t, u, math::WrapToPi(phi - t + u) }; + } + + template + auto ReedsSheppPlanner::LpRupLumRm(T x, T y, T phi) -> std::optional + { + const T xi{ x + math::Sin(phi) }; + const T eta{ y - T{ 1 } - math::Cos(phi) }; + const T rho{ (T{ 2 } + math::Hypot(xi, eta)) / T{ 4 } }; + if (rho > T{ 1 }) + return std::nullopt; + + const T u{ math::Acos(rho) }; + const auto tauOmega{ TauOmega(u, -u, xi, eta, phi) }; + if (tauOmega[0] < -tolerance || tauOmega[1] > tolerance) + return std::nullopt; + return Triple{ tauOmega[0], u, tauOmega[1] }; + } + + template + auto ReedsSheppPlanner::LpRumLumRp(T x, T y, T phi) -> std::optional + { + const T xi{ x + math::Sin(phi) }; + const T eta{ y - T{ 1 } - math::Cos(phi) }; + const T rho{ (T{ 20 } - xi * xi - eta * eta) / T{ 16 } }; + if (rho < T{} || rho > T{ 1 }) + return std::nullopt; + + const T u{ -math::Acos(rho) }; + const auto tauOmega{ TauOmega(u, u, xi, eta, phi) }; + if (tauOmega[0] < -tolerance || tauOmega[1] < -tolerance) + return std::nullopt; + return Triple{ tauOmega[0], u, tauOmega[1] }; + } + + template + auto ReedsSheppPlanner::LpRmSmLm(T x, T y, T phi) -> std::optional + { + const T xi{ x - math::Sin(phi) }; + const T eta{ y - T{ 1 } + math::Cos(phi) }; + const T distance{ math::Hypot(xi, eta) }; + if (distance < T{ 2 }) + return std::nullopt; + + const T straight{ math::Sqrt(distance * distance - T{ 4 }) }; + const T u{ T{ 2 } - straight }; + const T t{ math::WrapToPi(math::Atan2(eta, xi) + math::Atan2(straight, T{ -2 })) }; + const T v{ math::WrapToPi(phi - halfPi - t) }; + if (t < -tolerance || u > tolerance || v > tolerance) + return std::nullopt; + return Triple{ t, u, v }; + } + + template + auto ReedsSheppPlanner::LpRmSmRm(T x, T y, T phi) -> std::optional + { + const T xi{ x + math::Sin(phi) }; + const T eta{ y - T{ 1 } - math::Cos(phi) }; + const T distance{ math::Hypot(eta, xi) }; + if (distance < T{ 2 }) + return std::nullopt; + + const T t{ math::Atan2(xi, -eta) }; + const T u{ T{ 2 } - distance }; + const T v{ math::WrapToPi(t + halfPi - phi) }; + if (t < -tolerance || u > tolerance || v > tolerance) + return std::nullopt; + return Triple{ t, u, v }; + } + + template + auto ReedsSheppPlanner::LpRmSLmRp(T x, T y, T phi) -> std::optional + { + const T xi{ x + math::Sin(phi) }; + const T eta{ y - T{ 1 } - math::Cos(phi) }; + const T distance{ math::Hypot(xi, eta) }; + if (distance < T{ 2 }) + return std::nullopt; + + const T u{ T{ 4 } - math::Sqrt(distance * distance - T{ 4 }) }; + const T t{ math::WrapToPi(math::Atan2((T{ 4 } - u) * xi - T{ 2 } * eta, T{ -2 } * xi + (u - T{ 4 }) * eta)) }; + const T v{ math::WrapToPi(t - phi) }; + if (u > tolerance || t < -tolerance || v < -tolerance) + return std::nullopt; + return Triple{ t, u, v }; + } + +#ifdef ROBOTICS_TOOLBOX_COVERAGE_BUILD + extern template class ReedsSheppPlanner; +#endif +} diff --git a/robotics/planning/test/CMakeLists.txt b/robotics/planning/test/CMakeLists.txt index 447a63c..80bbc85 100644 --- a/robotics/planning/test/CMakeLists.txt +++ b/robotics/planning/test/CMakeLists.txt @@ -9,9 +9,12 @@ target_link_libraries(robotics.planning_test PUBLIC target_sources(robotics.planning_test PRIVATE TestConvexCollision.cpp + TestDubinsPath.cpp + TestDynamicWindowApproach.cpp TestGridSearch.cpp TestPotentialField.cpp TestPrimitiveDistance.cpp + TestReedsSheppPath.cpp TestRrtConnect.cpp TestSelfCollision.cpp ) diff --git a/robotics/planning/test/TestDubinsPath.cpp b/robotics/planning/test/TestDubinsPath.cpp new file mode 100644 index 0000000..512b3f5 --- /dev/null +++ b/robotics/planning/test/TestDubinsPath.cpp @@ -0,0 +1,139 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/planning/CarPath.hpp" +#include "robotics/planning/DubinsPath.hpp" +#include +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Path = planning::CarPath; + using planning::Steering; + + constexpr float pi{ std::numbers::pi_v }; + + class TestDubinsPath + : public ::testing::Test + { + protected: + static void ExpectPath(const Path& path, const std::array& letters, const std::array& lengths) + { + ASSERT_EQ(path.count, 3u); + for (std::size_t k = 0; k < 3; ++k) + { + EXPECT_EQ(path.segments[k].steering, letters[k]); + EXPECT_NEAR(path.segments[k].length, lengths[k], 1e-4f); + } + } + + static void ExpectPose(const Transform& actual, const Transform& expected) + { + const auto error{ (expected.Inverse() * actual).Pose() }; + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(error[i], 0.0f, 1e-4f); + } + + static void ExpectReaches(const Path& path, const Transform& from, const Transform& goal) + { + ExpectPose(path.Sample(from, path.Length()), goal); + } + + planning::DubinsPlanner planner{ 1.0f }; + Transform start{ Transform::Identity() }; + }; +} + +TEST_F(TestDubinsPath, straight_ahead_is_a_single_segment) +{ + const auto goal{ Transform::FromPose(4.0f, 0.0f, 0.0f) }; + const auto path{ planner.Shortest(start, goal) }; + + EXPECT_NEAR(path.Length(), 4.0f, 1e-4f); + EXPECT_NEAR(path.segments[0].length, 0.0f, 1e-4f); + EXPECT_NEAR(path.segments[2].length, 0.0f, 1e-4f); + ExpectReaches(path, start, goal); +} + +TEST_F(TestDubinsPath, csc_shortest_is_lsl) +{ + const auto path{ planner.Shortest(start, Transform::FromPose(4.0f, 4.0f, pi / 2.0f)) }; + + ExpectPath(path, { Steering::Left, Steering::Straight, Steering::Left }, { 0.785398f, 4.242641f, 0.785398f }); + EXPECT_NEAR(path.Length(), 5.813437f, 1e-4f); +} + +TEST_F(TestDubinsPath, csc_shortest_is_rsl) +{ + const auto path{ planner.Shortest(start, Transform::FromPose(3.0f, -2.0f, pi / 3.0f)) }; + + ExpectPath(path, { Steering::Right, Steering::Straight, Steering::Left }, { 1.379520f, 0.896575f, 2.426718f }); + EXPECT_NEAR(path.Length(), 4.702813f, 1e-4f); +} + +TEST_F(TestDubinsPath, ccc_shortest_is_rlr) +{ + const auto path{ planner.Shortest(start, Transform::FromPose(0.2f, 0.6f, -2.5f)) }; + + ExpectPath(path, { Steering::Right, Steering::Left, Steering::Right }, { 0.752181f, 4.974858f, 0.439492f }); + EXPECT_NEAR(path.Length(), 6.166531f, 1e-4f); +} + +TEST_F(TestDubinsPath, turning_on_the_spot_needs_lrl) +{ + const auto path{ planner.Shortest(start, Transform::FromPose(0.0f, 0.0f, pi / 2.0f)) }; + + ExpectPath(path, { Steering::Left, Steering::Right, Steering::Left }, { 0.424031f, 5.560451f, 0.424031f }); + EXPECT_NEAR(path.Length(), 6.408513f, 1e-4f); +} + +TEST_F(TestDubinsPath, single_arc_goal_survives_the_float_boundary) +{ + const auto goal{ Transform::FromPose(1.0f, 1.0f, pi / 2.0f) }; + const auto path{ planner.Shortest(start, goal) }; + + EXPECT_NEAR(path.Length(), pi / 2.0f, 1e-4f); + ExpectReaches(path, start, goal); +} + +TEST_F(TestDubinsPath, mirroring_the_goal_swaps_left_and_right) +{ + const auto path{ planner.Shortest(start, Transform::FromPose(3.0f, 2.0f, -pi / 3.0f)) }; + + ExpectPath(path, { Steering::Left, Steering::Straight, Steering::Right }, { 1.379520f, 0.896575f, 2.426718f }); +} + +TEST_F(TestDubinsPath, radius_scales_every_length) +{ + const planning::DubinsPlanner wide{ 2.0f }; + + EXPECT_NEAR(wide.Shortest(start, Transform::FromPose(8.0f, 8.0f, pi / 2.0f)).Length(), 11.626874f, 1e-4f); +} + +TEST_F(TestDubinsPath, sample_walks_along_the_path) +{ + const auto goal{ Transform::FromPose(4.0f, 4.0f, pi / 2.0f) }; + const auto path{ planner.Shortest(start, goal) }; + + ExpectPose(path.Sample(start, 0.0f), start); + ExpectPose(path.Sample(start, path.Length() / 2.0f), Transform::FromPose(2.207107f, 1.792893f, pi / 4.0f)); + ExpectPose(path.Sample(start, path.Length()), goal); + ExpectPose(path.Sample(start, path.Length() + 1.0f), goal); +} + +TEST_F(TestDubinsPath, every_shortest_path_reaches_its_goal) +{ + const planning::DubinsPlanner planner15{ 1.5f }; + const auto from{ Transform::FromPose(1.0f, 2.0f, pi / 4.0f) }; + const std::array coordinates{ -3.0f, -1.0f, 0.5f, 2.0f }; + const std::array headings{ -2.5f, -pi / 2.0f, 0.0f, 1.0f, pi }; + + for (const float x : coordinates) + for (const float y : coordinates) + for (const float heading : headings) + { + const auto goal{ Transform::FromPose(x, y, heading) }; + ExpectReaches(planner15.Shortest(from, goal), from, goal); + } +} diff --git a/robotics/planning/test/TestDynamicWindowApproach.cpp b/robotics/planning/test/TestDynamicWindowApproach.cpp new file mode 100644 index 0000000..3501f79 --- /dev/null +++ b/robotics/planning/test/TestDynamicWindowApproach.cpp @@ -0,0 +1,94 @@ +#include "robotics/planning/DynamicWindowApproach.hpp" +#include +#include +#include +#include +#include + +namespace +{ + using Point = math::Vector2; + using Dwa = planning::DynamicWindowApproach; + + class TestDynamicWindowApproach + : public ::testing::Test + { + protected: + static void ExpectWindow(const planning::VelocityWindow& actual, const planning::VelocityWindow& expected) + { + EXPECT_NEAR(actual.vMin, expected.vMin, 1e-6f); + EXPECT_NEAR(actual.vMax, expected.vMax, 1e-6f); + EXPECT_NEAR(actual.omegaMin, expected.omegaMin, 1e-6f); + EXPECT_NEAR(actual.omegaMax, expected.omegaMax, 1e-6f); + } + + static void ExpectCommand(const std::optional>& actual, float v, float omega) + { + ASSERT_TRUE(actual.has_value()); + EXPECT_NEAR(actual->v, v, 1e-5f); + EXPECT_NEAR(actual->omega, omega, 1e-5f); + } + + planning::DwaParameters parameters{ 0.0f, 1.0f, 2.0f, 1.0f, 3.0f, 0.25f, 1.0f, 0.2f, 2.0f, 1.0f, 1.0f, 0.5f }; + Dwa dwa{ parameters }; + Point ahead{ 5.0f, 0.0f }; + }; +} + +TEST_F(TestDynamicWindowApproach, straight_clearance_stops_at_the_disc_contact) +{ + EXPECT_NEAR(Dwa::ArcClearance(1.0f, 0.0f, Point{ 2.0f, 0.1f }, 0.2f, 2.0f), 1.826795f, 1e-5f); + EXPECT_EQ(Dwa::ArcClearance(1.0f, 0.0f, Point{ -1.0f, 0.0f }, 0.2f, 2.0f), 2.0f); +} + +TEST_F(TestDynamicWindowApproach, arc_clearance_on_a_turn) +{ + EXPECT_NEAR(Dwa::ArcClearance(1.0f, 1.0f, Point{ 1.0f, 1.0f }, 0.2f, 2.0f), 1.370461f, 1e-5f); + EXPECT_NEAR(Dwa::ArcClearance(1.0f, -1.0f, Point{ 1.0f, -1.0f }, 0.2f, 2.0f), 1.370461f, 1e-5f); + EXPECT_EQ(Dwa::ArcClearance(1.0f, 1.0f, Point{ 3.0f, 3.0f }, 0.2f, 2.0f), 2.0f); +} + +TEST_F(TestDynamicWindowApproach, contact_and_rotation_in_place) +{ + EXPECT_EQ(Dwa::ArcClearance(1.0f, 0.0f, Point{ 0.1f, 0.05f }, 0.2f, 2.0f), 0.0f); + EXPECT_EQ(Dwa::ArcClearance(0.0f, 1.0f, Point{ 0.5f, 0.0f }, 0.2f, 2.0f), 2.0f); +} + +TEST_F(TestDynamicWindowApproach, window_respects_acceleration_and_bounds) +{ + ExpectWindow(dwa.Window(0.5f, 0.0f), { 0.25f, 0.75f, -0.75f, 0.75f }); + ExpectWindow(dwa.Window(0.95f, 1.9f), { 0.7f, 1.0f, 1.15f, 2.0f }); + ExpectWindow(dwa.Window(0.1f, -1.9f), { 0.0f, 0.35f, -2.0f, -1.15f }); +} + +TEST_F(TestDynamicWindowApproach, free_space_drives_fast_towards_the_goal) +{ + const auto command{ dwa.Compute(0.5f, 0.0f, ahead, {}) }; + + ExpectCommand(command, 0.75f, 0.0f); + EXPECT_EQ(command->clearance, 2.0f); +} + +TEST_F(TestDynamicWindowApproach, goal_to_the_left_turns_left) +{ + ExpectCommand(dwa.Compute(0.5f, 0.0f, Point{ 0.0f, 3.0f }, {}), 0.75f, 0.75f); +} + +TEST_F(TestDynamicWindowApproach, obstacle_slightly_left_turns_right) +{ + const std::array left{ Point{ 0.45f, 0.05f } }; + const std::array right{ Point{ 0.45f, -0.05f } }; + + ExpectCommand(dwa.Compute(0.5f, 0.0f, ahead, left), 0.375f, -0.75f); + ExpectCommand(dwa.Compute(0.5f, 0.0f, ahead, right), 0.375f, 0.75f); +} + +TEST_F(TestDynamicWindowApproach, too_fast_to_stop_returns_nullopt) +{ + std::array wall{}; + for (std::size_t k = 0; k < wall.size(); ++k) + wall[k] = Point{ 0.3f, -1.0f + 0.1f * static_cast(k) }; + + EXPECT_FALSE(dwa.Compute(0.75f, 0.0f, ahead, wall).has_value()); + ExpectCommand(dwa.Compute(0.0f, 0.0f, ahead, wall), 0.0f, 0.0f); +} diff --git a/robotics/planning/test/TestReedsSheppPath.cpp b/robotics/planning/test/TestReedsSheppPath.cpp new file mode 100644 index 0000000..40aa78f --- /dev/null +++ b/robotics/planning/test/TestReedsSheppPath.cpp @@ -0,0 +1,126 @@ +#include "numerical/math/SE2Transform.hpp" +#include "robotics/planning/CarPath.hpp" +#include "robotics/planning/DubinsPath.hpp" +#include "robotics/planning/ReedsSheppPath.hpp" +#include +#include +#include +#include + +namespace +{ + using Transform = math::SE2Transform; + using Path = planning::CarPath; + using planning::Steering; + + constexpr float pi{ std::numbers::pi_v }; + + class TestReedsSheppPath + : public ::testing::Test + { + protected: + template + static void ExpectPath(const Path& path, const std::array& letters, const std::array& lengths) + { + ASSERT_EQ(path.count, N); + for (std::size_t k = 0; k < N; ++k) + { + EXPECT_EQ(path.segments[k].steering, letters[k]); + EXPECT_NEAR(path.segments[k].length, lengths[k], 1e-4f); + } + } + + static void ExpectReaches(const Path& path, const Transform& from, const Transform& goal) + { + const auto error{ (goal.Inverse() * path.Sample(from, path.Length())).Pose() }; + for (std::size_t i = 0; i < 3; ++i) + EXPECT_NEAR(error[i], 0.0f, 1e-4f); + } + + planning::ReedsSheppPlanner planner{ 1.0f }; + Transform start{ Transform::Identity() }; + }; +} + +TEST_F(TestReedsSheppPath, straight_reverse_is_a_single_backward_segment) +{ + const auto goal{ Transform::FromPose(-3.0f, 0.0f, 0.0f) }; + const auto path{ planner.Shortest(start, goal) }; + + EXPECT_NEAR(path.Length(), 3.0f, 1e-4f); + for (std::size_t k = 0; k < path.count; ++k) + EXPECT_NEAR(path.segments[k].length, path.segments[k].steering == Steering::Straight ? -3.0f : 0.0f, 1e-4f); + ExpectReaches(path, start, goal); +} + +TEST_F(TestReedsSheppPath, forward_path_when_it_is_shortest) +{ + const auto path{ planner.Shortest(start, Transform::FromPose(3.0f, 0.5f, 0.2f)) }; + + ExpectPath<3>(path, { Steering::Left, Steering::Straight, Steering::Left }, { 0.169722f, 2.842168f, 0.030278f }); + EXPECT_NEAR(path.Length(), 3.042168f, 1e-4f); +} + +TEST_F(TestReedsSheppPath, sideways_shift_uses_cusps) +{ + const auto goal{ Transform::FromPose(0.0f, 1.0f, 0.0f) }; + const auto path{ planner.Shortest(start, goal) }; + + EXPECT_NEAR(path.Length(), 2.636232f, 1e-4f); + bool forward{ false }; + bool reverse{ false }; + for (std::size_t k = 0; k < path.count; ++k) + { + forward = forward || path.segments[k].length > 1e-4f; + reverse = reverse || path.segments[k].length < -1e-4f; + } + EXPECT_TRUE(forward); + EXPECT_TRUE(reverse); + ExpectReaches(path, start, goal); +} + +TEST_F(TestReedsSheppPath, ccsc_word_with_a_reversed_quarter_turn) +{ + const auto path{ planner.Shortest(start, Transform::FromPose(-2.0f, 1.0f, pi / 2.0f)) }; + + ExpectPath<4>(path, { Steering::Left, Steering::Straight, Steering::Right, Steering::Left }, { -0.729728f, -0.236068f, -pi / 2.0f, 0.729728f }); + EXPECT_NEAR(path.Length(), 3.266320f, 1e-4f); +} + +TEST_F(TestReedsSheppPath, ccc_word_with_a_cusp) +{ + const auto path{ planner.Shortest(start, Transform::FromPose(-1.0f, -1.0f, 0.5f)) }; + + ExpectPath<3>(path, { Steering::Left, Steering::Right, Steering::Left }, { 0.166243f, -0.965586f, -0.631829f }); + EXPECT_NEAR(path.Length(), 1.763658f, 1e-4f); +} + +TEST_F(TestReedsSheppPath, three_point_turn_reverses_heading_in_place) +{ + const auto goal{ Transform::FromPose(0.0f, 0.0f, pi) }; + const auto path{ planner.Shortest(start, goal) }; + + EXPECT_NEAR(path.Length(), pi, 1e-4f); + ExpectReaches(path, start, goal); +} + +TEST_F(TestReedsSheppPath, never_longer_than_dubins_symmetric_and_reaching) +{ + const planning::ReedsSheppPlanner reedsShepp{ 1.5f }; + const planning::DubinsPlanner dubins{ 1.5f }; + const auto from{ Transform::FromPose(1.0f, 2.0f, pi / 4.0f) }; + const std::array coordinates{ -3.0f, -1.0f, 0.5f, 2.0f }; + const std::array headings{ -2.5f, -pi / 2.0f, 0.0f, 1.0f, pi }; + + for (const float x : coordinates) + for (const float y : coordinates) + for (const float heading : headings) + { + const auto goal{ Transform::FromPose(x, y, heading) }; + const auto path{ reedsShepp.Shortest(from, goal) }; + + EXPECT_LE(path.Length(), dubins.Shortest(from, goal).Length() + 1e-5f); + EXPECT_NEAR(reedsShepp.Shortest(goal, from).Length(), path.Length(), 1e-4f); + ExpectReaches(path, from, goal); + } +}