Definition

In the previous tutorial, we discussed how coordinate frames rotate relative to each other. The goal was to find the orientation of the end effector of a robot (gripper, paint brush, robotic hand, vacuum suction cup, etc.) relative to the base of the robot.

However, when a robotic arm moves around in the world, orientation is just half the puzzle. The end effector changes position as well. To account for this change in the position of the end effector, we use what is called a displacement vector.

A vector is a list of numbers. In robotics, we typically use three numbers (all organized in a single column), to represent displacement (i.e. change in position) of one frame relative to another frame in the x, y, and z directions. 

We’ll use the following notation to represent the displacement of coordinate frame n relative to coordinate frame m.

Example 1 – Displacement in the x direction

We have two coordinate frames: frame 0 and frame 1. Let’s do a brief review of how to find rotation matrices before we get to the displacement vector.

Rotation Matrix

You can see that frame 1 has no rotation relative to frame 0.

Therefore, the first term we’ll use to calculate the rotation matrix from frame 0 to 1 will be the identity matrix. This rotation matrix shows how the axes of frame 1 project onto the axes of frame 0 when there is no rotation.

Now that we’ve converted the axes, we need to complete the derivation of the rotation matrix from frame 1 to 0 by finding the matrix that takes into account the rotation of frame 1 due to changes in θ₁.

θ₁ is a rotation around the z₀ axis. We therefore need to multiply the matrix we found above by the standard form of the z rotation matrix.

Displacement Vector

As mentioned earlier, rotation is just half the puzzle. Now that we know how frame 1 is rotated relative to frame 0, we now need to know how frame 1 is displaced relative to frame 0.

Our displacement vector needs to have three elements:

• Change in the position along the x-direction
• Change in the position along the y-direction
• Change in the position along the z-direction

For example, let’s say that the distance from the origin of frame 1 to the origin of frame 0 is a. There is no displacement in the y and z directions.

Here is the displacement vector:

No matter how much θ₁ changes (i.e. frame 1 rotates relative to frame 0), the origin of frame 1 will remain in the same position. Thus, the displacement vector will always remain the same. 

Now let’s take a look at another example.

Example 2 – Displacement in the y direction

Rotation Matrix

You can see that frame 1 has no rotation relative to frame 0.

Therefore,

Displacement Vector

In the graphic below, all we have is displacement in the y direction. I’ll call this distance b.

Here is the displacement vector:

No matter how much frame 1 rotates, the distance between the origins of both frames will remain the same. So the displacement vector will always be what you see above regardless of the value of the joint variable θ₁.

Example 3 – Displacement in the z direction

Rotation Matrix

You can see that frame 1 has no rotation relative to frame 0.

Therefore,

Displacement Vector

All we have is displacement in the z direction. I’ll call this distance c.

Here is the displacement vector:

Example 4 – Two Degree of Freedom Robotic Arm

Now, let’s take a look at some real-world examples. We’ll draw the displacement vector for a two degree of freedom robotic arm. 

Here is a kinematic diagram for a two degree of freedom robotic arm.

Rotation Matrix

Here are the rotation matrices for this two-degree of freedom manipulator.

The complete rotation matrix is as follows:

rot_mat_0_2 = (rot_mat_0_1)(rot_mat_1_2)

Displacement Vector

Let’s determine the displacement vector from frame 0 to frame 1. We need to find the displacement in the x₀, y₀, and z₀ direction.

The problem with this displacement vector above though is that it only makes sense when θ₁ = 0 degrees. What happens when θ₁ = 90 degrees?

You see that the displacement vector is different when θ₁ = 90 degrees. When θ₁ changes, the origin of frame 1 rotates around z₀. Therefore, the displacement of the origin of frame 1 relative to the origin of frame 0 changes as the joint variable changes.

To be able to control the position of the end effector in code, we need to find a displacement vector that will be correct regardless of the value of θ₁. To do that, we have to make the values in the displacement vector dependent on the joint variables θ.

Let’s see how to do this now.

We will draw an aerial view of the kinematic diagram we just worked with. The birds-eye view would look like this:

Now let’s assume that θ₁ = 45 degrees.

What would our displacement vector be for the displacement of frame 1 relative to frame 0?

Using trigonometry, we know that:

• cos(θ₁) = (length of red dashed line) / a₂   [displacement in the x₀ direction]
• sin(θ₁) = (length of green dashed line) / a₂   [displacement in the y₀ direction]
• a₁ [displacement in the z₀ direction]

The red dashed line represents the displacement of frame 1 relative to frame 0 in the x₀ direction. 

The green dashed line represents the displacement of frame 1 relative to frame 0 in the y₀ direction. 

Therefore, using algebra to solve for the length of both of these dashed lines, our displacement vector is:

Now let’s look at the displacement from frame 1 to frame 2. We’ll just examine both of these frames and forget frame 0 which we’ve already taken care of.

Using trigonometry, we know that:

