Skip to content

Coordinate systems

Robotics / math

2D Rotation

articulate robotics 2D rotation

2d_rotation

alt text

\[ \begin{bmatrix} x_1 \\ y_1 \end{bmatrix}= \begin{bmatrix} h\cos\phi \\ h\sin\phi \end{bmatrix} \]
\[ \begin{bmatrix} x_2 \\ y_2 \end{bmatrix}= \begin{bmatrix} h\cos(\phi + \theta) \\ h\sin(\phi + \theta) \end{bmatrix} \]

using Trigonometric identity

\[ \cos(a+b)=\cos a\cos b-\sin a\sin b \]
\[ \sin(a+b)=\sin a\cos b+\cos a\sin b \]
\[ \begin{bmatrix} x_2 \\ y_2 \end{bmatrix}= \begin{bmatrix} h\cos \phi\cos \theta-h\sin \phi\sin \theta \\ h\sin \phi\cos \theta+h\cos \phi\sin \theta \end{bmatrix} \]

substitute x1, y1

\[ \begin{bmatrix} x_2 \\ y_2 \end{bmatrix}= \begin{bmatrix} x_1\cos\theta & -y_1\sin\theta \\ x_1\sin\theta & y_1\cos\theta \end{bmatrix} \]

Matrix form

\[ \begin{bmatrix} x_2 \\ y_2 \end{bmatrix}= \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} x_1 \\ y_1 \end{bmatrix} \]

Demo

Python demo code using numpy 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
import numpy as np
import matplotlib.pyplot as plt


v = np.array([3, 2])

theta = np.deg2rad(30)

R = np.array([
    [np.cos(theta), -np.sin(theta)],
    [np.sin(theta),  np.cos(theta)]
])

v_rot = R @ v

print(v_rot)

plt.quiver(0,0,v[0],v[1],angles='xy',scale_units='xy',scale=1,color='green')
plt.quiver(0,0,v_rot[0],v_rot[1],angles='xy',scale_units='xy',scale=1,color='red')

plt.xlim(-5,5)
plt.ylim(-5,5)
plt.grid()
plt.gca().set_aspect('equal')

plt.show()

Tait–Bryan

Tait–Bryan angles are a way to describe orientation in 3D space using three rotations about three different axes.

euler

Roll–Pitch–Yaw angles are specifically called Tait–Bryan angles, which are a special case of Euler angles.

alt text

  • Roll (φ) → rotation about the X-axis
  • Pitch (θ) → rotation about the Y-axis
  • Yaw (ψ) → rotation about the Z-axis

3D rotation

articulate robotics 2D rotation

A common convention in robotics and aerospace is the ZYX order:

\[ R = R_z(\psi)\,R_y(\theta)\,R_x(\phi) \]

This means:

  • rotate around X (roll)
  • then around Y (pitch)
  • then around Z (yaw)

How to use

If you have a vector in the body frame

\[v = \begin{bmatrix} x \\ y \\ z \end{bmatrix}\]

you rotate it to the world frame by

\[v' = R v\]

Rotation matrix

ROLL (X - axis)

\[ R_x(\phi)= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix} \]

PITCH (Y - axis)

\[R_y(\theta)= \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}\]

YAW (Z - axis)

\[R_z(\psi)= \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}\]

Right hand rule

  • x axis: red
  • y axis: green
  • z axis: blue
© articulated robotics

Right hand grip rule

the rule tells us what direction a positive rotation around an axis goes.

© articulated robotics

Intrinsic and Extrinsic

  • Intrinsic: The rotation relative to current body axes , it's use in robotics and ROS
  • Extrinsic: The rotation always relative to the fixed world axes

Intrinsic

rotations are about the body axes.

\[ R_{\text{intrinsic}} = R_z(\text{yaw}) \cdot R_y(\text{pitch}) \cdot R_x(\text{roll}) \]
code 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
import numpy as np
import matplotlib.pyplot as plt

