## How to Describe the Rotation of a Robot in 3D

Before we get into how to describe the rotation of a robot in three dimensions, it is important you understand how to describe the rotation of a robot in two dimensions.

In my previous post, we learned how to calculate the coordinates of a point (or vector) in the 2D global reference frame given the coordinates of that point (or magnitude and direction of that vector) in the 2D robot (local) reference frame (i.e. the rotated coordinate system).

Here is the equation we use to convert a point in the 2D local reference frame to a point in the 2D global reference frame (If you don’t know how to multiply matrices together, there are a bunch of videos on YouTube that walk through the process):

Here is the equation we use to convert velocity in the 2D local reference frame to velocity in the 2D global reference frame:

The thing that makes all this possible is the two-dimensional rotation matrix:

To describe the rotation of a robot in three dimensions, we need to use the three-dimensional rotation matrix. Let’s explore this now.

# Converting a Point (Or Vector) in the 3D Local Reference Frame to a Point in the 3D Global Reference Frame

In two dimensions, the only angle we had to worry about was γ. γ represents the amount of rotation (in degrees or radians) of the robot x-axis from the global x-axis, going in the counterclockwise direction.

γ is often called the yaw angle. It is the angle of rotation about the z-axis.

We need to add two more angles:

• Rotation about the x axis = roll angle = α
• Rotation about the y-axis = pitch angle = β

If you want to learn more about these angles, check out my post on roll, pitch, and yaw.

So, how do we derive the three-dimensional rotation matrix? What we need to do is calculate the rotation matrix for all rotations a robot can do in a three-dimensional space. Since there are three axes, there are three different rotations we need to account for: rotation about the x-axis, rotation about the y-axis, and rotation about the z-axis.

## Yaw (Rotation about the z-axis)

Let’s start with rotation about the z-axis. Guess what? We already know that rotation matrix.

This is the two-dimensional case I explained at the beginning of this post. The x and y coordinates of a point in the robot reference frame will change when converted to coordinates in the global reference frame. However, the z coordinate will remain constant.

Here is the rotation matrix that takes care of rotation of a robot in 3D about the global z-axis:

## Pitch (Rotation about the y-axis)

When we rotate a point about the y-axis, the x and z coordinates of the point will change, but the y-coordinate will remain the same.

Here is the rotation matrix that enables us to convert a point (or vector) in the local reference frame to a point (or vector) in the global reference frame when all we have is rotation of the robot about the global y-axis.

## Roll (Rotation about the x-axis)

When we rotate a point about the x-axis, the y and z coordinates of the point will change, but the x-coordinate will remain the same.

Here is the rotation matrix that enables us to convert a point (or vector) in the local reference frame to a point (or vector) in the global reference frame when all we have is rotation of the robot about the global x-axis.

## Putting It All Together

Ok, so now we need to combine the three matrices above to calculate the full three-dimensional rotation matrix.

## Three-Dimensional Rotation Matrix in Python Code

Here is a NumPy-based method that converts angles into a 3×3 rotation matrix like the one above. You can use this method in whatever code you want to write.