• cos(θ₂) = (length of green dashed line) / a₄      [displacement in the x₁ direction]
• sin(θ₂) = (length of red dashed line) / a₄      [displacement in the y₁ direction]
• a₃   [displacement in the z₁ direction]

Example 5 – Cartesian Robot

Now, let’s derive the displacement vectors for a cartesian robot.

Let’s examine the displacement vector of frame 1 relative to frame 0. The kinematic diagram above shows the position and orientation of all the joints and links when the joint variable values are all 0 (i.e. the prismatic joints are in their home position…unextended).

Since we have only prismatic joints in this case, the direction of displacement will always stay constant. 

In our case, the only change to the value of displacement will be along the z₀ axis. The value of this displacement is a₁ + d₁, where d₁ represents the amount of extension of the joint when the cartesian robot is powered on. 

Therefore,

Following the same logic, the displacement vector of frame 1 to frame 2 is…

And finally, let’s write the displacement vector that represents the displacement from the origin of frame 2 to the origin of frame 3.

Example 6 – Articulated Robot

We’ll start by deriving the displacement vector from frame 0 to frame 1. We have to look and see how the position of the origin of frame 1 changes when the value of θ₁ changes.

Does it change? No. 

No matter what value θ₁ is, the origin will stay in the exact same position in 3D space. In this case, we have a fixed displacement vector. There is only displacement along the z₀ axis, therefore:

Let’s take a look at the displacement from the origin of frame 1 to the origin of frame 2. We have to look and see how the position of the origin of frame 2 changes when the value of θ₂ changes. 

Imagine θ₂ = 45 degrees. You can see that the origin of frame 2 changed along the y₁ axis as well as the x₁ axis. There was no change along the z₁ axis.

Now let’s look at the displacement of the origin of frame 3 from the origin of frame 2. The same logic applies:

References

Credit to Professor Angela Sodemann for teaching me this stuff. Dr. Sodemann is an excellent teacher (She runs a course on RoboGrok.com). On her YouTube channel, she provides some of the clearest explanations on robotics fundamentals you’ll ever hear.

In this tutorial, we’ll learn how to find the rotation matrices for different types of robotic arms. Rotation matrices help us represent the orientation of a robotic arm (i.e. which way a robotic arm is pointing). 

Rotation matrices will help us determine how the end effector of a robot (i.e. robotic gripper, paint brush, robotic hand, vacuum suction cup, etc.) changes orientation due to changes in the servo motor angles (i.e. joint variables).

Prerequisites

• You have Python installed and know how to write and run a basic program (e.g. “Hello World”).
  • If you’re using Windows or MacOS, you can download Anaconda and get access to Python that way. 
• You have NumPy installed.  NumPy is a scientific computing library for Python.
  • If you’re using Windows or macOS, you can download Anaconda and then use the following command to install NumPy (conda install numpy).
  • If you’re using Linux, you can use this command in the terminal window (pip3 install numpy).
• You know how to multiply matrices (if you don’t know, there are a bunch of videos on YouTube that explain how to do it).

Getting Started

In robotics, the orientation of a robotic system can be represented in mathematical terms using rotation matrices. Rotation matrices transform the coordinate axes (e.g. x, y, and z) representing the orientation of a 3D object in one frame to the coordinate axes of  another frame.

The definition of a rotation matrix is important, so be sure to refer back to it so you keep the big picture in mind.

Now, before you proceed in this section, read through my two posts where I explain:

• How to Describe the Rotation of a Robot in 2D
• How to Describe the Rotation of a Robot in 3D

In those cases, we use a robotic car, but the same principles apply to a robotic arm.

In the case of a two degree of freedom robotic arm, we have three coordinate frames (i.e. reference frames). 

• The base coordinate frame (x₀, y₀, and z₀ axes in the image below) could be our global reference frame. 
• We then have a local reference frame (x₁, y₁, z₁) that is rotated at an angle from the global reference frame. 

After that, we have another local reference frame (x₂, y₂, z₂) that is rotated relative to the local reference frame before it (x₁, y₁, z₁).

Imagine the base reference frame (x₀, y₀, z₀) is a person. Our base frame might ask, ”Hey, I see the (x₂, y₂, z₂) reference frame is rotated. What is the orientation of that reference frame in terms of my own reference frame?”

To answer that question, we would need to convert the (x₂, y₂, z₂) frame to the (x₁, y₁, z₁) frame using a rotation matrix. We would then convert the  (x₁, y₁, z₁) frame to the (x₀, y₀, z₀) frame using another rotation matrix. We would then have our answer.

Specifically, in our example above, when any of the servos above move, the only rotation you will have is rotation around the z-axes. There is no rotation around either the x or y-axes with any of these joints. The matrix that enables you to convert between two reference frames when you only have rotation around the z-axis is below where the joint variables of your servo motors (θ) would be plugged into where you see γ:

You can see how rotation matrices are powerful tools in robotics. With rotation matrices, we can calculate the orientation of a robotics gripper (i.e. the end effector, (x₂, y₂, z₂) ) in terms of the base reference frame (x₀, y₀, and z₀) using a sequence of matrix multiplications. This is particularly useful for applications where you need the gripper to point a particular direction (e.g. a robotic arm painting an automobile).

Example 1 – Two Degree of Freedom Robotic Arm

Let’s draw the rotation matrices for this two-degree of freedom manipulator.

As θ₁ changes, frame 1 will rotate around the z₀ axis. We can therefore represent this rotation using the standard z rotation matrix. We also need to multiply this matrix by the matrix that represents the projection of frame 1 on frame 0 when the joint variable is θ₁  = 0 degrees.

Following the same logic, the rotation matrix between frame 1 and frame 2 is:

The complete rotation matrix is as follows:

rot_mat_0_2 = (rot_mat_0_1)(rot_mat_1_2)

Let’s look at an example of how rotation matrices work in actual code. We’ll use Python.

Open up a new Python file and write the following code. As a guideline, the matrix “rot_mat_0_1” in the code below is the rotation matrix that would convert the (x₁, y₁, z₁) frame to the base frame (x₀, y₀, z₀). The matrix follows the format below where (x_L, y_L, z_L) is  (x₁, y₁, z₁) and  (x_G, y_G, z_G) is  (x₀, y₀, z₀). γ is the servo (joint) angle.

In the code, make sure:

• servo_0_angle = 0
• servo_1_angle = 0

import numpy as np # Scientific computing library

# Project: Calculating Rotation Matrices
# Author: Addison Sears-Collins
# Date created: August 1, 2020

# Servo (joint) angles in degrees
servo_0_angle = 0 # Joint 1
servo_1_angle = 0 # Joint 2

# Convert servo angles from degrees to radians
servo_0_angle = np.deg2rad(servo_0_angle)
servo_1_angle = np.deg2rad(servo_1_angle)

# Define the first rotation matrix.
# This matrix helps convert frame 1 to frame 0.
# There is only rotation around the z axis of servo_0.
rot_mat_0_1 = np.array([[np.cos(servo_0_angle), -np.sin(servo_0_angle), 0],
                        [np.sin(servo_0_angle), np.cos(servo_0_angle), 0],
                        [0, 0, 1]]) 

# Define the second rotation matrix.
# This matrix helps convert the 
# end-effector frame (frame 2) to frame 1.
# There is only rotation around the z axis of servo_1 (joint 2).
rot_mat_1_2 = np.array([[np.cos(servo_1_angle), -np.sin(servo_1_angle), 0],
                        [np.sin(servo_1_angle), np.cos(servo_1_angle), 0],
                        [0, 0, 1]]) 

# Calculate the rotation matrix that converts the 
# end-effector frame to the servo_0 frame.
rot_mat_0_2 = rot_mat_0_1 @ rot_mat_1_2

# Display the rotation matrix
print(rot_mat_0_2)

Now, run the code. Your output should be the identity matrix. What does this mean? It means that when both servo angles are 0 (remember in a kinematic diagram, we assume that all servo angles are 0 degrees), there is no rotation between frame 0 and frame 2.

When you look at the kinematic diagram below, you can see that the third reference frame (frame 2) has the exact same orientation as the first reference frame (frame 0).

Now, let’s take a look at a case where  θ₁ = 90 degrees (Servo 0 angle). Here is the kinematic diagram.

Make sure:

• servo_0_angle = 90
• servo_1_angle = 0

Here is the output:

The value in row 0, column 0 is extremely small. We can call it 0. Let’s put the output above in a table to make it easier to see.

Rotation Matrix From Frame 0 to Frame 2 When θ₁ = 90 degrees

The table above is another way to look at a rotation matrix. It shows the projection of the x₂, y₂, and z₂ axes on the x₀, y₀, z₀ axes, respectively. I use the word projection here because what a rotation matrix does is project the coordinate axes of one reference frame onto the coordinate axes of another reference frame…in this case, projecting the axes of frame 2 onto the axes of frame 0.

• If an axis in one frame points in the same direction as an axis in another frame, we put in a 1 in that cell of the matrix.
• If an axis in one frame points in the opposite direction as an axis in another frame, we put a -1 in that cell of the matrix.
• If an axis in one frame is perpendicular to an axis in another frame, we put a 0 in that cell of the matrix.

The template for a rotation matrix in this form is below. We want to know the projection of the n coordinate frame on the m coordinate frame. Another way to say this is that rot_mat_m_n is the rotation of frame n relative to frame m:

The yellow cells below, for example, tells us that the projection of x₂ on y₀ is 1. Does this make sense when we look at the kinematic diagram?

We can see in the diagram above that x₂ has the same orientation (i.e. pointing exact same direction) as y₀. So it makes sense that the value in the rotation matrix is 1.

Let’s look at another example. Column 2 shows that the projection of y₂ on x₀ is -1. Does this make sense?

We can see in the kinematic diagram below that y₂ points in the exact opposite direction of x₀. It therefore makes sense that we have -1 there in the second column.

