Part 1: The Geometry of Motion

Welcome to the first installment of our "Robotics Zero to Hero" series. Before a robot can interact with the world, it must answer a deceptively simple question: where am I, and which way am I facing? This is the Geometry of Motion, and getting it right is the foundation everything else is built on.

In this post we develop the mathematics of rigid-body pose in 3D, treat rotations as a Lie group rather than a bag of formulas, and set up the central theme of this entire series: why high-dimensional robots (like our continuum octopus) are so much harder than the low-dimensional rigid arms of classical robotics.

The Math: $SO(3)$, $SE(3)$, and Lie Groups

To place a rigid body in space we need two things:

1. Translation — a vector $\mathbf{t} \in \mathbb{R}^3$ locating the body's origin.
2. Rotation — an element of the Special Orthogonal group $SO(3)$ describing orientation.

A rotation matrix $R \in SO(3)$ satisfies two constraints:

The first says columns are orthonormal; the second rules out reflections. Note what this costs: $R$ has 9 entries but only 3 degrees of freedom. The 6 constraints carve out a curved 3-dimensional surface (a manifold) inside $\mathbb{R}^{9}$. That curvature is the source of nearly every subtlety in this post.

A full rigid-body (homogeneous) transformation stacks rotation and translation into a $4\times 4$ matrix in the Special Euclidean group $SE(3)$:

So the pose of one rigid body lives in a 6-dimensional space: 3 for position, 3 for orientation. Hold onto that number — it is the dividing line of this series.

The Lie algebra: angular velocity lives in $\mathfrak{so}(3)$

$SO(3)$ is not just a set; it is a smooth group (a Lie group). Its "tangent space at the identity" is the Lie algebra $\mathfrak{so}(3)$, the space of $3\times 3$ skew-symmetric matrices. Any angular velocity $\boldsymbol{\omega} = (\omega_x, \omega_y, \omega_z)$ maps to a matrix via the hat operator:

The exponential map turns an angular velocity into a rotation. For a unit axis $\hat{\mathbf{u}}$ and angle $\theta$ this is Rodrigues' formula:

and its inverse, the logarithm map, recovers the axis–angle from a rotation matrix. This pairing is what lets us do calculus on rotations — interpolate them, average them, and run gradient descent on them — which we will need for inverse kinematics (Part 3) and trajectory optimization (Part 7).

Representing rotations: there is no free lunch

Rotation matrices are great for composition but wasteful to store and awkward to optimize. Every alternative trades one weakness for another. The table below summarizes the competing representations and where each one breaks.

The deep reason there is no singularity-free 3-parameter global chart for $SO(3)$ is topological: $SO(3)$ is a curved, compact manifold that cannot be smoothly flattened onto $\mathbb{R}^3$. Unit quaternions are the standard escape hatch — a 4-number representation that double-covers $SO(3)$ and avoids gimbal lock — and SLERP (spherical linear interpolation, Shoemake 1985) gives constant-rate interpolation between orientations.

Python Implementation: Rotation Matrices

Let's construct the elemental rotation matrices and compose them.

Loading Interactive Python Environment...

Notice the last two checks. On real hardware, repeatedly multiplying rotation matrices accumulates floating-point error and $R$ slowly drifts off $SO(3)$. Production code periodically re-orthonormalizes (e.g. via SVD or quaternion normalization) — a small but essential piece of numerical hygiene.

High-Dimensional vs. Low-Dimensional Robots

This is the theme we return to in every post, so let's define it precisely here.

A robot's configuration $\mathbf q$ is the minimal set of numbers that fully specifies its shape. The set of all configurations is the configuration space (C-space), a manifold $\mathcal{Q}$ whose dimension $n$ is the robot's degrees of freedom.

• A low-dimensional robot has small $n$: a 6-DOF industrial arm has $\mathcal{Q} \approx (S^1)^6$, a 6-torus. Closed-form solutions exist, every configuration can be enumerated, and intuition mostly works.
• A high-dimensional robot has large $n$: a humanoid has $\approx 30$ DOF, a dexterous hand $\approx 20$, and a true continuum robot — like an octopus tentacle that bends everywhere along its length — has, in the ideal limit, infinite degrees of freedom. Its configuration is not a vector but a continuous curve $\mathbf{r}(s)$ in space.

Almost every "hard" problem in this series is really the statement: a tool that works beautifully in low dimensions degrades or collapses as $n$ grows. Keeping this contrast in view is the single most useful mental model for modern robotics.

Focus on the Octopus: Central Body to Tentacle Mapping

At Ensemble Control we are building a metallic continuum octopus robot: a central body (head/base) with multiple flexible tentacles. Even before a tentacle bends, we must place its base frame correctly relative to the body.

Let $T^{body}_{world}$ be the transform from world to the octopus body, and $T^{tentacle\_i}_{body}$ the fixed transform from the body center to the base of tentacle $i$. By composition in $SE(3)$:

If the body tilts, every tentacle's reference frame rotates with it — composition handles this automatically, which is exactly why we work in $SE(3)$ rather than tracking position and orientation separately.

The catch is what comes next. For a rigid arm, the chain ends after a handful of joints. For a tentacle, the "chain" never really ends — the body curves continuously, so $T^{tentacle\_i}_{world}$ is only the first link of an infinite chain. Capturing that curvature is the subject of Part 2, where we build Forward Kinematics and confront continuum models head-on.

Further reading: Murray, Li & Sastry, "A Mathematical Introduction to Robotic Manipulation" (1994); Lynch & Park, "Modern Robotics" (2017) for the Lie-group treatment of $SE(3)$.