```import numpy as np # NumPy library

def euler_rotation_matrix(alpha,beta,gamma):
"""
Generate a full three-dimensional rotation matrix from euler angles

Input
:param alpha: The roll angle (radians) - Rotation around the x-axis
:param beta: The pitch angle (radians) - Rotation around the y-axis
:param alpha: The yaw angle (radians) - Rotation around the z-axis

Output
:return: A 3x3 element matix containing the rotation matrix.
This rotation matrix converts a point in the local reference
frame to a point in the global reference frame.

"""
# First row of the rotation matrix
r00 = np.cos(gamma) * np.cos(beta)
r01 = np.cos(gamma) * np.sin(beta) * np.sin(alpha) - np.sin(gamma) * np.cos(alpha)
r02 = np.cos(gamma) * np.sin(beta) * np.cos(alpha) + np.sin(gamma) * np.sin(alpha)

# Second row of the rotation matrix
r10 = np.sin(gamma) * np.cos(beta)
r11 = np.sin(gamma) * np.sin(beta) * np.sin(alpha) + np.cos(gamma) * np.cos(alpha)
r12 = np.sin(gamma) * np.sin(beta) * np.cos(alpha) - np.cos(gamma) * np.sin(alpha)

# Third row of the rotation matrix
r20 = -np.sin(beta)
r21 = np.cos(beta) * np.sin(alpha)
r22 = np.cos(beta) * np.cos(alpha)

# 3x3 rotation matrix
rot_matrix = np.array([[r00, r01, r02],
[r10, r11, r12],
[r20, r21, r22]])

return rot_matrix

def rotate(p1,alpha,beta,gamma):
"""
Rotates a point p1 in 3D space in the local reference frame to
a point p2 in the global reference frame.

Input
:param p1: A 3 element array containing the position of a point in the
local reference frame (xL,yL,zL)
:param alpha: The roll angle (radians) - Rotation around the x-axis
:param beta: The pitch angle (radians) - Rotation around the y-axis
:param alpha: The yaw angle (radians) - Rotation around the z-axis

Output
:return: p2: A 3 element array containing the position of a point in the
global reference frame (xG,yG,zG)

"""
p2 = euler_rotation_matrix(alpha, beta, gamma) @ p1
return p2

def main():

# Point that we want to rotate from local frame to global frame
p1 = np.array([5,6,7])

# Rotation angles
alpha = (30/180.0)*np.pi
beta =  (40/180.0)*np.pi
gamma = (70/180.0)*np.pi

print(f'local coordinates p1: {p1}')
print(f'Rotated by Roll {alpha}, Pitch {beta}, Yaw: {gamma}')
p2 = rotate(p1,alpha,beta,gamma)

print(f'global coordinates p2:{p2}')

if __name__ == '__main__':
main()
```

## Example Calculation

Let’s say that we have a point P2 (x,y,z) in the local reference frame that has coordinates (5,-2,7). It is rotated the following angles:

• Yaw = 45°
• Pitch = 25°
• Roll = 52°

### What is the full three-dimensional rotation matrix?

Remember our equation for the full three-dimensional rotation matrix.

We plug the numbers in the problem statement into this equation above.

### What are the coordinates of point P1 (x,y,z), which is the point P2 in terms of the global reference frame?

To get the coordinates of point P1, we need to multiply P2 by the rotation matrix we found above.

You should get the numbers in yellow.

# Converting a Point (Or Vector) in the 3D Global Reference Frame to a Point in the 3D Local Reference Frame

Below we are going to derive the equation that will enable you to convert a point (or vector) in the 3D global reference frame to a point (or vector) in the 3D local reference frame. In other words, we are going to calculate the three-dimensional inverse rotation matrix.

If you remember, when we derived the three-dimensional rotation matrix earlier in this post, we started out with this equation.

You’ll notice that the first operation performed on a point in the local frame is to account for rotation about the x-axis (i.e. roll). Then we do pitch (rotation about the y-axis), and then we do yaw (rotation about the z-axis). This sequence of operations enables us to convert any point (or vector) in the 3D local reference frame to a point (or vector) in the 3D global reference frame.

To calculate the inverse three-dimensional rotation matrix, we need to find the inverse of each of the three matrices and then perform the operations in reverse order. Let’s walk through this together.

First, we need to calculate the inverse for each of the three matrices. The inverse of a rotation matrix R is equal to the rotation matrix’s transpose (RT = R-1), where T means “transpose” and -1 means “inverse”. If you don’t know how to take the transpose of a 3×3 matrix, take a look at this article.

And since the last operation performed is yaw, we need to take its transpose, then take the transpose of the pitch matrix, and then take the transpose of the roll matrix.

Here are the steps of the matrix multiplication:

And there you have it. This matrix below is the full three-dimensional inverse rotation matrix. Use it when you want to convert a point (or vector) in the 3D global reference frame to a point (or vector) in the 3D local reference frame:

## Three-Dimensional Inverse Rotation Matrix in Python Code