You can see in the third column of the rotation matrix that z₂ and z₀ both have the same orientation. Therefore, we have 1 in that column (and 0s in the other rows of that column).

You can change the value of the joint variables to whatever angle you want. Try:

• servo_0_angle = 12
• servo_1_angle = 63

What was your output? Did you get this?

Example 2 – Cartesian Robot

Let’s see how to create the rotation matrices for the cartesian robot.

Our goal is to find a rotation matrix that can help us convert frame 3 (the end-effector frame) to frame 0 (the base frame). My notation for this rotation matrix is rot_mat_0_3. 

To find rot_mat_0_3, we need to first find the “internal” rotation matrices. That is the rotation matrices from frame 3 to frame 2, from frame 2 to frame 1, and then from frame 1 to frame 0. We then multiply these rotation matrices together to get the final rotation matrix.

Here is how all this looks mathematically:

rot_mat_0_3 = (rot_mat_0_1)(rot_mat_1_2)(rot_mat_2_3)

Let’s go through this step by step. First, let’s find rot_mat_0_1.

The first column represents the projection x₁ on each of the axes of frame 0. We can see that x₁ has the same orientation as y₀. We put a 1 in that corresponding cell in the rotation matrix. We put a 0 in the other cells because x₁ is perpendicular to x₀ and z₀. In this case, we are assuming the joint variables are all 0…i.e. none of the linear actuators are extended from their 0 position.

Using the same logic above, we can fill out the second column of the table. y₁ has the same orientation as z₀.

We then finish the third column of the rotation matrix.

Therefore in the case where the joint variables are all 0.

That matrix above tells us how frame 1 converts to frame 0 when the joint variable is 0 (i.e. the linear actuator is not extended).

Now let’s create the rotation matrix between frame 2 and frame 1. Remember our diagram:

You can see that frame 3 has the same orientation as frame 2. Therefore, we use the identity matrix.

Remember the matrix above tells us how frame 3 converts to frame 2 when the joint variable is 0 (i.e. the linear actuator is not extended). However, since there is no rotation for any of the joints (since they are all prismatic joints), the three rotation matrices we’ve derived above will be the exact same for any value of the joint variable.

Example 3 – Articulated Robot

Now let’s create the rotation matrices for an articulated manipulator.

Our goal is to find a rotation matrix that can help us convert frame 3 (the end-effector frame) to frame 0 (the base frame). My notation for this rotation matrix is rot_mat_0_3. 

rot_mat_0_3 = (rot_mat_0_1)(rot_mat_1_2)(rot_mat_2_3)

Let’s go through this step by step. First, let’s find rot_mat_0_1.

The first column represents the projection x₁ on each of the axes of frame 0. We can see that x₁ has the same orientation as x₀. We put a 1 in that corresponding cell in the rotation matrix. We put a 0 in the other cells because x₁ is perpendicular to y₀ and z₀.

Using the same logic above, we can fill out the second column of the table. y₁ has the same orientation as z₀.

We then finish the third column of the rotation matrix.

The matrix above tells us how frame 1 converts to frame 0 when the joint variable is 0 (i.e. the linear actuator is not extended). But are we done yet? No we are not.

The matrix above only tells us how frame 1 converts to frame 0 when the joint variables (i.e. servo angles) are at 0 degrees. All we’ve done so far is convert one axes to another, but we haven’t accounted for the fact that a revolute joint (in comparison to the prismatic, linear actuator joints we worked with in the cartesian robots case) rotates to different angles when the articulated robot (e.g. robotic arm) is in action.

If all we used was the matrix above, that wouldn’t be sufficient because we have no way of converting frame 1 to frame 0 when the robot is in action. We need to add another matrix that takes into account the rotation of the base frame servo (i.e. changes in θ₁).

Look at the diagram above. You can see that frame 1 can rotate in response to changes in θ₁. When θ₁ changes, it causes rotation around both the (I’ve linked below to the standard form of the y and z rotation matrices…derivation included in case you’re interested where these matrices come from):

• y₁ axis of frame 1
• z₀ axis of frame 0

Which matrix do we use? 

It doesn’t matter. However, which one we choose impacts the order of the matrix multiplication. Remember that, unlike regular multiplication, the order in which two matrices are multiplied matters.

If you use the standard z-rotation matrix (assume rotation around the z₀ axis of frame 0), you would place that before the matrix we found earlier. 

If you use the standard y-rotation matrix (assume rotation around the y₁ axis of frame 1), you would place that after the matrix we found earlier (i.e. on the right). That would look like this:

Multiplying the matrices above, together, we get:

The middle column of the matrix above represents the projection of the y₁ axis onto frame 0. You can see that, no matter what servo 0 (i.e. joint 1) does, y₁ will always point in the same direction as z₀.

Now let’s create the rotation matrix between frame 2 and frame 1. Remember our diagram:

The first thing we need to do is to convert the x, y, and z axes of frame 2 to the x, y, and z axes of frame 1 assuming all the joint variables (i.e. the servo angles θ) are 0 degrees. You can see in the kinematic diagram that:

• x-axis of frame 2 is just like the x-axis of frame 1
• y-axis of frame 2 is just like the y-axis of frame 1
• z-axis of frame 2 is just like the z-axis of frame 1

Therefore, the first term we’ll use to calculate the rotation matrix from frame 2 to 1 will be the identity matrix.

Now that we know the frame 1 equivalent to the axes in frame 2, we need to complete the derivation of the rotation matrix from frame 2 to 1 by finding the matrix that takes into account the rotation of frame 2 due to changes θ_2.. 

θ₂ is a rotation around the z₁ axis and also the z₂ axis. We therefore need to multiply the matrix we found above by the standard form of the z rotation matrix. 

You can see that frame 3 has the same orientation as frame 2. Therefore, the same logic applies again.

Now we’re done. The three matrices above would go into your code and would enable you to flexibly convert frame 3 (end-effector frame) into frame 0, given that you know the angles of each servo motor.

rot_mat_0_3 = (rot_mat_0_1)(rot_mat_1_2)(rot_mat_2_3)

Example 4 – Six Degree of Freedom Robotic Arm

Draw the Denavit-Hartenberg Frames

Now let’s create the rotation matrices for a 6 degree of freedom robotic arm like the one pictured below.

Our goal is to find a rotation matrix that can help us convert frame 5 (the point where the tip of both fingers of the robotic gripper make contact with each other) to frame 0 (the base frame). My notation for this rotation matrix is rot_mat_0_5.

rot_mat_0_5 = (rot_mat_0_1)(rot_mat_1_2)(rot_mat_2_3)(rot_mat_3_4)(rot_mat_4_5)

Let’s go through this step by step. First, let’s find rot_mat_0_1.

Now we have one matrix needed to calculate the rot_mat_0_1. We need to find the other now. To do that, we need to now account for rotation between the two frames. Frame 1 can rotate in response to changes in θ₁. When θ₁ changes, it causes rotation around the z₀ axis of frame 0.

Let’s use the standard z-rotation matrix (assume rotation around the z₀ axis of frame 0). We need to place that before the matrix we just found:

Multiplying the matrices above, together, we get:

Now let’s create the rotation matrix between frame 2 and frame 1. Remember our diagram:

The first thing we need to do is to convert the x, y, and z axes of frame 2 to the x, y, and z axes of frame 1. You can see in the kinematic diagram that:

• x-axis of frame 2 is just like the x-axis of frame 1
• y-axis of frame 2 is just like the y-axis of frame 1
• z-axis of frame 2 is just like the z-axis of frame 1

Therefore, the first term we’ll use to calculate the rotation matrix from frame 2 to 1 will be the identity matrix.

Now that we know the frame 1 equivalent to the axes in frame 2, we need to complete the derivation of the rotation matrix from frame 2 to 1 by finding the matrix that takes into account the rotation of frame 2 due to changes θ_2.. 

θ₂ is a rotation around the z₁ axis. z₁ and the z₂ axis both point the same direction. We therefore need to multiply the matrix we found above by the standard form of the z rotation matrix. We can put it before or after…it doesn’t matter in this case. Let’s put it before.

You can see that frame 3 has the same orientation as frame 2. Therefore, the same logic applies again.

Now, let’s find rot_mat_3_4.

Now that we’ve projected the axes of frame 4 onto frame 3, we need to account for rotation between the axes.

Frame 4 can rotate in response to changes in θ₄. When θ₄ changes, it causes rotation around the z₃ axis of frame 3.

Let’s use the standard z-rotation matrix (assume rotation around the z₃ axis of frame 3). We need to place that before the matrix we just found:

Now we multiply the matrices together:

Finally, we need to convert from frame 5 to frame 4. You can see that both frames have the same orientation, so we use the identity matrix to project the axes of frame 5 on to frame 4.

Now we need to complete the derivation of the rotation matrix from frame 5 to 4 by finding the matrix that takes into account the rotation of frame 5 due to changes in θ_5. 

θ₅ is a rotation around the z₄ axis. z₄ and the z₅ axes both point the same direction. We therefore need to multiply the identity matrix by the standard form of the z rotation matrix.

Create the Code in Python

Here is the code. I saved the file as rotation_matrices_6dof.py.

import numpy as np # Scientific computing library

# Project: Calculating Rotation Matrices for a 6 DOF Robotic Arm
# Author: Addison Sears-Collins
# Date created: August 6, 2020

# Servo (joint) angles in degrees
servo_0_angle = 0 # Joint 1
servo_1_angle = 0 # Joint 2
servo_2_angle = 0 # Joint 3
servo_3_angle = 0 # Joint 4
servo_4_angle = 0 # Joint 5

# This servo would open and close the gripper (end-effector)
servo_5_angle = 0 # Joint 6

