Skip to content

Commit 63c9b91

Browse files
docs(roadmap): add the SE(2) transform spec (item 66) (#363)
Planar counterpart of item 53 for robotics-toolbox-cpp's wheeled family, with the same linear-first ordering: twists (vx, vy, ω), wrenches (fx, fy, τ). The rotation is stored as a unit complex number, so composition, Apply and Inverse need no trigonometry. Exp/Log use item 53's 0.1 series threshold; Log is finite on the whole (−π, π] branch. Normalized handles drift in integrated poses; ToSE3 lifts the pose for mobile manipulators. The test vectors were checked in double: Exp matches a 3×3 matrix exponential to 8e-10 over 2000 random twists, and Exp(Log(T)) round-trips to 6e-10. Claude-Session: https://claude.ai/code/session_01Mmp77ieEy16GxJL8KsLsVx Co-authored-by: Claude <noreply@anthropic.com>
1 parent d611820 commit 63c9b91

5 files changed

Lines changed: 240 additions & 3 deletions

File tree

‎ROADMAP.md‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,8 @@ Difficulty legend:
3737

3838
Items 66–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as
3939
NT66–NT73; NT53–NT65 are deployed). They serve its mobile, aerial, legged, marine and localization
40-
families, also have non-robotics consumers, and have no spec yet. All are *(float-first)*.
40+
families and also have non-robotics consumers. Item 66 has a spec under [`roadmap/`](roadmap/README.md);
41+
items 67–73 have none yet. All are *(float-first)*.
4142

4243
Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family
4344
unit-test reference [`TESTING.md`](TESTING.md) depends on but which
@@ -53,7 +54,8 @@ the library does not yet expose. Detailed below under
5354
tracking, scan matching — at a fraction of SE(3)'s cost.
5455
- **Algorithm / paper:** J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in
5556
robotics" (2018), Appendix C.
56-
- **Reuses:** Item 54 (angle wrapping).
57+
- **Reuses:** Item 53 (ordering convention, `ToSE3`).
58+
- **Spec:** [roadmap/math/SE2Transform](roadmap/math/SE2Transform/implementation.md).
5759

5860
### 67. Kabsch / Umeyama alignment
5961
- **What:** Least-squares rotation, translation and optional scale between two corresponding point sets

‎roadmap/README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,7 @@ with `EXPECT_NEAR` and `math::Tolerance<float>()` (or an explicit tolerance).
101101
`LmsAdaptiveFilter` (21)
102102

