The inverse kinematics problem in robotics asks the following question: What do the angles of the servo motors need to be given our desired position and orientation of the end effector of a robotic arm (e.g. gripper, hand, vacuum suction cup, etc.)?

You can see how this problem has all sorts of real-world applications. For example, imagine we have a robotic arm that is inside a warehouse. We want the robotic arm to pick up items that are in one location and place them in another (i.e. pick and place).

This problem is exactly the inverse kinematics problem. We know where we want the robotic arm to go, and we need to solve for the joint variables (e.g. servo motor angles) so that we can command the robotic arm’s end effector to the correct location to pick up the items.

In this tutorial, I will cover one way to solve the inverse kinematics problem. It is called the graphical approach. The graphical approach depends on us using trigonometry to solve for the values of the joint variables given the desired position of the end effector. 

The graphical approach to inverse kinematics is useful for robotic arms with three degrees of freedom or less. Beyond three degrees of freedom, we need to use another approach, which I will cover in a future tutorial.

Example 1 – Two Degree of Freedom Robotic Arm: Revolute + Prismatic

Consider the following kinematic diagram of a two degree of freedom robotic arm. We have a revolute joint (i.e. regular servo motor with rotation motion) at the base of the arm. This revolute joint is connected to a prismatic joint (i.e. a linear actuator that produces linear motion).

For this robotic arm above, we want to be able to tell it where the end effector (frame 2 above…x₂, y₂, z₂ coordinate axes) should go. To accomplish this, we need to have trigonometric equations in place that enable us to solve for the joint variables (e.g. servo angle for Joint 1 and linear actuator displacement for Joint 2) given our desired end effector position.

Draw the Kinematic Diagram From Either an Aerial View or Side View

Let’s start by drawing the kinematic diagram from an aerial view. That is, we want to look down on the plane created by the x₀ and y₀ axes. 
We will let θ₁ = 45 degrees. This angle will make it easier for us to draw the triangles (i.e. trigonometry) we need to solve the inverse kinematics.

Let’s start by drawing the kinematic diagram from an aerial view. That is, we want to look down on the plane created by the x₀ and y₀ axes. 

We will let θ₁ = 45 degrees. This angle will make it easier for us to draw the triangles (i.e. trigonometry) we need to solve the inverse kinematics.

Sketch Triangles on Top of the Kinematic Diagram

The first triangle we will draw enables us to determine the position of the end effector (i.e. the origin of coordinate frame 2) relative to the position of the base frame (i.e. the origin of coordinate frame 0). 

The side of the triangle that is parallel to the vector y₀ will be labeled as y_0_2. This distance represents the y position of the end effector relative to the base frame of the robotic arm.

The side of the triangle that is parallel to the vector x₀ will be labeled as x_0_2. This distance represents the x position of the end effector relative to the base frame of the robotic arm.

The Three Useful Trigonometric Relationships

There are three trigonometric relationships that will help us solve the inverse kinematics for this robotic arm. Solving the inverse kinematics means finding the values for each joint variable.

1. Pythagorean Theorem: a² + b² = c²

2. SOHCAHTOA

This trigonometric relationship says that given a right triangle with angle A and sides a, b, and c:

• sin A = opposite/hypotenuse = a/c
• cos A = adjacent/hypotenuse = b/c
• tan A = opposite/adjacent = a/b

3. Law of Cosines

The law of cosines is a generalized form of the Pythagorean theorem. It works for right triangles as well as for non-right triangles. The three relationships are as follows

• a² = b² + c² – 2bccosα
• b² = a² + c² – 2accosβ
• c² = a² + b² – 2abcosγ

Solve for d₂

To solve for d₂, we need to select one of the three trigonometric relationships. Let’s use the Pythagorean theorem. We have:

(x_0_2)² + (y_0_2)² = (a₂ + a₃ + d₂)²

In the equation above, the only unknown value is d₂ because we know the link lengths (a₂ and a₃), and x_0_2 and y_0_2 represent the position of the end effector that we desire. The displacement of the linear actuator (d₂) is what we want to know. 

Let’s solve for d₂.

square_root((x_0_2)² + (y_0_2)²) = (a₂ + a₃ + d₂)

