How to Find Displacement Vectors for Robotic Arms

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.

1-displacement-vectorJPG

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.

2-no-rotationJPG

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.

3-rotation-frame-0-to-1JPG

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

4-rotation-frame-0-1JPG

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

5-standard-form-z-rot-matrixJPG
6-rot-mat-0-1JPG

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.

7-no-displacement-y-z-directionsJPG

Here is the displacement vector:

8-here-is-the-displacement-vectorJPG

No matter how much θ1 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.

9-frame-1-no-rotationJPG

Therefore,

10-rotation-matrix-0-to-1JPG

Displacement Vector

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

11-distance-bJPG

Here is the displacement vector:

12-here-is-the-displacement-vectorJPG

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

Example 3 – Displacement in the z direction

Rotation Matrix

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

13-no-rotationJPG

Therefore,

14-rotation-0-to-1JPG

Displacement Vector

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

15-just-displacement-in-zJPG

Here is the displacement vector:

16-here-is-the-displacement-vectorJPG

Example 4 – Two Degree of Freedom Robotic Arm

22-beam-mount-pointing-right

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.

13-add-y-axesJPG

Rotation Matrix

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

17-here-are-the-rotation-matricesJPG

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 x0, y0, and z0 direction.

13-add-y-axesJPG
18-displacement-0-to-1JPG

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

5-theta-1-90-degreesJPG
19-what-happens-theta-90JPG

You see that the displacement vector is different when θ1 = 90 degrees. When θ1 changes, the origin of frame 1 rotates around z0. 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 θ1. 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:

20-aerial-viewJPG

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

21-theta-45-degreesJPG

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

Using trigonometry, we know that:

  • cos(θ1) = (length of red dashed line) / a2   [displacement in the x0 direction]
  • sin(θ1) = (length of green dashed line) / a2   [displacement in the y0 direction]
  • a1 [displacement in the z0 direction]

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

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

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

22-our-displacement-vector-isJPG

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.

23-forget-frame-0JPG

Using trigonometry, we know that:

  • cos(θ2) = (length of green dashed line) / a4      [displacement in the x1 direction]
  • sin(θ2) = (length of red dashed line) / a4      [displacement in the y1 direction]
  • a3   [displacement in the z1 direction]
24-displacement-vector-1-to-2JPG

Example 5 – Cartesian Robot

15-example-of-cartesian-robot

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

13-cartesian-robotJPG

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 z0 axis. The value of this displacement is a1 + d1, where d1 represents the amount of extension of the joint when the cartesian robot is powered on. 

Therefore,

25-cartesian-displacement-0-to-1JPG

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

26-displacement-frame1-to-2JPG

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

27-origin-frame-2-to-3JPG

Example 6 – Articulated Robot

23-example-of-articulated-robot
28-articulated-robotJPG

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 θ1 changes.

Does it change? No. 

No matter what value θ1 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 z0 axis, therefore:

29-only-displacement-along-z0JPG

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 θ2 changes. 

Imagine θ2 = 45 degrees. You can see that the origin of frame 2 changed along the y1 axis as well as the x1 axis. There was no change along the z1 axis.

30-no-change-along-z1JPG
31-displacement-vector-1-to-2JPG

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

32-displacement-vector-2-to-3JPG

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.

How to Find the Rotation Matrices for Robotic Arms

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:

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 (x0, y0, and z0 axes in the image below) could be our global reference frame. 
  • We then have a local reference frame (x1, y1, z1) that is rotated at an angle from the global reference frame. 

After that, we have another local reference frame (x2, y2, z2) that is rotated relative to the local reference frame before it (x1, y1, z1).

1-2dof-robotic-armJPG

Imagine the base reference frame (x0, y0, z0) is a person. Our base frame might ask, ”Hey, I see the (x2, y2, z2) 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 (x2, y2, z2) frame to the (x1, y1, z1) frame using a rotation matrix. We would then convert the  (x1, y1, z1) frame to the (x0, y0, z0) 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 γ:

2-yaw

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, (x2, y2, z2) ) in terms of the base reference frame (x0, y0, and z0) 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).

painting-robot

Example 1 – Two Degree of Freedom Robotic Arm

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

1-2dof-robotic-armJPG

As θ1 changes, frame 1 will rotate around the z0 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.

2-rot-0-1JPG

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

3-rot-1-2JPG

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 (x1, y1, z1) frame to the base frame (x0, y0, z0). The matrix follows the format below where (xL, yL, zL) is  (x1, y1, z1) and  (xG, yG, zG) is  (x0, y0, z0). γ 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.

4-identity-matrixJPG

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

1-2dof-robotic-armJPG

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

5-theta-1-90-degreesJPG

Make sure:

  • servo_0_angle = 90
  • servo_1_angle = 0

Here is the output:

6-theta-0-90-degreesJPG

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 θ1 = 90 degrees

7-frame-0-to-frame-2JPG

The table above is another way to look at a rotation matrix. It shows the projection of the x2, y2, and z2 axes on the x0, y0, z0 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:

8-rot-m-to-nJPG

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

9-x2-on-y0JPG
5-theta-1-90-degreesJPG

We can see in the diagram above that x2 has the same orientation (i.e. pointing exact same direction) as y0. 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 y2 on x0 is -1. Does this make sense?

10-does-this-make-senseJPG

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

5-theta-1-90-degreesJPG

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

11-point-in-same-directionJPG

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?