Here is a NumPy-based method that converts angles into a 3×3 inverse rotation matrix like the one above. You can use this method in whatever code you want to write.

```import numpy as np

def euler_rotation_matrix(alpha,beta,gamma):
"""
Generate a full three-dimensional rotation matrix from euler angles

Input
:param alpha: The roll angle (radians) - Rotation around the x-axis
:param beta: The pitch angle (radians) - Rotation around the y-axis
:param alpha: The yaw angle (radians) - Rotation around the z-axis

Output
:return: A 3x3 element matix containing the rotation matrix.
This rotation matrix converts a point in the local reference
frame to a point in the global reference frame.

"""
# First row of the rotation matrix
r00 = np.cos(gamma) * np.cos(beta)
r01 = np.cos(gamma) * np.sin(beta) * np.sin(alpha) - np.sin(gamma) * np.cos(alpha)
r02 = np.cos(gamma) * np.sin(beta) * np.cos(alpha) + np.sin(gamma) * np.sin(alpha)

# Second row of the rotation matrix
r10 = np.sin(gamma) * np.cos(beta)
r11 = np.sin(gamma) * np.sin(beta) * np.sin(alpha) + np.cos(gamma) * np.cos(alpha)
r12 = np.sin(gamma) * np.sin(beta) * np.cos(alpha) - np.cos(gamma) * np.sin(alpha)

# Third row of the rotation matrix
r20 = -np.sin(beta)
r21 = np.cos(beta) * np.sin(alpha)
r22 = np.cos(beta) * np.cos(alpha)

# 3x3 rotation matrix
rot_matrix = np.array([[r00, r01, r02],
[r10, r11, r12],
[r20, r21, r22]])

return rot_matrix

def rotate(p1,alpha,beta,gamma):
"""
Rotates a point p1 in 3D space in the local reference frame to
a point p2 in the global reference frame.

Input
:param p1: A 3 element array containing the position of a point in the
local reference frame (xL,yL,zL)
:param alpha: The roll angle (radians) - Rotation around the x-axis
:param beta: The pitch angle (radians) - Rotation around the y-axis
:param alpha: The yaw angle (radians) - Rotation around the z-axis

Output
:return: p2: A 3 element array containing the position of a point in the
global reference frame (xG,yG,zG)

"""
p2 = euler_rotation_matrix(alpha, beta, gamma) @ p1
return p2

def inverse_rotation(p2,alpha,beta,gamma):
"""
Inverse rotation from a point p2 in global 3D reference frame
to a point p1 in the local (robot) reference frame.

Input
:param p2: A 3 element array containing the position of a point in the
global reference frame (xG,yG,zG)
:param alpha: The roll angle (radians) - Rotation around the x-axis
:param beta: The pitch angle (radians) - Rotation around the y-axis
:param alpha: The yaw angle (radians) - Rotation around the z-axis

Output
:return: A 3 element array containing the position of a point in the
local reference frame (xL,yL,zL)

"""
# First row of the inverse rotation matrix
r00 = np.cos(gamma) * np.cos(beta)
r01 = np.sin(gamma) * np.cos(beta)
r02 = -np.sin(beta)

# Second row of the inverse rotation matrix
r10 = np.cos(gamma) * np.sin(beta) * np.sin(alpha) - np.sin(gamma) * np.cos(alpha)
r11 = np.sin(gamma) * np.sin(beta) * np.sin(alpha) + np.cos(gamma) * np.cos(alpha)
r12 = np.cos(beta) * np.sin(alpha)

# Third row of the inverse rotation matrix
r20 = np.cos(gamma) * np.sin(beta) * np.cos(alpha) + np.sin(gamma) * np.sin(alpha)
r21 = np.sin(gamma) * np.sin(beta) * np.cos(alpha) - np.cos(gamma) * np.sin(alpha)
r22 = np.cos(beta) * np.cos(alpha)

# 3x3 inverse rotation matrix
inv_rot_matrix = np.array([[r00, r01, r02],
[r10, r11, r12],
[r20, r21, r22]])

return inv_rot_matrix @ p2

def main():

# Point that we want to rotate from local frame to global frame
p1 = np.array([5,6,7])

# Rotation angles
alpha = (30/180.0)*np.pi
beta =  (40/180.0)*np.pi
gamma = (70/180.0)*np.pi

print(f'local coordinates p1: {p1}')
print(f'Rotated by Roll {alpha}, Pitch {beta}, Yaw: {gamma}')
p2 = rotate(p1,alpha,beta,gamma)

print(f'global coordinates p2:{p2}')
p1_ = inverse_rotation(p2,alpha,beta,gamma)

print(f'inverse rotation back into local frame p1:{p1_}')

if __name__ == '__main__':
main()
```