(a₂ + a₃ + d₂) = square_root((x_0_2)² + (y_0_2)²)

d₂ = square_root((x_0_2)² + (y_0_2)²) – a₂ – a₃

Making the above equation look prettier, we have:

Solve for θ₁

Now, let’s solve for the other unknown, the value of θ₁. Looking at my three useful trigonometric relationships, SOHCAHTOA looks like it will help us.

tan θ₁ = opposite/adjacent = y_0_2 / x_0_2

Therefore,

θ₁ = tan^-1(y_0_2 / x_0_2)

Making it look prettier, we have:

We are done because we have equations for both joint variables.

Example 2 – Two Degree of Freedom Robotic Arm: Revolute + Revolute

Let’s look at another example. We’ll work out the inverse kinematics for the two degree of freedom robotic arm that we built in a previous tutorial.

Draw the Kinematic Diagram From Either an Aerial View or Side View

Sketch Triangles on Top of the Kinematic Diagram

Let’s draw the first triangle in green.

Let’s zoom in and draw another triangle. We’ll make this one pink.

Let the long side of the triangle we just created be r₁.

Solve for the Unknown Variables

Solve for r₁

Let’s start by looking at the green right triangle above. To solve for r₁, we can use the Pythagorean Theorem.

r₁²  = (x_0_2)² + (y_0_2)² 

Take the square root of both sides. We’ll label this as equation 1…
(1) r₁  = square_root((x_0_2)² + (y_0_2)²)

Solve for θ₁

Now let’s look at the pink triangle. Because this isn’t a right triangle, we’ll have to use the law of cosines.

To use the law of cosines and to solve for θ₁, we need to label some angles.

I have added two new angles, ϕ₁ and ϕ₂. 

We can see from the diagram that:

θ₁ =  ϕ₂ – ϕ₁

Using the law of cosines, we have:

a₄² = r₁² + a₂² – 2r₁a₂cos(ϕ₁)

Now, solve for ϕ₁.  This will be equation #2.

(2) ϕ₁ = arccos((a₄² – r₁² – a₂²)/(-2r₁a₂))

Also, we can see from the diagram that…

(3) ϕ₂ = arctan((y_0_2) / (x_0_2)) 

Now that we have expressions for ϕ₁ and ϕ₂, we can solve for θ₁.

(4) θ₁ =  ϕ₂ – ϕ₁

Solve for θ₂

To solve for θ₂, we need to label yet another angle. I’ll label that obtuse angle in the pink triangle  ϕ₃.

Look at the diagram above. You can see that:

ϕ₃ + θ₂ = 180°

Therefore,

θ₂ = 180° – ϕ₃ 

To find the value of ϕ₃, we need to use the law of cosines.

r₁² = a₂² + a₄² – 2a₂a₄cos(ϕ₃)

Solving for ϕ₃, we get…

(5) ϕ₃ = arccos((r₁² – a₂² – a₄²)/(-2a₂a₄))

Now, we can solve for θ₂…

(6) θ₂ = 180° – ϕ₃ 

Put It All Together

Now, let’s gather all our useful equations together:

(1) r₁  = square_root((x_0_2)² + (y_0_2)²) 

(2) ϕ₁ = arccos((a₄² – r₁² – a₂²)/(-2r₁a₂))

(3) ϕ₂ = arctan((y_0_2) / (x_0_2)) 

(4) θ₁ =  ϕ₂ – ϕ₁

(5) ϕ₃ = arccos((r₁² – a₂² – a₄²)/(-2a₂a₄))

(6) θ₂ = 180° – ϕ₃ 

Implement Inverse Kinematics for a Real Robotic Arm

Now that we’ve derived all our equations, let’s see all this math in action on a real robot. We want to be able to specify an (x,y) coordinate where we want the end effector of the robot to go to. The robotic arm will then move to that location.

To complete this section, you need to have completed this tutorial.

When we created code for the forward kinematics of this robotic arm, we set both joint angles to 45 degrees. We then ran the code, and the end effector of the robotic arm moved to the (x = 4 cm, y = 10 cm) position on the dry erase board.