# Convert servo angles from degrees to radians
servo_0_angle = np.deg2rad(servo_0_angle)
servo_1_angle = np.deg2rad(servo_1_angle)
servo_2_angle = np.deg2rad(servo_2_angle)
servo_3_angle = np.deg2rad(servo_3_angle)
servo_4_angle = np.deg2rad(servo_4_angle)
servo_5_angle = np.deg2rad(servo_5_angle)

# This matrix helps convert the servo_1 frame to the servo_0 frame.
rot_mat_0_1 = np.array([[np.cos(servo_0_angle), 0, np.sin(servo_0_angle)],
                        [np.sin(servo_0_angle), 0, -np.cos(servo_0_angle)],
                        [0, 1, 0]]) 

# This matrix helps convert the servo_2 frame to the servo_1 frame.
rot_mat_1_2 = np.array([[np.cos(servo_1_angle), -np.sin(servo_1_angle), 0],
                        [np.sin(servo_1_angle), np.cos(servo_1_angle), 0],
                        [0, 0, 1]]) 

# This matrix helps convert the servo_3 frame to the servo_2 frame.
rot_mat_2_3 = np.array([[np.cos(servo_2_angle), -np.sin(servo_2_angle), 0],
                        [np.sin(servo_2_angle), np.cos(servo_2_angle), 0],
                        [0, 0, 1]]) 

# This matrix helps convert the servo_4 frame to the servo_3 frame.
rot_mat_3_4 = np.array([[-np.sin(servo_3_angle), 0, np.cos(servo_3_angle)],
                        [np.cos(servo_3_angle), 0, np.sin(servo_3_angle)],
                        [0, 1, 0]]) 

# This matrix helps convert the servo_5 frame to the servo_4 frame.
rot_mat_4_5 = np.array([[np.cos(servo_4_angle), -np.sin(servo_4_angle), 0],
                        [np.sin(servo_4_angle), np.cos(servo_4_angle), 0],
                        [0, 0, 1]]) 

# Calculate the rotation matrix that converts the 
# end-effector frame (frame 5) to the servo_0 frame.
rot_mat_0_5 = rot_mat_0_1 @ rot_mat_1_2 @ rot_mat_2_3 @ rot_mat_3_4 @ rot_mat_4_5

# Display the rotation matrix
print(rot_mat_0_5)

Here is the output:

We see that the rotation matrix that converts the end-effector frame into the base frame is:

As an error check, see if the matrix above makes sense.

1. Column 1 is saying that x₅ and z₀ point the same direction, and every other axis combination is perpendicular.
2. Column 2 indicates that y₅ and y₀ point in the opposite direction, and every other axis combination is perpendicular.
3. Column 3 indicates that z₅ and x₀ point the same direction, and every other axis combination is perpendicular.

You can see in the diagram below that the rotation matrix from frame 0 to frame 5 when all joint variables are 0 degrees makes sense.

Going Beyond Rotation

Up until now, we have learned how to account for changes in the orientation of reference frames in robotic manipulators. We know how to convert the orientation of the end-effector frame into the orientation of the base 0 frame. All we need to know are the angles of each servo, and we can plug those into the matrices and solve for the solution. 

But rotation (i.e. orientation) is just half of the puzzle. The end-effector (i.e. paint brush, robotic gripper, robotic hand, vacuum suction cup, etc.) can change position as well as orientation. If we want to program a robot to pick up an item off a conveyor belt in a factory, we need to make sure the end-effector is oriented correctly, but we also need to make sure it goes to the right position in three-dimensional space (i.e. has the right x, y, and z coordinates). 

After all, the origin (0, 0, 0) of the reference frame (i.e. x-y-z coordinate frame) for the end-effector is not located in the same position as the origin of the base frame. So in addition to the end-effector’s orientation, we also need to know its position in terms of the base frame if we want to have our robot do useful work. 

We have ignored the link lengths (labeled with the letter a) and the fact that the end-effector is displaced from the base frame. 

In my next post, we’ll examine how to account for this displacement.

Keep building!

References

Credit to Professor Angela Sodemann from whom I learned these important robotics fundamentals. Dr. Sodemann teaches robotics over at her website, RoboGrok.com. While she uses the PSoC in her work, I use Arduino and Raspberry Pi since I’m more comfortable with these computing platforms. Angela is an excellent teacher and does a fantastic job explaining various robotics topics over on her YouTube channel.

In this tutorial, we’ll learn the fundamentals of assigning Denavit-Hartenberg coordinate frames (i.e. x, y, and z axes) to different types of robotic arms.

Denavit-Hartenberg (D-H) frames help us to derive the equations that enable us to control a robotic arm. 

The D-H frames of a particular robotic arm can be classified as follows:

• Global coordinate frame: This coordinate frame can have many names…world frame, base frame, etc. In this two degree of freedom robotic arm, the global coordinate frame is where the robot makes contact with the dry-erase board.
• Joint frames: We need a coordinate frame for each joint.
• End-effector frame: We need a coordinate frame for the end effector of the robot (i.e. the gripper, hand, etc….that piece of the robot that has a direct effect on the world).