## How to Describe the Rotation of a Robot in 2D

In this post, we’ll take a look at how to describe the rotation of a robot in a two-dimensional (2D) space.

Consider the robot below. It’s moving around in a 2D coordinate frame. Its position in this coordinate frame can be described at any time by its x-coordinate and its y-coordinate.

We’ll call this coordinate frame the “global” reference frame because it defines the world that the robot is moving around in.

There is also another coordinate frame: the local reference frame. The local reference frame is the coordinate frame from the perspective of the robot (as opposed to the world). Local reference frame…robot reference frame…robot axes…all ways of saying the same thing.

Let’s draw the y and x-axes of the local reference frame. The y-axis points towards the front of the robot, and the x-axis is perpendicular to the y-axis. The local reference frame is labeled with the subscript L, and the global reference frame is labeled with the subscript G.

Notice that the local reference frame is rotated at an angle from the global reference frame. We’ll call this angle γ.

γ = Angular difference between the global x-axis and the local x-axis

You’ll also note that the angular difference between the global y-axis and the local y-axis is also equal to γ.

The rate at which the angle γ is changing (in the counterclockwise direction) is known as the angular velocity, and is often represented by the Greek letter ω. It’s the change in gamma divided by the change in time.

ω = (change in γ) / (change in time) = (γfinal – γinitial) / (tfinal – tinitial)

Now our robot isn’t very useful if all it does is rotate. Let’s suppose the robot is an omnidirectional robot…it can move in any direction. How do we describe the velocity of the robot?

Velocity is a measure of how fast something is moving in a particular direction. It is speed (e.g. miles per hour, meters per second, etc.) + direction.

The velocity of our 2D robot can be broken down into two components: velocity in the x-direction and velocity in the y-direction. The sum of both of those vectors defines the velocity of the robot.

Let’s draw the two velocity vectors right now from the perspective of the local reference frame.

Suppose the robot is moving due north (as shown by the red arrow below) with respect to the global reference frame, while maintaining the same angle of rotation. In other words, the front of the robot is still pointed towards the “northwest,” while the robot is moving directly north, parallel to the global y-axis.

How do we calculate the magnitude of the robot’s velocity (i.e. its speed)?

We need to add the two velocity vectors together, using the head-to-tail method. The head of the first vector points to the tail of the next vector.

You probably notice that the angle between Vx and Vy is 90 degrees, so we can use the Pythagorean Theorem to calculate the speed (i.e. magnitude of the velocity) of the robot.

This velocity above is the velocity of the robot in the local reference frame. In other words, both  Vx and Vy are parallel to the robot’s x-axis and y-axis respectively.

How do we convert these velocities in the local reference frame to velocities in the global reference frame? And why would we want to do that?

The reason for doing this conversion is so that we can know where in the world the robot will be at a particular time in the future. Just knowing the robot velocity vectors doesn’t help us. We need to know how the position of the robot is changing with respect to time in the global reference frame.

So let’s figure that out now. To solve this problem, we’ll need to use some trigonometry.

Let’s start from the graphic you are already familiar with.

Assume the center of the robot above is currently at the origin in the global reference frame.

What is VxG, the velocity in the x-direction on the global reference frame? To answer that, we need to recognize that both the x and y components of the robot’s velocity in the local reference frame can be broken down into global reference frame components (i.e. the pink and green arrows below).