Since we are doing the inverse kinematics problem, we will now do everything in reverse. We will tell the robot to move the end effector to the (x = 4 cm, y = 10 cm) position on the dry erase board. The code we will write will automatically calculate the joint angles that will make that motion happen.

Let’s open a new sketch in the Arduino IDE. We’ll call this program ik_2dof_robotic_arm_v1.ino. 

Here is the code:

/*
Program: Inverse Kinematics on a Two Degree of Freedom Robotic Arm (Graphical Method)
File: ik_2dof_robotic_arm_v1.ino
Description: This program is an implementation of inverse kinematics for
  a two degree of freedom robotic arm. Given a desired x and y position
  for the end effector, the servo angles are calculated and then set.
Author: Addison Sears-Collins
Website: https://automaticaddison.com
Date: August 26, 2020
*/
 
#include <VarSpeedServo.h> 

// Define the number of servos
#define SERVOS 2

// Create the servo objects.
VarSpeedServo myservo[SERVOS]; 

// Speed of the servo motors
// Speed=1: Slowest
// Speed=255: Fastest.
const int desired_speed = 75;

// Attach servos to digital pins on the Arduino
int servo_pins[SERVOS] = {3,5};

// Desired x and y position of the end effector
double x_pos = 4.0;
double y_pos = 10.0;

// Link lengths in centimeters
const double a1 = 4.7;
const double a2 = 5.9;
const double a3 = 5.4;
const double a4 = 6.0;

// Define the inverse kinematics variables
double r1 = 0.0;
double phi_1 = 0.0;
double phi_2 = 0.0;
double phi_3 = 0.0;
double theta_1 = 0.0;
double theta_2 = 0.0;

void setup() {

  // Set the baud rate for the serial port at 9600 bps
  // Used for trouble shooting
  //Serial.begin(9600);
  
  // Attach the servos to the servo object 
  // attach(pin, min, max  ) - Attaches to a pin 
  //   setting min and max values in microseconds
  //   default min is 544, max is 2400  
  // Alter these numbers until both servos have a 
  //   180 degree range.
  myservo[0].attach(servo_pins[0], 544, 2475);  
  myservo[1].attach(servo_pins[1], 500, 2475); 

  // Set initial servo positions 
  myservo[0].write(0, desired_speed, true);  
  myservo[1].write(calc_servo_1_angle(0), desired_speed, true);

  // Wait one second to let servos get into position
  delay(1000);
}
 
void loop() {  

    /* Calculate the inverse kinematics*/
    // (1) r1  = square_root((x_0_2)^2 + (y_0_2)^2) 
    // Value is in centimeters
    r1 = sqrt((x_pos * x_pos) + (y_pos * y_pos));

    // (2) ϕ1 = arccos((a4^2 - r1^2 - a2^2)/(-2*r1*a2))
    // The returned value is in the range [0, pi] radians. 
    phi_1 = acos(((a4 * a4) - (r1 * r1) - (a2 * a2))/(-2.0 * r1 * a2));

    // (3) ϕ2 = arctan((y_0_2) / (x_0_2)) 
    // The atan2() function computes the principal value of the 
    // arc tangent of y/x, using the signs of both arguments to 
    // determine the quadrant of the return value. 
    // The returned value is in the range [-pi, +pi] radians.
    phi_2 = atan2(y_pos,x_pos);

    // (4) θ1 =  ϕ1 - ϕ2
    // Value is in radians
    theta_1 = phi_2 - phi_1;

    // (5) ϕ3 = arccos((r1^2 - a2^2 - a4^2)/(-2*a2*a4))
    // Value is in radians
    phi_3 = acos(((r1 * r1) - (a2 * a2) - (a4 * a4))/(-2.0 * a2 * a4));

    //(6) θ2 = 180° - ϕ3 
    theta_2 = PI - phi_3;
    
    /* Convert the joint (servo) angles from radians to degrees*/
    theta_1 = theta_1 * RAD_TO_DEG; // Joint 1
    theta_2 = theta_2 * RAD_TO_DEG; // Joint 2

    /* Move the end effector to the desired x and y position */
    // Set Joint 1 (Servo 0) angle
    myservo[0].write(theta_1, desired_speed, true); 

    // Set Joint 2 (Servo 1) angle
    myservo[1].write(calc_servo_1_angle(theta_2), desired_speed, true); 

    // Used for troubleshooting
    /*
    Serial.print("Theta 1: ");
    Serial.print(theta_1);
    Serial.println(" degrees");
    Serial.print("Theta 2: ");
    Serial.print(theta_2);
    Serial.println(" degrees");
    Serial.println();
    */
    
    // Wait half a second
    delay(500);
}     

