A coordinate frame in 3D is a pair \( \mathcal{F} = (O, \{\mathbf{e}_1,\mathbf{e}_2,\mathbf{e}_3\}) \), where:

• \( O \in \mathbb{R}^3 \) is the origin point,
• \( \mathbf{e}_1,\mathbf{e}_2,\mathbf{e}_3 \) are basis vectors, typically orthonormal.

Orthonormality means \( \mathbf{e}_i^\top \mathbf{e}_j = \delta_{ij} \) where \( \delta_{ij} \) is the Kronecker delta. Any geometric vector \( \mathbf{v} \) can be represented in frame \( \mathcal{F} \) by coordinates \( [\mathbf{v}]_{\mathcal{F}} = \begin{bmatrix} v_1&v_2&v_3 \end{bmatrix}^\top \) such that \( \mathbf{v} = v_1\mathbf{e}_1 + v_2\mathbf{e}_2 + v_3\mathbf{e}_3 \).

Key distinction: A vector is a geometric object independent of frames, while its coordinate representation depends on the chosen basis.

\[ \mathbf{v} \;\text{(geometry)} \quad\neq\quad [\mathbf{v}]_{\mathcal{F}} \;\text{(numbers)}. \]

If two frames \( \mathcal{A} \) and \( \mathcal{B} \) share the same origin but different bases, there exists a unique orthonormal matrix \( \mathbf{R}_{\mathcal{A}\mathcal{B}} \) such that

\[ [\mathbf{v}]_{\mathcal{A}} = \mathbf{R}_{\mathcal{A}\mathcal{B}} [\mathbf{v}]_{\mathcal{B}}, \quad \mathbf{R}_{\mathcal{A}\mathcal{B}}^\top \mathbf{R}_{\mathcal{A}\mathcal{B}} = \mathbf{I}. \]

We will use this change-of-basis idea repeatedly, without yet diving into the deeper rotation theory reserved for the next lesson.

The world frame \( \mathcal{W} \) is a fixed reference frame attached to the environment (lab, map, Earth, etc.). It is typically treated as inertial, meaning Newtonian mechanics are simplest in this frame.

A point \( P \) in space has a position vector relative to \( \mathcal{W} \): \( [\mathbf{p}]_{\mathcal{W}} \in \mathbb{R}^3 \). If \( O_{\mathcal{W}} \) is the origin of \( \mathcal{W} \), then

\[ \mathbf{p} = \overrightarrow{O_{\mathcal{W}}P}, \qquad [\mathbf{p}]_{\mathcal{W}} = \begin{bmatrix} x_{\mathcal{W}} \\ y_{\mathcal{W}} \\ z_{\mathcal{W}} \end{bmatrix}. \]

In experiments, the world frame might coincide with a motion-capture coordinate system or a simulation's global axes.

The body frame \( \mathcal{B} \) is rigidly attached to the robot's main body (base link for a manipulator, chassis for a mobile robot). Its origin is often at a mechanically meaningful point (center of mass, geometric center, or base joint).

The body frame moves with the robot, so coordinates in \( \mathcal{B} \) are constant for body-fixed points. Let \( \mathbf{p} \) be a point fixed on the robot. Then \( [\mathbf{p}]_{\mathcal{B}} \) is constant over time, while \( [\mathbf{p}]_{\mathcal{W}}(t) \) changes as the robot moves.

Suppose the body origin is at world position \( [\mathbf{t}_{\mathcal{W}\mathcal{B}}]_{\mathcal{W}} \), and the body axes are rotated relative to the world by \( \mathbf{R}_{\mathcal{W}\mathcal{B}} \). Then the coordinate relation for any point is

\[ [\mathbf{p}]_{\mathcal{W}} = \mathbf{R}_{\mathcal{W}\mathcal{B}}[\mathbf{p}]_{\mathcal{B}} + [\mathbf{t}_{\mathcal{W}\mathcal{B}}]_{\mathcal{W}}. \]

This is a rigid transform: a rotation (basis change) plus translation (origin shift). We will revisit this equation when we discuss homogeneous transforms in Lesson 4.

Each sensor on a robot defines its own natural coordinate frame \( \mathcal{S}_i \), fixed to the sensor housing. Examples:

• IMU frame aligned with accelerometer/gyroscope axes.
• Camera optical frame with \( z \)-axis along viewing direction.
• LiDAR frame centered at the laser origin.

Sensor measurements are naturally expressed in their sensor frame. For instance, a 3-axis accelerometer returns \( [\mathbf{a}]_{\mathcal{S}} \) . To use this in control or estimation, we often need it in the body or world frame.

If the sensor frame is rigidly mounted on the body, its pose relative to the body is constant: translation \( [\mathbf{t}_{\mathcal{B}\mathcal{S}}]_{\mathcal{B}} \) and rotation \( \mathbf{R}_{\mathcal{B}\mathcal{S}} \). Then for any sensed point/vector:

\[ [\mathbf{p}]_{\mathcal{B}} = \mathbf{R}_{\mathcal{B}\mathcal{S}}[\mathbf{p}]_{\mathcal{S}} + [\mathbf{t}_{\mathcal{B}\mathcal{S}}]_{\mathcal{B}}. \]

Interpretation: translate from sensor origin to body origin, and rotate sensor coordinates into body axes.

A manipulator’s tool frame (also called end-effector frame) \( \mathcal{T} \) is attached to the robot's working point: gripper center, welding tip, suction cup center, etc.

Tool frames are crucial because tasks are specified in tool coordinates: “move the gripper 5 cm forward” means along \( \mathcal{T} \)’s axes.

Let \( \mathcal{T} \) be rigidly attached to a distal link (wrist link). Its pose relative to that link is fixed, but its pose relative to the body/world changes with joints. We won’t derive joint kinematics here (reserved for Course 2), but the coordinate algebra remains:

\[ [\mathbf{p}]_{\mathcal{W}} = \mathbf{R}_{\mathcal{W}\mathcal{T}}[\mathbf{p}]_{\mathcal{T}} + [\mathbf{t}_{\mathcal{W}\mathcal{T}}]_{\mathcal{W}}. \]

The meaning of \( \mathbf{R}_{\mathcal{W}\mathcal{T}} \) and \( \mathbf{t}_{\mathcal{W}\mathcal{T}} \) is identical to the body case, only the moving frame is now the tool.

Frames form a directed graph (tree in many robots). Each edge stores a rigid transform. If we know transforms along a path, we can combine them to move coordinates between distant frames.

flowchart LR
  W["World frame W"] --> B["Body frame B"]
  B --> S1["Sensor frame S1 (camera)"]
  B --> S2["Sensor frame S2 (IMU)"]
  B --> T["Tool frame T"]
        

Suppose we want a point from sensor frame to world frame. Using the two relations:

\[ [\mathbf{p}]_{\mathcal{B}} = \mathbf{R}_{\mathcal{B}\mathcal{S}}[\mathbf{p}]_{\mathcal{S}} + [\mathbf{t}_{\mathcal{B}\mathcal{S}}]_{\mathcal{B}}, \]

\[ [\mathbf{p}]_{\mathcal{W}} = \mathbf{R}_{\mathcal{W}\mathcal{B}}[\mathbf{p}]_{\mathcal{B}} + [\mathbf{t}_{\mathcal{W}\mathcal{B}}]_{\mathcal{W}}, \]

substituting yields the composed transform:

\[ [\mathbf{p}]_{\mathcal{W}} = \mathbf{R}_{\mathcal{W}\mathcal{B}}\mathbf{R}_{\mathcal{B}\mathcal{S}}[\mathbf{p}]_{\mathcal{S}} + \mathbf{R}_{\mathcal{W}\mathcal{B}}[\mathbf{t}_{\mathcal{B}\mathcal{S}}]_{\mathcal{B}} + [\mathbf{t}_{\mathcal{W}\mathcal{B}}]_{\mathcal{W}}. \]

This is just linear algebra: multiply rotations, add translated offsets.

Let frames \( \mathcal{A}, \mathcal{B}, \mathcal{C} \) share an origin. By definition, \( [\mathbf{v}]_{\mathcal{A}} = \mathbf{R}_{\mathcal{A}\mathcal{B}}[\mathbf{v}]_{\mathcal{B}} \) and \( [\mathbf{v}]_{\mathcal{B}} = \mathbf{R}_{\mathcal{B}\mathcal{C}}[\mathbf{v}]_{\mathcal{C}} \).

Substitute the second into the first:

\[ [\mathbf{v}]_{\mathcal{A}} = \mathbf{R}_{\mathcal{A}\mathcal{B}} \big(\mathbf{R}_{\mathcal{B}\mathcal{C}}[\mathbf{v}]_{\mathcal{C}}\big) = (\mathbf{R}_{\mathcal{A}\mathcal{B}}\mathbf{R}_{\mathcal{B}\mathcal{C}})[\mathbf{v}]_{\mathcal{C}}. \]

Since this holds for all vectors \( \mathbf{v} \), the transform from \( \mathcal{C} \) to \( \mathcal{A} \) is

\[ \mathbf{R}_{\mathcal{A}\mathcal{C}} = \mathbf{R}_{\mathcal{A}\mathcal{B}}\mathbf{R}_{\mathcal{B}\mathcal{C}}. \]

This “composition law” is the algebraic reason a frame tree can be traversed by multiplying rotations along edges.

We implement the basic transform \( [\mathbf{p}]_{\mathcal{W}} = \mathbf{R}_{\mathcal{W}\mathcal{B}}[\mathbf{p}]_{\mathcal{B}} + \mathbf{t}_{\mathcal{W}\mathcal{B}} \) with standard robotics libraries and also from scratch.

8.1 Python (NumPy / SciPy spatial tools)

import numpy as np

# Example rotation (world from body) and translation
R_WB = np.array([[0.0, -1.0, 0.0],
                 [1.0,  0.0, 0.0],
                 [0.0,  0.0, 1.0]])
t_WB = np.array([1.0, 2.0, 0.5])

# Point in body frame
p_B = np.array([0.3, 0.0, 0.2])

# Transform to world frame
p_W = R_WB @ p_B + t_WB
print("p_W =", p_W)

# Inverse transform (world to body)
p_B_recovered = R_WB.T @ (p_W - t_WB)
print("p_B recovered =", p_B_recovered)
      

8.2 C++ (Eigen)

#include <iostream>
#include <Eigen/Dense>

int main() {
    Eigen::Matrix3d R_WB;
    R_WB << 0, -1, 0,
            1,  0, 0,
            0,  0, 1;

    Eigen::Vector3d t_WB(1.0, 2.0, 0.5);
    Eigen::Vector3d p_B(0.3, 0.0, 0.2);

    Eigen::Vector3d p_W = R_WB * p_B + t_WB;
    std::cout << "p_W = " << p_W.transpose() << std::endl;

    Eigen::Vector3d p_B_rec = R_WB.transpose() * (p_W - t_WB);
    std::cout << "p_B recovered = " << p_B_rec.transpose() << std::endl;
    return 0;
}
      

8.3 Java (EJML)

import org.ejml.simple.SimpleMatrix;

public class FrameTransform {
    public static void main(String[] args) {
        SimpleMatrix R_WB = new SimpleMatrix(new double[][]{
            {0, -1, 0},
            {1,  0, 0},
            {0,  0, 1}
        });

        SimpleMatrix t_WB = new SimpleMatrix(3,1,true, new double[]{1.0, 2.0, 0.5});
        SimpleMatrix p_B  = new SimpleMatrix(3,1,true, new double[]{0.3, 0.0, 0.2});

        SimpleMatrix p_W = R_WB.mult(p_B).plus(t_WB);
        System.out.println("p_W = " + p_W);

        SimpleMatrix p_B_rec = R_WB.transpose().mult(p_W.minus(t_WB));
        System.out.println("p_B recovered = " + p_B_rec);
    }
}
      

8.4 Matlab / Simulink (Matrix Form)

R_WB = [0 -1 0;
        1  0 0;
        0  0 1];
t_WB = [1; 2; 0.5];

p_B = [0.3; 0; 0.2];

p_W = R_WB * p_B + t_WB
p_B_recovered = R_WB.' * (p_W - t_WB)
      

In Simulink, this same computation is done with a 3x3 Gain block for \( \mathbf{R}_{\mathcal{W}\mathcal{B}} \), a Sum block for \( +\mathbf{t}_{\mathcal{W}\mathcal{B}} \), and vector inputs for \( [\mathbf{p}]_{\mathcal{B}} \).