# --- Rotation Matrices ---
def Rx(theta):
    return np.array([[1, 0, 0],
                     [0, np.cos(theta), -np.sin(theta)],
                     [0, np.sin(theta),  np.cos(theta)]])

def Ry(theta):
    return np.array([[np.cos(theta), 0, np.sin(theta)],
                     [0, 1, 0],
                     [-np.sin(theta), 0, np.cos(theta)]])

def Rz(theta):
    return np.array([[np.cos(theta), -np.sin(theta), 0],
                     [np.sin(theta),  np.cos(theta), 0],
                     [0, 0, 1]])

# --- Plot Function ---
def plot_axes(R, ax, title):
    origin = np.zeros(3)
    x_axis = R @ np.array([1, 0, 0])
    y_axis = R @ np.array([0, 1, 0])
    z_axis = R @ np.array([0, 0, 1])

    ax.quiver(*origin, *x_axis, color="r")
    ax.quiver(*origin, *y_axis, color="g")
    ax.quiver(*origin, *z_axis, color="b")

    ax.set_xlim([-1.5, 1.5])
    ax.set_ylim([-1.5, 1.5])
    ax.set_zlim([-1.5, 1.5])
    ax.set_title(title)

# --- Angles ---
roll = np.deg2rad(90)
pitch = np.deg2rad(45)
yaw = np.deg2rad(180)

# --- Intrinsic sequence ---
R0 = np.eye(3)
R1 = R0 @ Rx(roll)      # roll about body X
R2 = R1 @ Ry(pitch)     # then pitch about new Y
R3 = R2 @ Rz(yaw)       # then yaw about new Z

# --- Plot ---
fig = plt.figure(figsize=(10,10))
plot_axes(R0, fig.add_subplot(221, projection="3d"), "Step 1: Rest")
plot_axes(R1, fig.add_subplot(222, projection="3d"), "Step 2: Roll 90° (body X)")
plot_axes(R2, fig.add_subplot(223, projection="3d"), "Step 3: + Pitch 45° (body Y)")
plot_axes(R3, fig.add_subplot(224, projection="3d"), "Step 4: + Yaw 180° (body Z)")
plt.tight_layout()
plt.show()

alt text

Extrinsic

rotations are about the fixed world axes.

\[R_{\text{extrinsic}} = R_x(\text{roll}) \cdot R_y(\text{pitch}) \cdot R_z(\text{yaw})\]
code 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
import numpy as np
import matplotlib.pyplot as plt

# --- Rotation Matrices ---
def Rx(theta):
    return np.array([[1, 0, 0],
                     [0, np.cos(theta), -np.sin(theta)],
                     [0, np.sin(theta),  np.cos(theta)]])

def Ry(theta):
    return np.array([[np.cos(theta), 0, np.sin(theta)],
                     [0, 1, 0],
                     [-np.sin(theta), 0, np.cos(theta)]])

def Rz(theta):
    return np.array([[np.cos(theta), -np.sin(theta), 0],
                     [np.sin(theta),  np.cos(theta), 0],
                     [0, 0, 1]])

# --- Plot Function ---
def plot_axes(R, ax, title):
    origin = np.zeros(3)
    x_axis = R @ np.array([1, 0, 0])
    y_axis = R @ np.array([0, 1, 0])
    z_axis = R @ np.array([0, 0, 1])

    ax.quiver(*origin, *x_axis, color="r")
    ax.quiver(*origin, *y_axis, color="g")
    ax.quiver(*origin, *z_axis, color="b")

    ax.set_xlim([-1.5, 1.5])
    ax.set_ylim([-1.5, 1.5])
    ax.set_zlim([-1.5, 1.5])
    ax.set_title(title)

# --- Angles ---
roll = np.deg2rad(90)
pitch = np.deg2rad(45)
yaw = np.deg2rad(180)

# --- Extrinsic sequence ---
R0 = np.eye(3)
R1 = Rx(roll) @ R0      # roll about world X
R2 = Ry(pitch) @ R1     # then pitch about world Y
R3 = Rz(yaw) @ R2       # then yaw about world Z