/*  This method converts the desired angle for Servo 1 into a control angle
 *  for Servo 1. It assumes that the 0 degree position on the kinematic
 *  diagram for Servo 1 is actually 90 degrees on the actual servo.
 *  The angle range for Servo 1 on the kinematic diagram is
 *  -90 to 90 degrees, with 0 degrees being the center position. 
 *  The actual servo range for the physical motor
 *  is 0 to 180 degrees. We convert the desired angle 
 *  to a value within that range.
 */
int calc_servo_1_angle (int input_angle) {
  
  int result;

  result = map(input_angle, -90, 90, 0, 180);

  return result;
}  

Upload the code to the Arduino. Make sure your robotic arm is properly wired up and has power.

Now, run the code. You should see that the robotic arm starts at its home position with both joint angles at 0 degrees. The end effector then moves to (x = 4, y = 10) on the dry erase board. Pretty cool, huh!

You can try different values for x and y, but make sure those valuables are reachable by the robotic arm. If you input x and y values that are outside the robot’s workspace, your robot will not move at all. It will remain in the home position.

You notice in the code that we used the atan2 function and not atan. Both functions are used to calculate the arctangent. However, unlike atan, atan2 takes into account the signs of both x and y to determine which quadrant the end effector needs to go to. 

The returned value for atan is in the range [-90 degrees, 90 degrees], so it only works if your desired end effector coordinate is in Quadrants I or IV of a cartesian grid.

The returned value for atan2 is in the range [-180 degrees, +180 degrees]. It therefore works in all four quadrants of a grid. 

atan2 is not as limited as atan, making it useful in our case when we want to make x negative and y positive, for example (which would be in quadrant II of a cartesian grid).

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 previous tutorials, we used known joint variables (i.e. servo motor angles, displacement of a linear actuator, etc.) to calculate the position and orientation of the end effector of a robotic arm (e.g. robotic gripper, robotic hand, vacuum suction cup, etc.) in 3D space. This is called forward kinematics.

Forward kinematics asks the question: Where is the end effector of a robot (e.g. gripper, hand, vacuum suction cup, etc.) located in space given that we know the angles of the servo motors?

Inverse kinematics is the forward kinematics problem in reverse. We know the position and orientation we want the end effector of a robotic arm to have, and we want to find the values of the joint variables that generate that desired position and orientation of the end effector.

• Joint Variables —–> Pose of the End effector of a Robotic Arm = Forward Kinematics
• Pose of the End effector of a Robotic Arm —–> Joint Variables = Inverse Kinematics

Inverse kinematics asks the question: What do the angles of the servo motors need to be given our desired position and orientation of the end effector of a robotic arm (e.g. gripper, hand, vacuum suction cup, etc.)?

Cover Image Source: Wikipedia.

Now that we’ve seen how to fill in Denavit-Hartenberg Tables, let’s see how we can use these tables and Python code to calculate homogeneous transformation matrices for robotic arms. Our goal is to then use these homogeneous transformation matrices to determine the position and orientation of the end effector of a robotic arm.

Example 1 – Cartesian Robot

Let’s start by calculating the homogeneous transformation matrix from frame 0 to frame 1.

Here was our derivation of the Denavit-Hartenberg table for the cartesian robot. Let’s enter that one in code. Here is the program in Python:

import numpy as np # Scientific computing library

# Project: Coding Denavit-Hartenberg Tables Using Python - Cartesian Robot
#          This only looks at frame 0 to frame 1.
# Author: Addison Sears-Collins
# Date created: August 21, 2020

# Link lengths in centimeters
a1 = 1 # Length of link 1
a2 = 1 # Length of link 2
a3 = 1 # Length of link 3