Let’s call the velocity of the robot in the global reference frame VxG and VyG. You can see from the above diagram of the pink and green vectors that:

VxG = (vector 1) – (vector 4)   … Minus sign because vector 4 is pointing in the negative global x-axis direction

VyG = (vector 2) + (vector 3)   … Plus sign because both vectors are pointing in the positive global y-axis direction

Now, how do we find vectors 1, 2, 3, and 4?

Do you remember the trigonometric ratios? Back in middle school, we used the acronym, SOHCAHTOA:

• Sine = Opposite / Hypotenuse (SOH)
• Cosine = Adjacent / Hypotenuse (CAH)
• Tangent = Opposite / Adjacent (TOA)

In the right triangle above: sin A = a/c; cos A = b/c; tan A = a/b.

Using these ratios as our guide, we get:

• vector 1 = VxL * cos(γ)
• vector 2 = VxL * sin(γ)
• vector 3 = VyL * cos(γ)
• vector 4 = VyL * sin(γ)

Plugging this information back in, we get the two equations that enable us to convert local velocities to global velocities.

• VxG = (VxL * cos(γ)) – (VyL * sin(γ))
• VyG = (VxL * sin(γ)) + (VyL * cos(γ))

If you’re familiar with linear algebra, you’ll notice that we can convert the system of equations above into matrix form:

The matrix above with the cosines and sines is often referred to in robotics as the rotation matrix (or two-dimensional rotation matrix). It helps us convert either vectors or a point in the local reference frame to a vector or a point in the global reference frame.

Here is how we convert a point in the local reference frame to a point in the global reference frame:

At this stage, you might now be wondering…how do I convert velocities in the global reference frame to velocities in the local reference frame? Say you were looking down on the robot from up above (i.e. aerial view) and had a radar gun that could measure the velocity of the robot…what would the velocity be from the perspective of the robot’s own x and y axes?

To answer that, let’s draw our local and global velocity vectors.

Now let’s write out equations for the local velocities in the x and y directions:

• VxL = (vector 1) + (vector 4)   … Plus sign because both vectors are pointing in the positive local x-axis direction
• VyL = -(vector 2) + (vector 3)  … Minus sign because vector 2 is pointing in the negative local y-axis direction

Using the trigonometric ratios we discussed earlier, we have the following equations:

• vector 1 = VxG * cos(γ)
• vector 2 = VXG * sin(γ)
• vector 3 = VyG * cos(γ)
• vector 4 = VyG * sin(γ)

Plugging this information back in, we get the two equations that enable us to convert global velocities to local velocities.

• VxL = (VxG * cos(γ)) + (VyG * sin(γ))
• VyL = -(VxG * sin(γ)) + (VyG * cos(γ))

In matrix form, we have:

And there you have it. That’s how to describe the rotation of a robot in two dimensions.

In a future post, I’ll show you how to describe the rotation of a robot in three dimensions.

## Yaw, Pitch, and Roll Diagrams Using 2D Coordinate Systems

In robotics, we commonly represent the position of a robot in space using three axes:

• x-axis
• y-axis
• z-axis

We represent rotation about each of these axes using the following standard names:

• roll (rotation about the x-axis)
• pitch (rotation about the y-axis)
• yaw (rotation about the z-axis)

Now, let’s say we have a 4-wheeled robot in space. The robot is moving in the +y direction as shown in the figure below. The robot turns slightly to the left, rotating around the z-axis.

Rotation about the z-axis is yaw rotation.

What would the new coordinate system look like from the robot’s perspective if we used just a 2D coordinate system (instead of a 3D coordinate system like we did above)?

Yaw rotation means the robot rotates about the z-axis in the counter-clockwise direction, so the robot’s new coordinate system would look like this (where the angle γ represents the amount of rotation of the coordinate axes in degrees or radians):

Note that the z-axis is coming right straight out of the page from that blue arrow above.

Now, here are the illustrations for the roll and pitch rotations using 2D coordinate systems:

The x-axis is that black dot above. It is coming right straight out of the page.

The y-axis is that black dot above. It is coming right straight out of the page.