# --- Plot ---
fig = plt.figure(figsize=(10,10))
plot_axes(R0, fig.add_subplot(221, projection="3d"), "Step 1: Rest")
plot_axes(R1, fig.add_subplot(222, projection="3d"), "Step 2: Roll 90° (world X)")
plot_axes(R2, fig.add_subplot(223, projection="3d"), "Step 3: + Pitch 45° (world Y)")
plot_axes(R3, fig.add_subplot(224, projection="3d"), "Step 4: + Yaw 180° (world Z)")
plt.tight_layout()
plt.show()

alt text

Rotate using scipy

  • Lowercase ("xyz") = intrinsic (body-fixed).
  • Uppercase ("XYZ") = extrinsic (world-fixed).
code 
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as R

# --- Angles (degrees) ---
roll, pitch, yaw = 90, 45, 180

# --- Intrinsic rotation (body axes: roll -> pitch -> yaw) ---
rot_intrinsic = R.from_euler("zyx", [yaw, pitch, roll], degrees=True)
zyx_intrinsic = rot_intrinsic.as_matrix()

rot_intrinsic2 = R.from_euler("xyz", [roll, pitch, yaw], degrees=True)
xyz_intrinsic = rot_intrinsic2.as_matrix()

# --- Extrinsic rotation (world axes: roll -> pitch -> yaw) ---
rot_extrinsic = R.from_euler("ZYX", [yaw, pitch, roll], degrees=True)
ZYX_extrinsic = rot_extrinsic.as_matrix()

rot_extrinsic2 = R.from_euler("XYZ", [roll, pitch, yaw], degrees=True)
XYZ_extrinsic = rot_extrinsic2.as_matrix()

# --- Helper plot function ---
def plot_axes(R, ax, title):
    origin = np.zeros(3)
    x_axis = R @ np.array([1, 0, 0])
    y_axis = R @ np.array([0, 1, 0])
    z_axis = R @ np.array([0, 0, 1])

    ax.quiver(*origin, *x_axis, color="r", label="X")
    ax.quiver(*origin, *y_axis, color="g", label="Y")
    ax.quiver(*origin, *z_axis, color="b", label="Z")

    ax.set_xlim([-1.5, 1.5])
    ax.set_ylim([-1.5, 1.5])
    ax.set_zlim([-1.5, 1.5])
    ax.set_xlabel("X")
    ax.set_ylabel("Y")
    ax.set_zlabel("Z")
    ax.set_title(title)
    ax.legend()

# --- Plot both ---
fig = plt.figure(figsize=(12,6))

# Vector
R0 = np.eye(3)

# Rotate vector
v_intrinsic = zyx_intrinsic @ R0
v_extrinsic = ZYX_extrinsic @ R0

ax1 = fig.add_subplot(221, projection="3d")
plot_axes(v_intrinsic, ax1, "zyx (body frame)")
ax1.get_legend().remove() 

ax1 = fig.add_subplot(223, projection="3d")
plot_axes(xyz_intrinsic, ax1, "xyz")
ax1.get_legend().remove() 

ax2 = fig.add_subplot(222, projection="3d")
plot_axes(v_extrinsic, ax2, "ZYX")
ax2.get_legend().remove()

ax2 = fig.add_subplot(224, projection="3d")
plot_axes(XYZ_extrinsic, ax2, "XYZ ()")
ax2.get_legend().remove()
# plt.tight_layout()
plt.show()

# --- Print matrices ---
np.set_printoptions(precision=4, suppress=True)
print("Intrinsic Rotation Matrix:\n", zyx_intrinsic)
print("\nExtrinsic Rotation Matrix:\n", ZYX_extrinsic)

alt text

Intrinsic (body axes, lowercase) Equivalent Extrinsic (world axes, uppercase reversed)
"xyz" → roll-pitch-yaw (body x→y→z) "ZYX" → yaw-pitch-roll (world z→y→x)
"xzy" "YZX"
"yxz" "ZXY"
"yzx" "XZY"
"zxy" "YXZ"
"zyx" "XYZ"

Reference