Section 4.2: Points, vectors, poses, frames

"A robot without frames has many coordinates and no agreement."

A Meticulous Mapping Agent

Many robotics bugs come from treating all length-three arrays as the same kind of object. A grasp point, a surface normal, a gravity vector, a camera optical axis, and a base position may all fit in a NumPy array. They should not all be transformed the same way.

This section develops the habit of asking two questions for every spatial value: what kind of geometric object is it, and in which frame is it expressed?

Theory

A rigid pose can be written as $T=(R,t)$, where $R\in SO(3)$ is a rotation and $t\in R^3$ is a translation. The key distinction is that a point (a location) and a free vector (a displacement) transform differently between frames:

$$p^A = R_{AB}\,p^B + t_{AB}, \qquad v^A = R_{AB}\,v^B.$$

A free vector transforms by rotation only because a displacement has orientation and scale but no absolute location, so the translation $t_{AB}$ must not act on it. In homogeneous form this is enforced by the trailing coordinate: a point is $[\,p^B;\,1\,]$ and a vector is $[\,v^B;\,0\,]$, so

$$\begin{bmatrix} p^A \\ 1 \end{bmatrix} = \begin{bmatrix} R_{AB} & t_{AB} \\ 0\ 0\ 0 & 1 \end{bmatrix} \begin{bmatrix} p^B \\ 1 \end{bmatrix}, \qquad \begin{bmatrix} v^A \\ 0 \end{bmatrix} = \begin{bmatrix} R_{AB} & t_{AB} \\ 0\ 0\ 0 & 1 \end{bmatrix} \begin{bmatrix} v^B \\ 0 \end{bmatrix}.$$

A frame is a named coordinate system attached to something physical or conceptual: world, map, odom, base_link, camera_optical, wrist, object, or contact_patch. A pose gives the origin and axes of one frame relative to another.

Worked Example

A gripper approach point and an approach direction are both expressed in the tool frame. The point should rotate and translate into the base frame. The direction should rotate only.

# Points, vectors, poses, frames: keep the section idea tied to observable evidence.
# Run this diagnostic probe before trusting the maintained library path.
import numpy as np

R_base_tool = np.array([[0.0, -1.0, 0.0],
                        [1.0,  0.0, 0.0],
                        [0.0,  0.0, 1.0]])
t_base_tool = np.array([0.30, 0.10, 0.50])
p_tool = np.array([0.20, 0.00, 0.00])
v_tool = np.array([1.00, 0.00, 0.00])
p_base = R_base_tool @ p_tool + t_base_tool
v_base = R_base_tool @ v_tool
print("point", np.round(p_base, 3))
print("vector", np.round(v_base, 3))

Builder Recipe

1. Name whether the quantity is a point, vector, pose, twist, wrench, or covariance.
2. Name the source frame and destination frame before multiplying anything.
3. Write one unit test where translation should affect the value and one where it should not.
4. Use pose objects or transform messages at subsystem boundaries.
5. Log both the frame name and the numerical value.

Production Pattern

Points, vectors, poses, frames sits inside the Part II robotics contract: geometry defines where things are, kinematics defines what motion is possible, dynamics defines what motion costs, control defines how errors are corrected, and sensing defines what the agent can know on time.

Keep points and vectors separate: translations move points, while directions only rotate. This makes the section useful to students, builders, and researchers at the same time: the idea has an intuitive role, a formal interface, a runnable check, and a failure mode that can be reproduced.

Use this recipe when turning Points, vectors, poses, frames into code, a simulator experiment, or a robot diagnostic. The point is not to use every library. The point is to keep the hand-built baseline and the maintained-tool path comparable.

1. Name every frame with a parent, child, unit convention, and timestamp policy.
2. Write one hand-checked transform chain and verify identity, inverse, and composition tests.
3. Run the same transform through ROS 2 tf2 or SciPy Rotation, then compare one point and one direction vector.
4. Record a frame audit with source sensor, latency, and expected sign convention.
5. Debug failed behavior by replaying the transform tree before changing policy or controller code.

A point translates, a vector does not, and a pose carries orientation plus origin. Test those distinctions in a tiny case before passing data through tf2, SciPy Rotation, or a simulator.