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. Apply a full SE(3) move to 10,000 surface normals and every single one silently picks up a 0.5 m translation offset; apply only the rotation, and every single one is correct. A typical camera-to-base translation is 0.3 to 0.6 m, so a misclassified normal vector is not nudged by a rounding error but displaced by a distance larger than the object being grasped. Same array shape, same dtype, opposite outcomes.

Ask two questions for every spatial value: what kind of geometric object is it, and in which frame is it expressed? Figure 4.2A previews the four geometric types on a 2D plane before they are extended to full SE(3) poses.

Theory

Figure 4.2B shows this rule visually: a point picks up both rotation and translation, while a free vector picks up rotation only. A rigid pose can be written as \(T=(R,t)\), where \(R\in SO(3)\) (Special Orthogonal group in 3D, the set of all valid rotation matrices) 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.$$

Homogeneous form

A free vector transforms by rotation only because a displacement has orientation and scale but no absolute location, so this is called the point-versus-vector contract, and the translation \(t_{AB}\) must not act on it. ("Free" here means the vector is defined only by its direction and length, not by where it starts; a surface normal or a velocity direction is free in this sense, while a point is tied to one fixed location.) 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. A number without a frame is a measurement without a ruler: precise, confident, and wrong.

In a real robot stack these named frames are not a loose collection: they form a tree, with one frame (typically world or map) as the root and every other frame attached as a child of exactly one parent, each edge carrying the pose that relates child to parent. ROS 2 tf2 stores and time-buffers exactly this tree, and Section 5.5 builds the same tree from joint angles via forward kinematics. To move a quantity between any two frames, walk up from the source frame to the nearest common ancestor and back down to the destination frame, composing poses along the path; this is why a single misclassified point or vector anywhere in the tree, not just at the two endpoints, corrupts every downstream frame that depends on it.

Frames matter in embodied AI because a robot's sensors, joints, and effectors each move independently. A depth camera mounted on a rotating head produces coordinates in its own lens frame. Suppose the controller reads those coordinates as if they already lived in the base frame. The robot then reaches toward where the camera was pointing when it was still, not where the object actually is. Every link in a kinematic chain introduces a new frame, and correctness depends on tracking which frame each measurement lives in at every moment.

A frame is three orthogonal axes and an origin; every measurement is stored relative to them, and a sensor value means nothing without the frame it was taken in. Moving data between frames requires a pose, the explicit record of how one frame's origin and axes sit inside another. Compose those poses along a kinematic chain and any quantity reaches any frame without ambiguity.

Worked Example

The same classify-then-transform logic that the algorithm spells out in symbols becomes a few lines of NumPy once you fix a concrete pose and two quantities to move.

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))

The distinction matters most at three moments in a typical pipeline. Raw sensor output enters the system, where depth images give points and an IMU (inertial measurement unit, a sensor reporting acceleration and angular rate) gives vectors, yet both arrive as plain arrays. Data crosses a subsystem boundary such as a ROS 2 topic or a function call returning a dict. A learned model produces spatial outputs that must feed a classical controller. At each of these hand-off points the receiving code sees only shape and dtype, so it most easily forgets the geometric type. Name the type explicitly at each boundary, in a docstring, a message field name, or a wrapper class, and the distinction stays visible without adding runtime cost.

Builder Recipe

1. Name whether the quantity is a point, vector, pose, twist (a combined linear-and-angular velocity), wrench (a combined force-and-torque), or covariance (a matrix describing uncertainty).
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

Having seen the contract hold on a single point and vector, it helps to step back and place these geometric types in the larger pipeline they feed.

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. 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.