|
| 1 | +# SE(2) Transform, Planar Twists and Wrenches — Implementation Pseudocode |
| 2 | + |
| 3 | +> Roadmap ref: #66 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) |
| 4 | +
|
| 5 | +Planar rigid transform with the twist/wrench algebra of item 53, reduced to the plane: the pose type of |
| 6 | +ground vehicles and planar mechanisms (odometry, path tracking, scan matching). It follows item 53's |
| 7 | +convention: 3-vectors are ordered **linear part first**, twists `(vx, vy, ω)` and wrenches `(fx, fy, τ)`. |
| 8 | +The rotation is stored as a unit complex number `(cos θ, sin θ)`, as Sophus and manif do. Composition, |
| 9 | +`Apply` and `Inverse` then need no trigonometry; only the conversion to and from a heading does. |
| 10 | + |
| 11 | +## Data structures |
| 12 | + |
| 13 | +```cpp |
| 14 | +template<typename T> using Vector2 = math::Vector<T, 2> |
| 15 | +template<typename T> using Matrix2 = math::SquareMatrix<T, 2> |
| 16 | +# planar twist (vx, vy, ω), planar wrench (fx, fy, τ) and the pose triple (x, y, θ) are math::Vector3<T> |
| 17 | + |
| 18 | +template<typename T> # static_assert(std::is_floating_point_v<T>); instantiated for float |
| 19 | +struct SE2Transform: |
| 20 | + T cosine{ 1 } # unit complex rotation (cos θ, sin θ) |
| 21 | + T sine{ 0 } |
| 22 | + Vector2<T> translation{} |
| 23 | + # equivalently the 3×3 homogeneous matrix [[R(θ), p],[0 0 1]] |
| 24 | +``` |
| 25 | +
|
| 26 | +## Interface |
| 27 | +
|
| 28 | +```text |
| 29 | +static SE2Transform Identity() |
| 30 | +static SE2Transform FromPose(T x, T y, T theta) # the only trig on construction |
| 31 | +Vector3<T> Pose() const # (x, y, θ), θ = atan2(sin, cos) ∈ (−π, π] |
| 32 | +T Heading() const |
| 33 | +Matrix2<T> Rotation() const |
| 34 | +friend SE2Transform operator*(const SE2Transform& lhs, const SE2Transform& rhs) # hot path |
| 35 | +SE2Transform Inverse() const # closed form |
| 36 | +Vector2<T> Apply(const Vector2<T>& point) const # R·x + p; hot path |
| 37 | +Vector2<T> Rotate(const Vector2<T>& direction) const # R·x |
| 38 | +SquareMatrix<T, 3> Homogeneous() const |
| 39 | +Matrix3<T> Adjoint() const # twist map for (vx, vy, ω) |
| 40 | +Vector3<T> TransformTwist(const Vector3<T>& twist) const # Adjoint()·twist |
| 41 | +Vector3<T> TransformWrench(const Vector3<T>& wrench) const # dual map, see below |
| 42 | +SE2Transform Normalized() const # rescale (cos, sin) to unit length |
| 43 | +SE3Transform<T> ToSE3() const # rotation about z, translation (x, y, 0) |
| 44 | +
|
| 45 | +static SE2Transform Exp(const Vector3<T>& twist, T theta) # e^{[ξ]θ}; θ is the elapsed time for a velocity twist; hot path |
| 46 | +Vector3<T> Log() const # inverse of Exp with theta = 1 |
| 47 | +static Vector3<T> PoseError(const SE2Transform& target, const SE2Transform& current) |
| 48 | +``` |
| 49 | + |
| 50 | +`operator*` is a hidden friend, as in item 53. The heading is never stored, so `Pose` always returns it |
| 51 | +wrapped. Only `FromPose` checks for a unit rotation: an aggregate-initialised transform must be given a |
| 52 | +unit `(cosine, sine)`. |
| 53 | + |
| 54 | +## Algorithm (pseudocode) |
| 55 | + |
| 56 | +```text |
| 57 | +function FromPose(x, y, θ): return { cos θ, sin θ, (x, y) } |
| 58 | +function Pose(): return (p.x, p.y, atan2(sine, cosine)) |
| 59 | +function Rotation(): return [[c, −s], [s, c]] |
| 60 | +
|
| 61 | +function operator*(a, b): # complex product + rotated offset |
| 62 | + return { a.c·b.c − a.s·b.s, a.s·b.c + a.c·b.s, a.R·b.p + a.p } |
| 63 | +function Inverse(): return { c, −s, −Rᵀ·p } |
| 64 | +
|
| 65 | +function Adjoint(): # V_A = Ad_T · V_B for V = (vx, vy, ω) |
| 66 | + return [[ c, −s, p.y ], |
| 67 | + [ s, c, −p.x ], |
| 68 | + [ 0, 0, 1 ]] |
| 69 | +
|
| 70 | +function TransformWrench(w = (fx, fy, τ)): # power-consistent dual of Adjoint |
| 71 | + f_A = R·(fx, fy) |
| 72 | + return (f_A; τ + p.x·f_A.y − p.y·f_A.x) |
| 73 | +
|
| 74 | +function Normalized(): |
| 75 | + k = 1 / Hypot(c, s); return { c·k, s·k, p } |
| 76 | +
|
| 77 | +function Exp(ξ = (vx, vy, ω), θ): # OPTIMIZE_FOR_SPEED |
| 78 | + φ = ω·θ |
| 79 | + if |φ| < 0.1: # series, no 0/0 and no cancellation |
| 80 | + A = 1 − φ²/6 + φ⁴/120 # sin φ / φ |
| 81 | + B = φ/2 − φ³/24 + φ⁵/720 # (1 − cos φ) / φ |
| 82 | + else: |
| 83 | + A = sin φ / φ |
| 84 | + B = 2·sin²(φ/2) / φ # half-angle form of (1 − cos φ)/φ |
| 85 | + (ux, uy) = (vx·θ, vy·θ) |
| 86 | + return { cos φ, sin φ, (A·ux − B·uy, B·ux + A·uy) } # p = V(φ)·u, V = [[A, −B], [B, A]] |
| 87 | +
|
| 88 | +function Log(): # returns (vx, vy, ω) with ω = Heading() ∈ (−π, π] |
| 89 | + φ = atan2(s, c); h = φ/2 |
| 90 | + if |φ| < 0.1: α = 1 − φ²/12 − φ⁴/720 # h·cot h |
| 91 | + else: α = h·cos h / sin h # finite on the whole branch: α(±π) = 0 |
| 92 | + return (α·p.x + h·p.y, −h·p.x + α·p.y, φ) # V(φ)⁻¹·p |
| 93 | +
|
| 94 | +function PoseError(target, current): # base-frame error, like item 53's PoseError |
| 95 | + eθ = atan2(target.s·current.c − target.c·current.s, |
| 96 | + target.c·current.c + target.s·current.s) # heading of target·currentᵀ, short way |
| 97 | + return (target.p − current.p; eθ) |
| 98 | +``` |
| 99 | + |
| 100 | +## Complexity & memory |
| 101 | + |
| 102 | +- Compose / inverse / apply / rotate / adjoint: `O(1)`, at most 8 multiplies, no trigonometry. |
| 103 | +- `Exp`: one `sin`/`cos` pair plus one `sin` (half angle); `Log`: one `atan2` and one `sin`/`cos` pair. |
| 104 | +- Memory: 4 scalars per transform. No heap. |
| 105 | + |
| 106 | +## Numerical / embedded notes |
| 107 | + |
| 108 | +- **Convention (normative):** twists `(vx, vy, ω)`, wrenches `(fx, fy, τ)`, `PoseError` and `Log` return |
| 109 | + linear part first. robotics-toolbox-cpp's wheeled models, odometry and trackers depend on this ordering. |
| 110 | +- `Log` has no singular point: on the branch `φ ∈ (−π, π]` the coefficient `(φ/2)·cot(φ/2)` stays finite |
| 111 | + and reaches 0 at `±π`. It does return the **wrapped** angle, so `Log(Exp(ξ, θ)) = ξ·θ` only while |
| 112 | + `|ω·θ| < π`. Odometry that must count full turns integrates `Exp` and does not round-trip through `Log`. |
| 113 | +- The series threshold `|φ| < 0.1` mirrors item 53. The truncation error there is `≈1.6e-10`, below |
| 114 | + `float` resolution, so `Exp` is continuous across the switch. |
| 115 | +- Composing in `float` lets `cos² + sin²` drift from 1 by roughly one ulp per product. A transform that is |
| 116 | + integrated over time (odometry) calls `Normalized` every step, which costs one `Hypot`. Single compositions |
| 117 | + need no renormalisation. |
| 118 | +- `FromPose` accepts any angle; no wrapping is needed because only `cos θ` and `sin θ` are stored. |
| 119 | +- Float-only: `static_assert(std::is_floating_point_v<T>)`. |
| 120 | + |
| 121 | +## Deployment |
| 122 | + |
| 123 | +- Header: `numerical/math/SE2Transform.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on |
| 124 | + `operator*`/`Apply`/`Exp`, and `extern template struct SE2Transform<float>;` under |
| 125 | + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. Includes `SE3Transform.hpp` for `ToSE3` and `Vector3`. |
| 126 | +- Coverage: `numerical/math/SE2Transform.cpp` → `template struct SE2Transform<float>;` |
| 127 | +- Test: `numerical/math/test/TestSE2Transform.cpp` |
| 128 | +- Doc: `doc/math/SE2Transform.md` (per `doc/TEMPLATE.md`) + row in `doc/math/README.md` |
| 129 | +- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → |
| 130 | + `numerical_add_coverage_sources(numerical.math ...)`; `TestSE2Transform.cpp` → `numerical.math_test`. |
| 131 | +- Generic pattern: see `roadmap/DEPLOYMENT.md`. |
0 commit comments