To draw the frames (i.e. x, y, and z axes), we follow four rules that will enable us to take a shortcut when deriving the mathematics for the robot. These rules collectively are known as the Denavit-Hartenberg Convention.

Four Rules of the Denavit-Hartenberg Convention

Here are the four rules that guide the drawing of the D-H coordinate frames:

1. The z-axis is the axis of rotation for a revolute joint. 
2. The x-axis must be perpendicular to both the current z-axis and the previous z-axis.
3. The y-axis is determined from the x-axis and z-axis by using the right-hand coordinate system.
4. The x-axis must intersect the previous z-axis (rule does not apply to frame 0).

1. The z-axis is the axis of rotation for a revolute joint (like Joints 1 and 2 in the diagram below). 

For example, in the diagram below, the z-axis (pink line) for Joint 1 will point straight upwards out of the servo motor. This coordinate frame is the global reference frame. It is the frame that is connected to the first joint.

Let’s draw the second z-axis.

For our last z-axis, I have a choice. I’ll put it in the same direction as the second z-axis.

2. The x-axis must be perpendicular to both the current z-axis and the previous z-axis.

Let’s draw the x-axis for the global reference frame. We need to make sure it is perpendicular to the z₀ axis. We have a choice here. I’ll go with the x-axis pointing to the right since it is easier to see on the diagram.

Let’s draw the x-axis for the Joint 2 reference frame. We need to make sure it is perpendicular to both z₀ and z₁.

Now, let’s draw x₂.

3. The y-axis is determined from the x-axis and z-axis by using the right-hand coordinate system.

For the right-hand rule, you:

• Take your right hand and point your four fingers in the direction of the x-axis.
• Point your thumb in the direction of the z-axis.
• Your palm points in the direction of the y-axis.

So, in this diagram below, how do we draw y₀?

• x₀ points to the right (point the four fingers of your right hand in that direction).
• z₀ points toward the sky (point your thumb in that direction).
• Therefore, y₀ points into the page.

Below I have drawn the y-axis for coordinate frames 1 and 2 using the right-hand rule.

4. The x-axis must intersect the previous z-axis (rule does not apply to frame 0).

You can see from the red lines below that this rule holds for the diagram we drew.

More Practice With Denavit-Hartenberg Frames

Example 1 – Two Degree of Freedom Robotic Arm

Let’s get some more practice drawing D-H frames on a kinematic diagram. We’ll use this diagram below of the two-degree of freedom robotic arm.

Remember the four rules.

Rule #1: The z-axis is the axis of rotation for a revolute joint (like Joints 1 and 2 in the diagram below). 

Rule #2: The x-axis must be perpendicular to both the current z-axis and the previous z-axis.

Rule #3: The y-axis is determined from the x-axis and z-axis by using the right-hand coordinate system.

Rule #4: The x-axis must intersect the previous z-axis (rule does not apply to frame 0).

We draw a dashed line extending the x and z-axes and confirm that the axes intersect.

Example 2 – Cartesian Robot

Let’s do some more examples so that you get comfortable drawing kinematic diagrams. 

We’ll start with the cartesian robot. You’ll often see this robot in 3D printing, laser cutting, and computer numerical control (CNC) applications.

Here is an example of a cartesian robot.

A cartesian robot is made up of three prismatic joints, that correspond to the x, y, and z axes. These joints are perpendicular to each other.

Whereas a revolute joint produces rotational motion, a prismatic joint produces linear (i.e. sliding) motion along a single axis. In a real application, a prismatic joint is a linear actuator. This type of actuator can be purchased at any online store that sells electronics equipment (e.g. Amazon, eBay, etc.).

Let’s draw the kinematic diagram for a cartesian robot.

Here is our first joint:

Let’s add our second and third joint.

Now, let’s label the links. When drawing the kinematic diagram for prismatic joints, we assume that each joint is not extended.

Let’s draw in arrows to show the direction of motion (we’ll use the letter d to represent the direction of motion from a 0 position of the linear actuator…i.e. prismatic joint).

Let’s draw the axes. 

Rule #1: For a prismatic joint, the z-axis has to be the direction of motion.

Rule #2: The x-axis must be perpendicular to both the current z-axis and the previous z-axis.

Rule #3: The y-axis is determined from the x-axis and z-axis by using the right-hand coordinate system.

You stick your fingers in the direction of x. Your thumb goes in the direction of z. Your palm faces the direction of y.

Rule #4: The x-axis must intersect the previous z-axis (rule does not apply to frame 0).

Check each frame to see if this rule holds. You extend the z-axes and see if they intersect the next x axis. You’ll find that Rule 4 will be followed for all frames.

Example 3 – Articulated Robot

Articulated robots are your standard robot arms. They are the most common types of robots you will find in factories. This type of robot is the one that is most similar to the human arm.

Here is an example of an articulated robot.

