Phase VI — Lie Groups & Manifold Optimization | Week 11 | 2.5 hours Seven days, one unified rotation toolkit. Prove you own it.
This review consolidates everything needed to reason about robot orientation: from abstract group axioms to the practical choice of quaternions in tf2, from Rodrigues' formula in the IMU driver to SLERP in the trajectory planner. The full pipeline — read IMU quaternion → convert → compose → display Euler — runs every control cycle on every OKS AMR.
Group Axioms (Day 71)
├── Matrix Groups: GL(n) ⊃ O(n) ⊃ SO(n)
│
├── SO(2) (Day 72)
│ ├── 2×2 rotation matrix
│ ├── exp/log maps (angle ↔ matrix)
│ └── Abelian — rotations commute
│
└── SO(3) (Days 73–76)
├── Rodrigues' Formula (Day 73)
│ ├── axis-angle → matrix via exp map
│ └── hat/vee operators
├── Quaternions (Day 74)
│ ├── unit quaternion ↔ rotation
│ └── double cover (q = -q)
├── SLERP & Averaging (Day 75)
│ ├── geodesic interpolation
│ └── Fréchet mean on S³
└── Representations Compared (Day 76)
├── Euler angles (gimbal lock)
├── axis-angle (singularity at θ=0,π)
├── quaternion (double cover)
└── matrix (9 params, 6 constraints)
| Formula | Expression |
|---|---|
| SO(2) exp | $R(\theta) = \begin{pmatrix}\cos\theta & -\sin\theta \\ \sin\theta & \cos\theta\end{pmatrix}$ |
| SO(2) log | $\theta = \text{atan2}(R_{10}, R_{00})$ |
| Hat operator | $[\omega]_\times = \begin{pmatrix}0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0\end{pmatrix}$ |
| Rodrigues | $R = I + \sin\theta\,[\hat{n}]_\times + (1-\cos\theta)\,[\hat{n}]_\times^2$ |
| SO(3) log | $\theta = \arccos\frac{\text{tr}(R)-1}{2}$, $\omega = \frac{\theta}{2\sin\theta}\text{vee}(R-R^T)$ |
| Quat → angle | $\theta = 2\arccos\|w\|$ |
| SLERP | $q(t) = \frac{\sin((1-t)\Omega)}{\sin\Omega}q_0 + \frac{\sin(t\Omega)}{\sin\Omega}q_1$ |
import numpy as np
from code.lie_groups.lie_groups import (
so3_exp, so3_log, hat, vee,
angle_axis_to_quaternion, quaternion_to_rotation, quaternion_slerp
)
class RotationConverter:
"""Unified rotation representation converter."""
@staticmethod
def euler_to_matrix(roll, pitch, yaw):
"""ZYX Euler (radians) → 3×3 rotation matrix."""
cr, sr = np.cos(roll), np.sin(roll)
cp, sp = np.cos(pitch), np.sin(pitch)
cy, sy = np.cos(yaw), np.sin(yaw)
return np.array([
[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
[sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
[-sp, cp*sr, cp*cr]
])
@staticmethod
def matrix_to_euler(R):
"""3×3 rotation matrix → ZYX Euler angles (roll, pitch, yaw)."""
sy = np.sqrt(R[0,0]**2 + R[1,0]**2)
if sy > 1e-6:
roll = np.arctan2(R[2,1], R[2,2])
pitch = np.arctan2(-R[2,0], sy)
yaw = np.arctan2(R[1,0], R[0,0])
else:
roll = np.arctan2(-R[1,2], R[1,1])
pitch = np.arctan2(-R[2,0], sy)
yaw = 0.0
return roll, pitch, yaw
@staticmethod
def axis_angle_to_matrix(omega):
return so3_exp(np.asarray(omega, dtype=float))
@staticmethod
def matrix_to_axis_angle(R):
return so3_log(R)
@staticmethod
def quaternion_to_matrix(q):
return quaternion_to_rotation(np.asarray(q, dtype=float))
@staticmethod
def matrix_to_quaternion(R):
tr = np.trace(R)
if tr > 0:
s = 2.0 * np.sqrt(tr + 1.0)
return np.array([0.25*s, (R[2,1]-R[1,2])/s,
(R[0,2]-R[2,0])/s, (R[1,0]-R[0,1])/s])
elif R[0,0] > R[1,1] and R[0,0] > R[2,2]:
s = 2.0 * np.sqrt(1+R[0,0]-R[1,1]-R[2,2])
return np.array([(R[2,1]-R[1,2])/s, 0.25*s,
(R[0,1]+R[1,0])/s, (R[0,2]+R[2,0])/s])
elif R[1,1] > R[2,2]:
s = 2.0 * np.sqrt(1+R[1,1]-R[0,0]-R[2,2])
return np.array([(R[0,2]-R[2,0])/s, (R[0,1]+R[1,0])/s,
0.25*s, (R[1,2]+R[2,1])/s])
else:
s = 2.0 * np.sqrt(1+R[2,2]-R[0,0]-R[1,1])
return np.array([(R[1,0]-R[0,1])/s, (R[0,2]+R[2,0])/s,
(R[1,2]+R[2,1])/s, 0.25*s])
def convert(self, value, from_repr, to_repr):
"""Convert between any two representations.
Representations: 'euler', 'axis_angle', 'quaternion', 'matrix'
"""
# First convert to matrix as canonical form
if from_repr == 'euler':
R = self.euler_to_matrix(*value)
elif from_repr == 'axis_angle':
R = self.axis_angle_to_matrix(value)
elif from_repr == 'quaternion':
R = self.quaternion_to_matrix(value)
elif from_repr == 'matrix':
R = np.array(value)
else:
raise ValueError(f"Unknown repr: {from_repr}")
# Then convert from matrix to target
if to_repr == 'matrix':
return R
elif to_repr == 'euler':
return self.matrix_to_euler(R)
elif to_repr == 'axis_angle':
return self.matrix_to_axis_angle(R)
elif to_repr == 'quaternion':
return self.matrix_to_quaternion(R)
else:
raise ValueError(f"Unknown repr: {to_repr}")
# --- Demo: full pipeline ---
rc = RotationConverter()
print("=== Full Conversion Pipeline ===")
# Start with IMU quaternion (ROS convention: x,y,z,w → our convention: w,x,y,z)
imu_quat_ros = [0.0, 0.0, 0.383, 0.924] # ~45° around z
imu_quat = [imu_quat_ros[3], *imu_quat_ros[:3]] # convert to w,x,y,z
R = rc.convert(imu_quat, 'quaternion', 'matrix')
euler = rc.convert(imu_quat, 'quaternion', 'euler')
aa = rc.convert(imu_quat, 'quaternion', 'axis_angle')
print(f"IMU quaternion (w,x,y,z): [{imu_quat[0]:.3f}, {imu_quat[1]:.3f},"
f" {imu_quat[2]:.3f}, {imu_quat[3]:.3f}]")
print(f"Rotation matrix:\n{np.round(R, 4)}")
print(f"Euler (deg): roll={np.degrees(euler[0]):.1f}, pitch={np.degrees(euler[1]):.1f},"
f" yaw={np.degrees(euler[2]):.1f}")
print(f"Axis-angle: {np.round(aa, 4)} (angle={np.degrees(np.linalg.norm(aa)):.1f}°)")
# --- Compose: apply an additional rotation ---
delta_omega = np.array([0.0, 0.0, np.radians(15)]) # 15° yaw increment
R_delta = rc.axis_angle_to_matrix(delta_omega)
R_composed = R @ R_delta
euler_new = rc.matrix_to_euler(R_composed)
print(f"\nAfter +15° yaw: roll={np.degrees(euler_new[0]):.1f}°,"
f" pitch={np.degrees(euler_new[1]):.1f}°, yaw={np.degrees(euler_new[2]):.1f}°")
# --- Roundtrip error check ---
print(f"\n=== Roundtrip Error Check ===")
reprs = ['euler', 'axis_angle', 'quaternion']
for src in reprs:
if src == 'euler':
val = (0.3, -0.2, 1.1)
elif src == 'axis_angle':
val = np.array([0.3, -0.2, 1.1])
else:
val = angle_axis_to_quaternion([0.3, -0.2, 1.1])
for dst in reprs:
if dst == src:
continue
intermediate = rc.convert(val, src, dst)
back = rc.convert(intermediate, dst, src)
if src == 'euler':
err = max(abs(a-b) for a, b in zip(val, back))
else:
err = np.max(np.abs(np.array(val) - np.array(back)))
print(f" {src:12s} → {dst:12s} → {src:12s}: max error = {err:.2e}")
Q1: What is $\det(R)$ for any $R \in \text{SO}(3)$?
Q2: How many DOF does a 3D rotation have?
Q3: What is the identity quaternion?
Q4: If $q$ represents a rotation, what does $-q$ represent?
Q5: At what Euler pitch angle does gimbal lock occur?
Q6: What is $\text{tr}(R)$ for a rotation by angle $\theta$?
B1: An IMU outputs $q = (0.707, 0, 0, 0.707)$. What is this rotation in Euler angles?
B2: Compose two rotations: 45° around x followed by 90° around z. Express the result as axis-angle.
B3: SLERP between $q_0 = (1,0,0,0)$ and $q_1 = (0,1,0,0)$ at $t=0.5$. What rotation does this represent?
Challenge 1: Prove that SO(3) is a 3-dimensional smooth manifold.
Challenge 2: Derive the Haar (uniform) measure on SO(3) in axis-angle coordinates: $d\mu = \frac{2(1-\cos\theta)}{\pi\theta^2}\,d\theta\,d\hat{n}$.
Challenge 3: Implement uniform random sampling on SO(3) using the quaternion method (4 Gaussian random variables, normalize).
def random_rotation():
"""Uniformly random rotation via quaternion."""
q = np.random.randn(4)
q = q / np.linalg.norm(q)
return quaternion_to_rotation(q)
# Verify: average trace should be 1 + 2*E[cos θ] = 1 (for uniform)
traces = [np.trace(random_rotation()) for _ in range(10000)]
print(f"Mean trace: {np.mean(traces):.3f} (expected: ~1.0 for uniform)")