Problem 1 (Coordinate Change): Two frames \( \mathcal{W} \) and \( \mathcal{B} \) share an origin. The columns of \( \mathbf{R}_{\mathcal{W}\mathcal{B}} \) are the body axes expressed in world coordinates. Show that if \( \mathbf{R}_{\mathcal{W}\mathcal{B}} \) is orthonormal, then its inverse equals its transpose.

Solution:

Orthonormality gives \( \mathbf{R}_{\mathcal{W}\mathcal{B}}^\top \mathbf{R}_{\mathcal{W}\mathcal{B}} = \mathbf{I} \). Multiply both sides on the right by \( \mathbf{R}_{\mathcal{W}\mathcal{B}}^{-1} \):

\[ \mathbf{R}_{\mathcal{W}\mathcal{B}}^\top = \mathbf{R}_{\mathcal{W}\mathcal{B}}^{-1}. \]

Problem 2 (Sensor to World): A camera frame \( \mathcal{S} \) is mounted on the robot body \( \mathcal{B} \). You are given constant extrinsics \( \mathbf{R}_{\mathcal{B}\mathcal{S}} \), \( \mathbf{t}_{\mathcal{B}\mathcal{S}} \), and time-varying body pose in world \( \mathbf{R}_{\mathcal{W}\mathcal{B}}(t) \), \( \mathbf{t}_{\mathcal{W}\mathcal{B}}(t) \). Derive \( [\mathbf{p}]_{\mathcal{W}} \) for a point measured as \( [\mathbf{p}]_{\mathcal{S}} \).

Solution:

First move from sensor to body, then body to world:

\[ [\mathbf{p}]_{\mathcal{B}} = \mathbf{R}_{\mathcal{B}\mathcal{S}}[\mathbf{p}]_{\mathcal{S}} + \mathbf{t}_{\mathcal{B}\mathcal{S}}. \]

\[ [\mathbf{p}]_{\mathcal{W}} = \mathbf{R}_{\mathcal{W}\mathcal{B}}[\mathbf{p}]_{\mathcal{B}} + \mathbf{t}_{\mathcal{W}\mathcal{B}}. \]

Substitute to obtain

\[ [\mathbf{p}]_{\mathcal{W}} = \mathbf{R}_{\mathcal{W}\mathcal{B}}\mathbf{R}_{\mathcal{B}\mathcal{S}}[\mathbf{p}]_{\mathcal{S}} + \mathbf{R}_{\mathcal{W}\mathcal{B}}\mathbf{t}_{\mathcal{B}\mathcal{S}} + \mathbf{t}_{\mathcal{W}\mathcal{B}}. \]

Problem 3 (Tool Displacement): A desired tool displacement in tool coordinates is \( \Delta \mathbf{p}_{\mathcal{T}} = \begin{bmatrix} 0.1 & 0 & 0 \end{bmatrix}^\top \) meters. If the current tool orientation in world is \( \mathbf{R}_{\mathcal{W}\mathcal{T}} \), express the same displacement in world coordinates.

Solution:

Displacements are vectors, so only rotation applies:

\[ \Delta \mathbf{p}_{\mathcal{W}} = \mathbf{R}_{\mathcal{W}\mathcal{T}} \Delta \mathbf{p}_{\mathcal{T}}. \]

Problem 4 (Frame Choice Error): A student integrates IMU acceleration directly as \( \dot{\mathbf{v}} = [\mathbf{a}]_{\mathcal{S}} \) in world dynamics. Explain mathematically why this is generally incorrect.

Solution:

The world-time derivative of velocity is expressed in world coordinates: \( \dot{\mathbf{v}}_{\mathcal{W}} \). IMU acceleration is in sensor coordinates. The correct relation is

\[ \dot{\mathbf{v}}_{\mathcal{W}} = \mathbf{R}_{\mathcal{W}\mathcal{S}} [\mathbf{a}]_{\mathcal{S}}. \]

Without applying the rotation \( \mathbf{R}_{\mathcal{W}\mathcal{S}} \), the student adds numbers in mismatched bases, producing a physically meaningless result.

We defined the four essential frames in robotics: a fixed world frame, a moving body frame, multiple sensor frames rigidly mounted on the robot, and a task-centric tool frame. Using only linear algebra, we established how points and vectors transform between frames, how transforms compose along a frame graph, and why careful bookkeeping of frames is necessary for correct perception and control.