103103
### `math`
104-
`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30)
104+
`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE2Transform` (66)
105105

106106
### `control_analysis`
107107
`ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32)
Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
# SE(2) Transform, Planar Twists and Wrenches — Overview
2+
3+
## What it is
4+
The algebra of motion in a plane: a pose is a heading plus a 2-D position; a *planar twist* is a
5+
body's instantaneous velocity (forward, sideways, turn rate); a *planar wrench* is a force in the plane
6+
together with a moment about the vertical axis. The adjoint moves twists and wrenches between frames, and
7+
the exponential and logarithm convert between a constant twist held for some time and the finite motion
8+
it produces.
9+
10+
## Why it matters (embedded)
11+
Ground robots, mobile bases, warehouse vehicles and planar mechanisms work with poses in the plane.
12+
Odometry, path tracking, landmark and scan-matching localization all need the same few operations.
13+
SE(2) costs a fraction of SE(3): four numbers per pose and no trigonometry in composition. Writing it
14+
once, with item 53's ordering, keeps planar and spatial code consistent when a mobile base carries an arm.
15+
16+
## How it works (intuition)
17+
The rotation is stored as the cosine and sine of the heading, so composing two poses multiplies two unit
18+
complex numbers and rotates one offset — no angle arithmetic and no wrap-around bugs. A constant twist
19+
held for a time moves the body along a circular arc (a straight line when the turn rate is zero). The
20+
exponential computes the end of that arc exactly, which is why it is the right way to integrate wheel
21+
odometry. The logarithm runs the other way: it returns the constant twist that would carry one pose to
22+
another in unit time, and so gives a pose difference that respects the vehicle's geometry.
23+
24+
## Key parameters
25+
- **Ordering convention** — linear part first for twists `(vx, vy, ω)` and wrenches `(fx, fy, τ)`.
26+
- **Heading branch** — headings and `Log` angles are reported in `(−π, π]`.
27+
- **Unit rotation** — `(cos, sin)` kept unit length; renormalised when a pose is integrated over time.
28+
29+
## Reference
30+
J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics,"
31+
arXiv:1812.01537 (2018), Appendix C; K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 3 and 13.
32+
33+
## See also
34+
`SE3Transform` (item 53, the spatial version and `ToSE3`'s target), `AngleUtilities` (item 54, heading
35+
wrap and differences); robotics-toolbox-cpp's wheeled-robot models and planar odometry.
Lines changed: 131 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,131 @@
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`.

‎roadmap/math/SE2Transform/tests.md‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
# SE(2) Transform — Unit Test Plan (Pseudocode)
2+
3+
> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap.
4+
5+
## Fixture
6+
7+
```cpp
8+
class TestSE2Transform : public ::testing::Test:
9+
SE2Transform<float> a{ FromPose(1, 2, π/2) }
10+
SE2Transform<float> b{ FromPose(−0.5, 0.3, −π/3) }
11+
Vector3<float> twist{ 0.4, −0.2, 0.7 }
12+
tolerance 1e-5
13+
# each case below is a TEST_F(TestSE2Transform, <name>)
14+
```
15+
16+
## Test cases (Arrange / Act / Assert)
17+
18+
```cpp
19+
apply_rotates_then_translates:
20+
Assert: a.Apply((1, 0)) ≈ (1, 3)
21+
composition_applies_right_operand_first:
22+
Assert: (a * b).Apply(x) ≈ a.Apply(b.Apply(x)) for x = (0.3, −0.2); (a * b).Pose() ≈ (0.7, 1.5, π/6)
23+
inverse_composes_to_identity:
24+
Assert: a.Inverse().Pose() ≈ (−2, 1, −π/2); a * a.Inverse() ≈ Identity and a.Inverse() * a ≈ Identity
25+
pose_returns_wrapped_heading:
26+
Assert: FromPose(0, 0, 3π/2).Heading() ≈ −π/2; b.Pose() ≈ (−0.5, 0.3, −π/3)
27+
homogeneous_matches_block_form:
28+
Assert: a.Homogeneous() == [[0, −1, 1], [1, 0, 2], [0, 0, 1]]
29+
adjoint_maps_twists_consistently_with_composition:
30+
Assert: Exp(a.TransformTwist(twist), t) ≈ a * Exp(twist, t) * a.Inverse() for t = 0.4 and t = 2
31+
adjoint_of_product_is_product_of_adjoints:
32+
Assert: (a * b).Adjoint() ≈ a.Adjoint() · b.Adjoint()
33+
wrench_and_twist_power_is_frame_invariant:
34+
Arrange: wrench W = (1.5, −0.7, 0.3)
35+
Assert: a.TransformWrench(W) · a.TransformTwist(twist) ≈ W · twist = 0.95
36+
exp_of_unicycle_arc:
37+
Assert: Exp((1, 0, 1), π/2).Pose() ≈ (1, 1, π/2) # quarter circle of radius 1
38+
exp_of_pure_translation:
39+
Assert: Exp((0.3, −0.4, 0), 2).Pose() ≈ (0.6, −0.8, 0)
40+
exp_is_continuous_across_series_threshold:
41+
Arrange: ξ = (0.8, 0.3, φ), θ = 1, φ ∈ {0.1 − 1e-4, 0.1 + 1e-4}
42+
Assert: both translations agree with the closed form V(φ)·(0.8, 0.3) to 1e-6
43+
log_inverts_exp:
44+
Assert: a.Log() ≈ (3π/4, π/4, π/2); Exp(T.Log(), 1) ≈ T for T = a, b, FromPose(0.2, −0.4, 179.9°)
45+
log_at_half_turn_is_finite:
46+
Assert: FromPose(1, 0, π).Log() ≈ (0, −π/2, π)
47+
pose_error_takes_the_short_way:
48+
Arrange: current = FromPose(1, 1, 0.2), target = FromPose(2, 0.5, 0.2 + 190°)
49+
Assert: PoseError(target, current) ≈ (1, −0.5, −170°) # −2.967060 rad
50+
normalized_restores_unit_rotation:
51+
Arrange: T = { 1.001·cos 0.3, 1.001·sin 0.3, (1, 2) }
52+
Assert: T.Normalized() has cos² + sin² ≈ 1, heading 0.3, translation (1, 2)
53+
to_se3_rotates_about_z:
54+
Assert: a.ToSE3() ≈ { RotationAboutAxis(ẑ, π/2), (1, 2, 0) }
55+
```
56+
57+
## Reference vectors
58+
59+
- `FromPose(1, 2, π/2)` applied to `(1, 0)` ⇒ `(1, 3)`; its inverse is `(−2, 1, −π/2)`.
60+
- `a * b` ⇒ `(0.7, 1.5, π/6)`.
61+
- `a.Log()` ⇒ `(3π/4, π/4, π/2) = (2.356194, 0.785398, 1.570796)`; `b.Log()` ⇒ `(−0.610529, 0.010271, −π/3)`.
62+
- `Exp` agrees with a double-precision 3×3 matrix exponential of `[[0, −ω, vx], [ω, 0, vy], [0, 0, 0]]·θ`
63+
to `8e-10` over 2000 random twists; `Exp(Log(T))` round-trips to `6e-10`.
64+
65+
## Edge cases
66+
67+
- `ω = 0` (pure translation) and `|ω·θ| > π` in `Exp` (the result is still exact; only `Log` wraps).
68+
- `Log` at heading exactly `π`: the branch returns `+π`, never `−π`.
69+
- 10⁵ compositions of a small step with `Normalized` after each keep `cos² + sin² = 1` to `1e-6`.

0 commit comments

Comments
 (0)