Articulated robots come in all shapes and sizes. Some of these types of robots can be pretty strong. If you go inside a car factory, you’ll see giant articulated robots lifting cars and trucks with ease.

Let’s draw the kinematic diagram for a three degree of freedom articulated robot. This robot is similar to an old robot named the Stanford Arm. The difference is that, in this robot diagram, we will make the third joint a revolute joint instead of a prismatic joint.

Let’s label the links (we’ll use the letter a to represent link lengths) and draw the direction of positive rotation.

Now, let’s go through the four rules.

Rule #1: The z-axis is the axis of rotation for a revolute joint (like Joints 0 and 1 in the diagram below).

For the end effector in the image above, I made the z-axis the same direction as the frame before it since it will make the math easier.

Rule #2: The x-axis must be perpendicular to both the current z-axis and the previous z-axis.

Note that, for the base frame, we can set the x-axis to be anything we want as long as rule #2 holds. I’ve made it go to the right in the diagram below.

For drawing the other x axes, you have a choice of which direction you want to make them. I like to look ahead to rule #4 (Rule #4: The x-axis must intersect the previous z-axis) to help with this decision.

Rule #3: The y-axis is determined from the x-axis and z-axis by using the right-hand coordinate system.

You stick your fingers in the direction of x. Your thumb goes in the direction of z. Your palm faces the direction of y.

Rule #4: The x-axis must intersect the previous z-axis (rule does not apply to frame 0).

Check each frame to see if this rule holds. You extend the z-axes and see if they intersect the next x axis. You’ll find that Rule 4 will be followed for all frames.

Example 4 – SCARA Robot

Let’s see how to draw the kinematic diagram for the SCARA robot.

Here is an example of the SCARA robot:

The SCARA robot is commonly used for pick and place (i.e. moving a part from one point to another) and small assembly applications.

Rule #1: The z-axis is the axis of rotation for a revolute joint. For a prismatic joint, the z-axis has to be the direction of motion.

Rule #2: The x-axis must be perpendicular to both the current z-axis and the previous z-axis.

Rule #3: The y-axis is determined from the x-axis and z-axis by using the right-hand coordinate system.

Rule #4: The x-axis must intersect the previous z-axis (rule does not apply to frame 0).

Check each frame to see if this rule holds. You extend the z-axes and see if they intersect the next x axis. You’ll find that Rule 4 will be followed for all frames.

Example 5 – Six Degree of Freedom Robotic Arm

Now, let’s draw the kinematic diagram and D-H frames for a 6 degree of freedom robotic arm like the one below. 

Note that the 6th servo is located on the gripper. I won’t include that joint in the analysis since it is not part of the main arm of the robot.

We start by drawing the kinematic diagram. Remember that, in a kinematic diagram, we assume that all servos are at 0 degrees (i.e. all joint variables are 0). Therefore, for some servos, we’ll have to make the assumption that the angle range of the servo is from -90 to 90 degrees as opposed to the normal 0 to 180 degree range so that we have a valid kinematic diagram.

Let’s label the links and draw the direction of positive rotation.

Now, let’s go through the four rules.

Rule #1: The z-axis is the axis of rotation for a revolute joint.

Rule #2: The x-axis must be perpendicular to both the current z-axis and the previous z-axis.

Up until now, we have always placed the origin of a coordinate frame at the center of the joint. However, doing this is not required. We can place the frame origin wherever we want.

Notice in the diagram below that we have to move the origin of frame 4 backwards, by a distance of a₄ in order to satisfy Rule #2.

Rule #3: The y-axis is determined from the x-axis and z-axis by using the right-hand coordinate system.

You stick your fingers in the direction of x. Your thumb goes in the direction of z. Your palm faces the direction of y.

Rule #4: The x-axis must intersect the previous z-axis.

If you go frame by frame in the diagram above, you can see that the rule holds.

Example 6 – Six Degree of Freedom Collaborative Robot

Let’s draw the kinematic diagram for a six degree of freedom robot like the Universal Robots UR5. At the end of this robotic arm, you would typically have some sort of end effector like a hand, gripper, or suction cup.

The kinematic diagram will be drawn with the robotic arm in a flat orientation, parallel to a table, for  example.

Remember the four rules:

1. The z-axis is the axis of rotation for a revolute joint. 
2. The x-axis must be perpendicular to both the current z-axis and the previous z-axis.
3. The y-axis is determined from the x-axis and z-axis by using the right-hand coordinate system.
4. The x-axis must intersect the previous z-axis (rule does not apply to frame 0).

We can see in the diagram that all the rules hold.

Example 7 – Six Degree of Freedom Industrial Robot

Let’s draw the kinematic diagram for a six degree of freedom industrial robot like the FANUC LRMate 200iD.

Go through each of the four rules. Here is what I drew:

Keep building!

References

Credit to Professor Angela Sodemann for teaching me this stuff. Dr. Sodemann is an excellent teacher (She runs a course on RoboGrok.com). On her YouTube channel, she provides some of the clearest explanations on robotics fundamentals you’ll ever hear.