# Initialize values for the displacements
d1 = 1 # Displacement of link 1
d2 = 1 # Displacement of link 2
d3 = 1 # Displacement of link 3

# Declare the Denavit-Hartenberg table. 
# It will have four columns, to represent:
# theta, alpha, r, and d
# We have the convert angles to radians.
d_h_table = np.array([[np.deg2rad(90), np.deg2rad(90), 0, a1 + d1],
                      [np.deg2rad(90), np.deg2rad(-90), 0, a2 + d2],
                      [0, 0, 0, a3 + d3]]) 

# Create the homogeneous transformation matrix
homgen_0_1 = np.array([[np.cos(d_h_table[0,0]), -np.sin(d_h_table[0,0]) * np.cos(d_h_table[0,1]), np.sin(d_h_table[0,0]) * np.sin(d_h_table[0,1]), d_h_table[0,2] * np.cos(d_h_table[0,0])],
                      [np.sin(d_h_table[0,0]), np.cos(d_h_table[0,0]) * np.cos(d_h_table[0,1]), -np.cos(d_h_table[0,0]) * np.sin(d_h_table[0,1]), d_h_table[0,2] * np.sin(d_h_table[0,0])],
                      [0, np.sin(d_h_table[0,1]), np.cos(d_h_table[0,1]), d_h_table[0,3]],
                      [0, 0, 0, 1]])  

# Print the homogeneous transformation matrix from frame 1 to frame 0.
print(homgen_0_1)

Run the program.

Here is the output:

The output is saying that the rotation matrix from frame 0 to frame 1 is as follows:

This is exactly what I got on an earlier post where I derived the rotation matrix from frame 0 to frame 1 for a cartesian robot.

You can also see the displacement vector from frame 1 to frame 0, the 3×1 vector on the right side of the matrix. The vector makes sense because it is saying that the position of frame 1 relative to frame 0 in the z₀ direction is 2 units (because a₁ = 1 and d₁ = 1):

Now, let’s add the two other homogeneous transformation matrices: from frame 1 to 2 and from frame 2 to 3.

Here is the code:

import numpy as np # Scientific computing library

# Project: Coding Denavit-Hartenberg Tables Using Python - Cartesian Robot
# Author: Addison Sears-Collins
# Date created: August 21, 2020

# Link lengths in centimeters
a1 = 1 # Length of link 1
a2 = 1 # Length of link 2
a3 = 1 # Length of link 3

# Initialize values for the displacements
d1 = 1 # Displacement of link 1
d2 = 1 # Displacement of link 2
d3 = 1 # Displacement of link 3

# Declare the Denavit-Hartenberg table. 
# It will have four columns, to represent:
# theta, alpha, r, and d
# We have the convert angles to radians.
d_h_table = np.array([[np.deg2rad(90), np.deg2rad(90), 0, a1 + d1],
                      [np.deg2rad(90), np.deg2rad(-90), 0, a2 + d2],
                      [0, 0, 0, a3 + d3]]) 