12-12-63JPG

Example 2 – Cartesian Robot

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

13-cartesian-robotJPG

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.

13-cartesian-robotJPG

The first column represents the projection x1 on each of the axes of frame 0. We can see that x1 has the same orientation as y0. We put a 1 in that corresponding cell in the rotation matrix. We put a 0 in the other cells because x1 is perpendicular to x0 and z0. 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.

14-frame0-to-frame1JPG

Using the same logic above, we can fill out the second column of the table. y1 has the same orientation as z0.

15-fill-in-second-columnJPG

We then finish the third column of the rotation matrix.

16-third-columnJPG

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

17-joint-variables-all-0JPG

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:

13-cartesian-robotJPG
18-frame2-to-1JPG
19-rot-1-2JPG

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

20-identity-matrixJPG

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.

21-articulated-manipulatorJPG

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 x1 on each of the axes of frame 0. We can see that x1 has the same orientation as x0. We put a 1 in that corresponding cell in the rotation matrix. We put a 0 in the other cells because x1 is perpendicular to y0 and z0.

22-rot-frame-0-1JPG

Using the same logic above, we can fill out the second column of the table. y1 has the same orientation as z0.

23-has-same-orientationJPG

We then finish the third column of the rotation matrix.

24-third-columnJPG

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 θ1).

21-articulated-manipulatorJPG

Look at the diagram above. You can see that frame 1 can rotate in response to changes in θ1. When θ1 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):

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 z0 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 y1 axis of frame 1), you would place that after the matrix we found earlier (i.e. on the right). That would look like this:

25-rot-0-1JPG

Multiplying the matrices above, together, we get:

26-multiply-together-we-getJPG

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

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

21-articulated-manipulatorJPG

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.

27-identity-matrixJPG

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.

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

28-rot-1-to-2JPG
29-rot-1-to-2JPG

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

30-rot-2-to-3JPG

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.

6dof-diy-robotic-arm

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.

31-six-dof-robotJPG

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.

32-rot-0-1JPG

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 θ1. When θ1 changes, it causes rotation around the z0 axis of frame 0.

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

33-rot-from-0-to-1JPG

Multiplying the matrices above, together, we get:

34-rot-0-to-1JPG

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

31-six-dof-robotJPG

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.

θ2 is a rotation around the z1 axis. z1 and the z2 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.

35-rot-1-to-2JPG
36-rot-1-to-2JPG

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

37-rot-2-to-3JPG

Now, let’s find rot_mat_3_4.

31-six-dof-robotJPG
38-rot-3-to-4JPG

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 θ4. When θ4 changes, it causes rotation around the z3 axis of frame 3.

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

39-rot-matrix-3-to-4JPG

Now we multiply the matrices together:

40-rot-matrix-3-to-4JPG

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. 

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

41-rot-4-to-5JPG
42-rot-4-to-5JPG

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:

43-here-is-the-outputJPG

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

44-rot-5-to-0JPG

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

  1. Column 1 is saying that x5 and z0 point the same direction, and every other axis combination is perpendicular.
  2. Column 2 indicates that y5 and y0 point in the opposite direction, and every other axis combination is perpendicular.
  3. Column 3 indicates that z5 and x0 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.

31-six-dof-robotJPG

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.

How to Assign Denavit-Hartenberg Frames to Robotic Arms

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.

1-connected-to-first-jointJPG

Let’s draw the second z-axis.

2-second-jointJPG

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

3-add-z-vectorJPG

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

4-easier-to-see-on-diagramJPG

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

5-draw-x1JPG

Now, let’s draw x2.

6-draw-x2JPG

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 y0?

  • x0 points to the right (point the four fingers of your right hand in that direction).
  • z0 points toward the sky (point your thumb in that direction).
  • Therefore, y0 points into the page.
7-draw-y0JPG

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

8-using-right-hand-ruleJPG

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.

9-rule-holdsJPG

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.

10-two-degree-of-freedom-robotic-armJPG

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

11-draw-z-axesJPG

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

12-add-x-axesJPG

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

13-add-y-axesJPG

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.

14-confirm-they-intersectJPG

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:

16-first-jointJPG

Let’s add our second and third joint.

17-second-third-jointJPG

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

18-each-link-unextendedJPG

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

19-direction-of-motionJPG

Let’s draw the axes. 

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

20-add-z-axesJPG

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

21-add-x-axesJPG

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.

22-add-yJPG

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.

23-example-of-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.

24-3-dof-armJPG

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

25-letter-a-represent-link-lengthsJPG

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

26-add-z-axesJPG

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.

27-add-x-axesJPG

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.

28-add-y-axesJPG

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.

30-base-kinematic-diagram-scara-robotJPG

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.

31-add-z-axisJPG

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

32-add-x-axisJPG

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

33-add-y-axisJPG

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.

6dof-diy-robotic-arm

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.

34-six-dof-armJPG

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

35-add-angles-robotic-armJPG

Now, let’s go through the four rules.

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

36-add-z-axisJPG

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 a4 in order to satisfy Rule #2.

37-add-x-axisJPG

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.

38-add-y-axisJPG

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.

39-6dof-collaborative-robotJPG

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

40-kinematic-diagram-collaborative-robot-ur5JPG

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.

41-fanuc-lrmate-200idJPG

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

42-denavit-hartenberg-frames-fanuc-lrmate-200idJPG

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.