diff --git a/ROADMAP.md b/ROADMAP.md index 644cd11..586914e 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -37,7 +37,8 @@ Difficulty legend: Items 66–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as NT66–NT73; NT53–NT65 are deployed). They serve its mobile, aerial, legged, marine and localization -families, also have non-robotics consumers, and have no spec yet. All are *(float-first)*. +families and also have non-robotics consumers. Item 66 has a spec under [`roadmap/`](roadmap/README.md); +items 67–73 have none yet. All are *(float-first)*. Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family unit-test reference [`TESTING.md`](TESTING.md) depends on but which @@ -53,7 +54,8 @@ the library does not yet expose. Detailed below under tracking, scan matching — at a fraction of SE(3)'s cost. - **Algorithm / paper:** J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics" (2018), Appendix C. -- **Reuses:** Item 54 (angle wrapping). +- **Reuses:** Item 53 (ordering convention, `ToSE3`). +- **Spec:** [roadmap/math/SE2Transform](roadmap/math/SE2Transform/implementation.md). ### 67. Kabsch / Umeyama alignment - **What:** Least-squares rotation, translation and optional scale between two corresponding point sets diff --git a/roadmap/README.md b/roadmap/README.md index d37a960..9a6061d 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -101,7 +101,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `LmsAdaptiveFilter` (21) ### `math` -`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) +`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE2Transform` (66) ### `control_analysis` `ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32) diff --git a/roadmap/math/SE2Transform/explanation.md b/roadmap/math/SE2Transform/explanation.md new file mode 100644 index 0000000..f16bc81 --- /dev/null +++ b/roadmap/math/SE2Transform/explanation.md @@ -0,0 +1,35 @@ +# SE(2) Transform, Planar Twists and Wrenches — Overview + +## What it is +The algebra of motion in a plane: a pose is a heading plus a 2-D position; a *planar twist* is a +body's instantaneous velocity (forward, sideways, turn rate); a *planar wrench* is a force in the plane +together with a moment about the vertical axis. The adjoint moves twists and wrenches between frames, and +the exponential and logarithm convert between a constant twist held for some time and the finite motion +it produces. + +## Why it matters (embedded) +Ground robots, mobile bases, warehouse vehicles and planar mechanisms work with poses in the plane. +Odometry, path tracking, landmark and scan-matching localization all need the same few operations. +SE(2) costs a fraction of SE(3): four numbers per pose and no trigonometry in composition. Writing it +once, with item 53's ordering, keeps planar and spatial code consistent when a mobile base carries an arm. + +## How it works (intuition) +The rotation is stored as the cosine and sine of the heading, so composing two poses multiplies two unit +complex numbers and rotates one offset — no angle arithmetic and no wrap-around bugs. A constant twist +held for a time moves the body along a circular arc (a straight line when the turn rate is zero). The +exponential computes the end of that arc exactly, which is why it is the right way to integrate wheel +odometry. The logarithm runs the other way: it returns the constant twist that would carry one pose to +another in unit time, and so gives a pose difference that respects the vehicle's geometry. + +## Key parameters +- **Ordering convention** — linear part first for twists `(vx, vy, ω)` and wrenches `(fx, fy, τ)`. +- **Heading branch** — headings and `Log` angles are reported in `(−π, π]`. +- **Unit rotation** — `(cos, sin)` kept unit length; renormalised when a pose is integrated over time. + +## Reference +J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics," +arXiv:1812.01537 (2018), Appendix C; K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 3 and 13. + +## See also +`SE3Transform` (item 53, the spatial version and `ToSE3`'s target), `AngleUtilities` (item 54, heading +wrap and differences); robotics-toolbox-cpp's wheeled-robot models and planar odometry. diff --git a/roadmap/math/SE2Transform/implementation.md b/roadmap/math/SE2Transform/implementation.md new file mode 100644 index 0000000..53a4ab7 --- /dev/null +++ b/roadmap/math/SE2Transform/implementation.md @@ -0,0 +1,131 @@ +# SE(2) Transform, Planar Twists and Wrenches — Implementation Pseudocode + +> Roadmap ref: #66 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) + +Planar rigid transform with the twist/wrench algebra of item 53, reduced to the plane: the pose type of +ground vehicles and planar mechanisms (odometry, path tracking, scan matching). It follows item 53's +convention: 3-vectors are ordered **linear part first**, twists `(vx, vy, ω)` and wrenches `(fx, fy, τ)`. +The rotation is stored as a unit complex number `(cos θ, sin θ)`, as Sophus and manif do. Composition, +`Apply` and `Inverse` then need no trigonometry; only the conversion to and from a heading does. + +## Data structures + +```cpp +template using Vector2 = math::Vector +template using Matrix2 = math::SquareMatrix +# planar twist (vx, vy, ω), planar wrench (fx, fy, τ) and the pose triple (x, y, θ) are math::Vector3 + +template # static_assert(std::is_floating_point_v); instantiated for float +struct SE2Transform: + T cosine{ 1 } # unit complex rotation (cos θ, sin θ) + T sine{ 0 } + Vector2 translation{} + # equivalently the 3×3 homogeneous matrix [[R(θ), p],[0 0 1]] +``` + +## Interface + +```text +static SE2Transform Identity() +static SE2Transform FromPose(T x, T y, T theta) # the only trig on construction +Vector3 Pose() const # (x, y, θ), θ = atan2(sin, cos) ∈ (−π, π] +T Heading() const +Matrix2 Rotation() const +friend SE2Transform operator*(const SE2Transform& lhs, const SE2Transform& rhs) # hot path +SE2Transform Inverse() const # closed form +Vector2 Apply(const Vector2& point) const # R·x + p; hot path +Vector2 Rotate(const Vector2& direction) const # R·x +SquareMatrix Homogeneous() const +Matrix3 Adjoint() const # twist map for (vx, vy, ω) +Vector3 TransformTwist(const Vector3& twist) const # Adjoint()·twist +Vector3 TransformWrench(const Vector3& wrench) const # dual map, see below +SE2Transform Normalized() const # rescale (cos, sin) to unit length +SE3Transform ToSE3() const # rotation about z, translation (x, y, 0) + +static SE2Transform Exp(const Vector3& twist, T theta) # e^{[ξ]θ}; θ is the elapsed time for a velocity twist; hot path +Vector3 Log() const # inverse of Exp with theta = 1 +static Vector3 PoseError(const SE2Transform& target, const SE2Transform& current) +``` + +`operator*` is a hidden friend, as in item 53. The heading is never stored, so `Pose` always returns it +wrapped. Only `FromPose` checks for a unit rotation: an aggregate-initialised transform must be given a +unit `(cosine, sine)`. + +## Algorithm (pseudocode) + +```text +function FromPose(x, y, θ): return { cos θ, sin θ, (x, y) } +function Pose(): return (p.x, p.y, atan2(sine, cosine)) +function Rotation(): return [[c, −s], [s, c]] + +function operator*(a, b): # complex product + rotated offset + return { a.c·b.c − a.s·b.s, a.s·b.c + a.c·b.s, a.R·b.p + a.p } +function Inverse(): return { c, −s, −Rᵀ·p } + +function Adjoint(): # V_A = Ad_T · V_B for V = (vx, vy, ω) + return [[ c, −s, p.y ], + [ s, c, −p.x ], + [ 0, 0, 1 ]] + +function TransformWrench(w = (fx, fy, τ)): # power-consistent dual of Adjoint + f_A = R·(fx, fy) + return (f_A; τ + p.x·f_A.y − p.y·f_A.x) + +function Normalized(): + k = 1 / Hypot(c, s); return { c·k, s·k, p } + +function Exp(ξ = (vx, vy, ω), θ): # OPTIMIZE_FOR_SPEED + φ = ω·θ + if |φ| < 0.1: # series, no 0/0 and no cancellation + A = 1 − φ²/6 + φ⁴/120 # sin φ / φ + B = φ/2 − φ³/24 + φ⁵/720 # (1 − cos φ) / φ + else: + A = sin φ / φ + B = 2·sin²(φ/2) / φ # half-angle form of (1 − cos φ)/φ + (ux, uy) = (vx·θ, vy·θ) + return { cos φ, sin φ, (A·ux − B·uy, B·ux + A·uy) } # p = V(φ)·u, V = [[A, −B], [B, A]] + +function Log(): # returns (vx, vy, ω) with ω = Heading() ∈ (−π, π] + φ = atan2(s, c); h = φ/2 + if |φ| < 0.1: α = 1 − φ²/12 − φ⁴/720 # h·cot h + else: α = h·cos h / sin h # finite on the whole branch: α(±π) = 0 + return (α·p.x + h·p.y, −h·p.x + α·p.y, φ) # V(φ)⁻¹·p + +function PoseError(target, current): # base-frame error, like item 53's PoseError + eθ = atan2(target.s·current.c − target.c·current.s, + target.c·current.c + target.s·current.s) # heading of target·currentᵀ, short way + return (target.p − current.p; eθ) +``` + +## Complexity & memory + +- Compose / inverse / apply / rotate / adjoint: `O(1)`, at most 8 multiplies, no trigonometry. +- `Exp`: one `sin`/`cos` pair plus one `sin` (half angle); `Log`: one `atan2` and one `sin`/`cos` pair. +- Memory: 4 scalars per transform. No heap. + +## Numerical / embedded notes + +- **Convention (normative):** twists `(vx, vy, ω)`, wrenches `(fx, fy, τ)`, `PoseError` and `Log` return + linear part first. robotics-toolbox-cpp's wheeled models, odometry and trackers depend on this ordering. +- `Log` has no singular point: on the branch `φ ∈ (−π, π]` the coefficient `(φ/2)·cot(φ/2)` stays finite + and reaches 0 at `±π`. It does return the **wrapped** angle, so `Log(Exp(ξ, θ)) = ξ·θ` only while + `|ω·θ| < π`. Odometry that must count full turns integrates `Exp` and does not round-trip through `Log`. +- The series threshold `|φ| < 0.1` mirrors item 53. The truncation error there is `≈1.6e-10`, below + `float` resolution, so `Exp` is continuous across the switch. +- Composing in `float` lets `cos² + sin²` drift from 1 by roughly one ulp per product. A transform that is + integrated over time (odometry) calls `Normalized` every step, which costs one `Hypot`. Single compositions + need no renormalisation. +- `FromPose` accepts any angle; no wrapping is needed because only `cos θ` and `sin θ` are stored. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `numerical/math/SE2Transform.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on + `operator*`/`Apply`/`Exp`, and `extern template struct SE2Transform;` under + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. Includes `SE3Transform.hpp` for `ToSE3` and `Vector3`. +- Coverage: `numerical/math/SE2Transform.cpp` → `template struct SE2Transform;` +- Test: `numerical/math/test/TestSE2Transform.cpp` +- Doc: `doc/math/SE2Transform.md` (per `doc/TEMPLATE.md`) + row in `doc/math/README.md` +- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → + `numerical_add_coverage_sources(numerical.math ...)`; `TestSE2Transform.cpp` → `numerical.math_test`. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/math/SE2Transform/tests.md b/roadmap/math/SE2Transform/tests.md new file mode 100644 index 0000000..027d2da --- /dev/null +++ b/roadmap/math/SE2Transform/tests.md @@ -0,0 +1,69 @@ +# SE(2) Transform — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestSE2Transform : public ::testing::Test: + SE2Transform a{ FromPose(1, 2, π/2) } + SE2Transform b{ FromPose(−0.5, 0.3, −π/3) } + Vector3 twist{ 0.4, −0.2, 0.7 } + tolerance 1e-5 +# each case below is a TEST_F(TestSE2Transform, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +apply_rotates_then_translates: + Assert: a.Apply((1, 0)) ≈ (1, 3) +composition_applies_right_operand_first: + Assert: (a * b).Apply(x) ≈ a.Apply(b.Apply(x)) for x = (0.3, −0.2); (a * b).Pose() ≈ (0.7, 1.5, π/6) +inverse_composes_to_identity: + Assert: a.Inverse().Pose() ≈ (−2, 1, −π/2); a * a.Inverse() ≈ Identity and a.Inverse() * a ≈ Identity +pose_returns_wrapped_heading: + Assert: FromPose(0, 0, 3π/2).Heading() ≈ −π/2; b.Pose() ≈ (−0.5, 0.3, −π/3) +homogeneous_matches_block_form: + Assert: a.Homogeneous() == [[0, −1, 1], [1, 0, 2], [0, 0, 1]] +adjoint_maps_twists_consistently_with_composition: + Assert: Exp(a.TransformTwist(twist), t) ≈ a * Exp(twist, t) * a.Inverse() for t = 0.4 and t = 2 +adjoint_of_product_is_product_of_adjoints: + Assert: (a * b).Adjoint() ≈ a.Adjoint() · b.Adjoint() +wrench_and_twist_power_is_frame_invariant: + Arrange: wrench W = (1.5, −0.7, 0.3) + Assert: a.TransformWrench(W) · a.TransformTwist(twist) ≈ W · twist = 0.95 +exp_of_unicycle_arc: + Assert: Exp((1, 0, 1), π/2).Pose() ≈ (1, 1, π/2) # quarter circle of radius 1 +exp_of_pure_translation: + Assert: Exp((0.3, −0.4, 0), 2).Pose() ≈ (0.6, −0.8, 0) +exp_is_continuous_across_series_threshold: + Arrange: ξ = (0.8, 0.3, φ), θ = 1, φ ∈ {0.1 − 1e-4, 0.1 + 1e-4} + Assert: both translations agree with the closed form V(φ)·(0.8, 0.3) to 1e-6 +log_inverts_exp: + Assert: a.Log() ≈ (3π/4, π/4, π/2); Exp(T.Log(), 1) ≈ T for T = a, b, FromPose(0.2, −0.4, 179.9°) +log_at_half_turn_is_finite: + Assert: FromPose(1, 0, π).Log() ≈ (0, −π/2, π) +pose_error_takes_the_short_way: + Arrange: current = FromPose(1, 1, 0.2), target = FromPose(2, 0.5, 0.2 + 190°) + Assert: PoseError(target, current) ≈ (1, −0.5, −170°) # −2.967060 rad +normalized_restores_unit_rotation: + Arrange: T = { 1.001·cos 0.3, 1.001·sin 0.3, (1, 2) } + Assert: T.Normalized() has cos² + sin² ≈ 1, heading 0.3, translation (1, 2) +to_se3_rotates_about_z: + Assert: a.ToSE3() ≈ { RotationAboutAxis(ẑ, π/2), (1, 2, 0) } +``` + +## Reference vectors + +- `FromPose(1, 2, π/2)` applied to `(1, 0)` ⇒ `(1, 3)`; its inverse is `(−2, 1, −π/2)`. +- `a * b` ⇒ `(0.7, 1.5, π/6)`. +- `a.Log()` ⇒ `(3π/4, π/4, π/2) = (2.356194, 0.785398, 1.570796)`; `b.Log()` ⇒ `(−0.610529, 0.010271, −π/3)`. +- `Exp` agrees with a double-precision 3×3 matrix exponential of `[[0, −ω, vx], [ω, 0, vy], [0, 0, 0]]·θ` + to `8e-10` over 2000 random twists; `Exp(Log(T))` round-trips to `6e-10`. + +## Edge cases + +- `ω = 0` (pure translation) and `|ω·θ| > π` in `Exp` (the result is still exact; only `Log` wraps). +- `Log` at heading exactly `π`: the branch returns `+π`, never `−π`. +- 10⁵ compositions of a small step with `Normalized` after each keep `cos² + sin² = 1` to `1e-6`.