# Homogeneous transformation matrix from frame 0 to frame 1
i = 0
homgen_0_1 = np.array([[np.cos(d_h_table[i,0]), -np.sin(d_h_table[i,0]) * np.cos(d_h_table[i,1]), np.sin(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.cos(d_h_table[i,0])],
                      [np.sin(d_h_table[i,0]), np.cos(d_h_table[i,0]) * np.cos(d_h_table[i,1]), -np.cos(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.sin(d_h_table[i,0])],
                      [0, np.sin(d_h_table[i,1]), np.cos(d_h_table[i,1]), d_h_table[i,3]],
                      [0, 0, 0, 1]])  

# Homogeneous transformation matrix from frame 1 to frame 2
i = 1
homgen_1_2 = np.array([[np.cos(d_h_table[i,0]), -np.sin(d_h_table[i,0]) * np.cos(d_h_table[i,1]), np.sin(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.cos(d_h_table[i,0])],
                      [np.sin(d_h_table[i,0]), np.cos(d_h_table[i,0]) * np.cos(d_h_table[i,1]), -np.cos(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.sin(d_h_table[i,0])],
                      [0, np.sin(d_h_table[i,1]), np.cos(d_h_table[i,1]), d_h_table[i,3]],
                      [0, 0, 0, 1]])  

# Homogeneous transformation matrix from frame 2 to frame 3
i = 2
homgen_2_3 = np.array([[np.cos(d_h_table[i,0]), -np.sin(d_h_table[i,0]) * np.cos(d_h_table[i,1]), np.sin(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.cos(d_h_table[i,0])],
                      [np.sin(d_h_table[i,0]), np.cos(d_h_table[i,0]) * np.cos(d_h_table[i,1]), -np.cos(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.sin(d_h_table[i,0])],
                      [0, np.sin(d_h_table[i,1]), np.cos(d_h_table[i,1]), d_h_table[i,3]],
                      [0, 0, 0, 1]])  

homgen_0_3 = homgen_0_1 @ homgen_1_2 @ homgen_2_3

# Print the homogeneous transformation matrices
print("Homogeneous Matrix Frame 0 to Frame 1:")
print(homgen_0_1)
print()
print("Homogeneous Matrix Frame 1 to Frame 2:")
print(homgen_1_2)
print()
print("Homogeneous Matrix Frame 2 to Frame 3:")
print(homgen_2_3)
print()
print("Homogeneous Matrix Frame 0 to Frame 3:")
print(homgen_0_3)
print()

Now, run the code.

Here is the output:

The homogeneous matrix from frame 0 to frame 3 is:

Let’s pull out the rotation matrix:

Let’s go through the matrix to see if it makes sense. Here is the diagram:

In the first column of the matrix, we see that the value on the third row is 1. It is saying that the x₃ vector is in the same direction as z₀. If we look at the diagram, we can see that this is in fact the case.

In the second column of the matrix, we see that the value on the first row is -1. It is saying that the y₃ vector is in the opposite direction as x₀. If we look at the diagram, we can see that this is in fact the case.

In the third column of the matrix, we see that the value on the second row is -1. It is saying that the z₃ vector is in the opposite direction as y₀. If we look at the diagram, we can see that this is in fact the case.

Finally, in the fourth column of the matrix, we have the displacement vector. Let’s pull that out.

The displacement vector portion of the matrix says that the position of the end effector of the robot is 2 units along x₀, -2 units along y₀, and 2 units along z₀. These results make sense given the link lengths and displacements were 1.

Example 2 – Six Degree of Freedom Robotic Arm

Let’s take a look at another example, a six degree of freedom robotic arm.

Here is the code:

import numpy as np # Scientific computing library

# Project: Coding Denavit-Hartenberg Tables Using Python - 6DOF Robotic Arm
#          This code excludes the servo motor that controls the gripper.
# Author: Addison Sears-Collins
# Date created: August 22, 2020

# Link lengths in centimeters
a1 = 1 # Length of link 1
a2 = 1 # Length of link 2
a3 = 1 # Length of link 3
a4 = 1 # Length of link 4
a5 = 1 # Length of link 5
a6 = 1 # Length of link 6

# Initialize values for the joint angles (degrees)
theta_1 = 0 # Joint 1
theta_2 = 0 # Joint 2
theta_3 = 0 # Joint 3
theta_4 = 0 # Joint 4
theta_5 = 0 # Joint 5

# Declare the Denavit-Hartenberg table. 
# It will have four columns, to represent:
# theta, alpha, r, and d
# We have the convert angles to radians.
d_h_table = np.array([[np.deg2rad(theta_1), np.deg2rad(90), 0, a1],
                      [np.deg2rad(theta_2), 0, a2, 0],
                      [np.deg2rad(theta_3), 0, a3, 0],
                      [np.deg2rad(theta_4 + 90), np.deg2rad(90), a5,0],
                      [np.deg2rad(theta_5), 0, 0, a4 + a6]]) 

# Homogeneous transformation matrix from frame 0 to frame 1
i = 0
homgen_0_1 = np.array([[np.cos(d_h_table[i,0]), -np.sin(d_h_table[i,0]) * np.cos(d_h_table[i,1]), np.sin(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.cos(d_h_table[i,0])],
                      [np.sin(d_h_table[i,0]), np.cos(d_h_table[i,0]) * np.cos(d_h_table[i,1]), -np.cos(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.sin(d_h_table[i,0])],
                      [0, np.sin(d_h_table[i,1]), np.cos(d_h_table[i,1]), d_h_table[i,3]],
                      [0, 0, 0, 1]])  

# Homogeneous transformation matrix from frame 1 to frame 2
i = 1
homgen_1_2 = np.array([[np.cos(d_h_table[i,0]), -np.sin(d_h_table[i,0]) * np.cos(d_h_table[i,1]), np.sin(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.cos(d_h_table[i,0])],
                      [np.sin(d_h_table[i,0]), np.cos(d_h_table[i,0]) * np.cos(d_h_table[i,1]), -np.cos(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.sin(d_h_table[i,0])],
                      [0, np.sin(d_h_table[i,1]), np.cos(d_h_table[i,1]), d_h_table[i,3]],
                      [0, 0, 0, 1]])  

# Homogeneous transformation matrix from frame 2 to frame 3
i = 2
homgen_2_3 = np.array([[np.cos(d_h_table[i,0]), -np.sin(d_h_table[i,0]) * np.cos(d_h_table[i,1]), np.sin(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.cos(d_h_table[i,0])],
                      [np.sin(d_h_table[i,0]), np.cos(d_h_table[i,0]) * np.cos(d_h_table[i,1]), -np.cos(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.sin(d_h_table[i,0])],
                      [0, np.sin(d_h_table[i,1]), np.cos(d_h_table[i,1]), d_h_table[i,3]],
                      [0, 0, 0, 1]])  

# Homogeneous transformation matrix from frame 3 to frame 4
i = 3
homgen_3_4 = np.array([[np.cos(d_h_table[i,0]), -np.sin(d_h_table[i,0]) * np.cos(d_h_table[i,1]), np.sin(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.cos(d_h_table[i,0])],
                      [np.sin(d_h_table[i,0]), np.cos(d_h_table[i,0]) * np.cos(d_h_table[i,1]), -np.cos(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.sin(d_h_table[i,0])],
                      [0, np.sin(d_h_table[i,1]), np.cos(d_h_table[i,1]), d_h_table[i,3]],
                      [0, 0, 0, 1]])  

# Homogeneous transformation matrix from frame 4 to frame 5
i = 4
homgen_4_5 = np.array([[np.cos(d_h_table[i,0]), -np.sin(d_h_table[i,0]) * np.cos(d_h_table[i,1]), np.sin(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.cos(d_h_table[i,0])],
                      [np.sin(d_h_table[i,0]), np.cos(d_h_table[i,0]) * np.cos(d_h_table[i,1]), -np.cos(d_h_table[i,0]) * np.sin(d_h_table[i,1]), d_h_table[i,2] * np.sin(d_h_table[i,0])],
                      [0, np.sin(d_h_table[i,1]), np.cos(d_h_table[i,1]), d_h_table[i,3]],
                      [0, 0, 0, 1]])  

homgen_0_5 = homgen_0_1 @ homgen_1_2 @ homgen_2_3 @ homgen_3_4 @ homgen_4_5 

# Print the homogeneous transformation matrices
print("Homogeneous Matrix Frame 0 to Frame 5:")
print(homgen_0_5)

Now, run the code:

Here is the output:

In matrix form, this is:

Does this make sense?  Here is the diagram:

In the first column of the matrix, we see that the value on the third row is 1. It is saying that the x₅ vector is in the same direction as z₀. If we look at the diagram, we can see that this is in fact the case.

In the second column of the matrix, we see that the value on the second row is -1. It is saying that the y₅ vector is in the opposite direction as y₀. If we look at the diagram, we can see that this is in fact the case.

In the third column of the matrix, we see that the value on the first row is 1. It is saying that the z₅ vector is in the same direction as x₀. If we look at the diagram, we can see that this is in fact the case.

Finally, in the fourth column of the matrix, we have the displacement vector. This portion of the matrix says that the position of the end effector of the robot is 4 units along x₀, 0 units along y₀, and 2 units along z₀. These results make sense given that we set all the link lengths to 1.

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.