The Ultimate Guide to Inverse Kinematics for 6DOF Robot Arms

Inverse kinematics is about calculating the angles of joints (i.e. angles of the servo motors on a robotic arm) that will cause the end effector of a robotic arm (e.g. robotics gripper, hand, vacuum suction cup, etc.) to reach some desired position (x, y, z) in 3D space.

sewbo-robotic-arm — Sewbo robotic arm in action. Sewbo Inc. is a startup which is developing automation solutions for the clothing manufacturing industry.

In this tutorial, we will learn about how to perform inverse kinematics for a six degree of freedom robotic arm. We will build from the work we did on this post where we used the graphical approach to inverse kinematics for a two degree of freedom SCARA-like robotic arm. This approach is also known as the analytical approach. It involves a lot of trigonometry and is fine for a robotic arm with three joints or less.

However, when we have a robotic arm with more than three degrees of freedom, we have to modify how we solve the inverse kinematics.

In this tutorial, we’ll take a look at two approaches: an analytical approach and a numerical approach. We’ll then code these up in Python so that you can see how the calculations are done in actual code.

Real-World Applications

This post contains a list of a lot of the applications of six degree of freedom robotic arms.

Prerequisites

Analytical Approach vs Numerical Approach to Inverse Kinematics

The analytical approach to inverse kinematics involves a lot of matrix algebra and trigonometry.

The advantage of this approach is that once you’ve drawn the kinematic diagram and derived the equations, computation is fast (compared to the numerical approach, which is iterative). You don’t have to make initial guesses for the joint angles like you do in the numerical approach (we’ll look at this approach later in this tutorial).

The disadvantage of the analytical approach is that the kinematic diagram and trigonometric equations are tedious to derive. Also, the solutions from one robotic arm don’t generalize to other robotic arms. You have to derive new equations for each new robotic arm you work with that has a different kinematic structure.

Analytical Approach to Inverse Kinematics

In this section, we’ll explore one analytical approach to inverse kinematics (i.e. there are many approaches).

Assumptions

In this approach, we first need to start off by making some assumptions.

We have to assume that:

The first three joints of the robotic arm are the only ones that determine the position of the end effector.
The last three joints (and any other joints after that) determine the orientation of the end effector.

Overall Steps

Here are the steps for calculating inverse kinematics for a six degree of freedom robotic arm.

Step 1: Draw the kinematic diagram of just the first three joints, and perform inverse kinematics using the graphical approach.

Step 2: Compute the forward kinematics on the first three joints to get the rotation of joint 3 relative to the global (i.e. base) coordinate frame. The outcome of this step will yield a matrix rot_mat_0_3, which means the rotation of frame 3 relative to frame 0 (i.e. the global coordinate frame).

Step 3: Calculate the inverse of rot_mat_0_3.

Step 4: Compute the forward kinematics on the last three joints, and extract the part that governs the rotation. This rotation matrix will be denoted as rot_mat_3_6.

Step 5: Determine the rotation matrix from frame 0 to 6 (i.e. rot_mat_0_6).

Step 6: Taking our desired x, y, and z coordinates as input, use the inverse kinematics equations from Step 1 to calculate the angles for the first three joints.

Step 7: Given the joint angles from Step 6, use the rotation matrix to calculate the values for the last three joints of the robotic arm.

Let’s run through an example.

Draw the Kinematic Diagram of Just the First Three Joints and Do Inverse Kinematics

First, let’s draw the kinematic diagram for our entire robot. If you need a refresher on how to draw kinematic diagrams, check out this tutorial.

Our robotic arm will have a cylindrical-style base (i.e. range of motion resembles a cylinder) and a spherical wrist (i.e. range of motion resembles a sphere). Here is its kinematic diagram:

Here is the kinematic diagram of just the first three joints:

Let’s do inverse kinematics for the diagram above using the graphical approach to inverse kinematics. We first need to draw the aerial view of the diagram above.

From the aerial view, we can see that we have two equations that come out of that.

θ₂ = tan^-1(y/x)
d₃ = sqrt(x² + y²) – a₃ – a₄

Now, let’s draw the side view of the robotic arm (sorry for the tiny print).

We can see from the above image that:

d₁ = z – a₁ – a₂

Calculate rot_mat_0_3

For this step, you can either use this method or the Denavit-Hartenberg method to calculate the rotation matrix of frame 3 relative to frame 0. Either of those methods will yield the following rotation matrix:

Calculate the Inverse of rot_mat_0_3

Now that we know rot_mat_0_3 (which we defined in the previous section), we need to take its inverse. The reason for this is due to the following expression:

We can left-multiply the inverse of the rotation matrix we found in the previous section to solve for rot_mat_3_6.

Calculate rot_mat_3_6

To calculate the rotation of frame 6 relative to frame 3, we need to go back to the kinematic diagram we drew earlier.

Using either the rotation matrix method or Denavit-Hartenberg, here is the rotation matrix you get when you consider just the frames from 3 to 6.

Determine rot_mat_0_6

Now, let’s determine the rotation matrix of frame 6 relative to frame 0, the global coordinate frame.

In this part, we need to determine what we want the orientation of the end effector to be so that we can define the rotation matrix of frame 6 relative to frame 0. We can choose any rotation matrix as long as it is a valid rotation matrix (I’ll explain what “valid” means below).

Imagine you want the end effector to point upwards towards the sky. In this case, z₆will point in the same direction as z₀. Accordingly, if you can imagine z₆ pointing straight upwards, x₆ would be oriented in the opposite direction as x₀, and y₆ would be oriented in the opposite direction as y₀. Our rotation matrix is thus:

The rotation matrix above is valid. The reason it is valid is because the length (also known as “norm” or “magnitude”) of each row and each column is 1. The length of a column or row is the square root of the sum of the squared elements.

For example, looking at column 1, we have:

Write Python Code

Open up your favorite Python IDE or wherever you like to write Python code.

Create up a new Python script. Call it inverse_kinematics_6dof_v1.py.

We want to set a desired position and orientation (relative to the base frame) for the end effector of the robotic arm and then have the program calculate the servo angles necessary to move the end effector to that position and orientation.

Write the following code. These expressions and matrices below (that we derived earlier) will be useful to you as you go through the code. Don’t be intimidated at how long the code is. Just copy and paste it into your favorite Python IDE or text editor, and read through it one line at a time.

#######################################################################################
# Progam: Inverse Kinematics for a 6DOF Robotic Arm Using an Analytical Approach
# Description: Given a desired end position (x, y, z) of the end effector of a robot, 
#   and a desired orientation of the end effector (relative to the base frame),
#   calculate the joint angles (i.e. angles for the servo motors).
# Author: Addison Sears-Collins
# Website: https://automaticaddison.com
# Date: October 19, 2020
# Reference: Sodemann, Dr. Angela 2020, RoboGrok, accessed 14 October 2020, <http://robogrok.com/>
#######################################################################################

import numpy as np # Scientific computing library 

# Define the desired position of the end effector
# This is the target (goal) location.
x = 4.0
y = 2.0

# Calculate the angle of the second joint
theta_2 = np.arctan2(y,x)
print(f'Theta 2 = {theta_2} radians\n')

# Define the desired orientation of the end effector relative to the base frame 
# (i.e. global frame)
# This is the target orientation.
# The 3x3 rotation matrix of frame 6 relative to frame 0
rot_mat_0_6 = np.array([[-1.0, 0.0, 0.0],
                        [0.0, -1.0, 0.0],
                        [0.0, 0.0, 1.0]])

# The 3x3 rotation matrix of frame 3 relative to frame 0
rot_mat_0_3 = np.array([[-np.sin(theta_2), 0.0, np.cos(theta_2)],
                        [np.cos(theta_2), 0.0, np.sin(theta_2)],
                        [0.0, 1.0, 0.0]])

# Calculate the inverse rotation matrix
inv_rot_mat_0_3 = np.linalg.inv(rot_mat_0_3)

# Calculate the 3x3 rotation matrix of frame 6 relative to frame 3
rot_mat_3_6 = inv_rot_mat_0_3 @ rot_mat_0_6
print(f'rot_mat_3_6 = {rot_mat_3_6}')

# We know the equation for rot_mat_3_6 from our pencil and paper
# analysis. The simplest term in that matrix is in the third column,
# third row. The value there in variable terms is cos(theta_5).
# From the printing above, we know the value there. Therefore, 
# cos(theta_5) = value in the third row, third column of rot_mat_3_6, which means...
# theta_5 = arccosine(value in the third row, third column of rot_mat_3_6)
theta_5 = np.arccos(rot_mat_3_6[2, 2])
print(f'\nTheta 5 = {theta_5} radians')

# Calculate the value for theta_6
# We'll use the expression in the third row, first column of rot_mat_3_6.
# -sin(theta_5)cos(theta_6) = rot_mat_3_6[2,0]
# Solving for theta_6...
# rot_mat_3_6[2,0]/-sin(theta_5) = cos(theta_6)
# arccosine(rot_mat_3_6[2,0]/-sin(theta_5)) = theta_6
theta_6 = np.arccos(rot_mat_3_6[2, 0] / -np.sin(theta_5))
print(f'\nTheta 6 = {theta_6} radians')

# Calculate the value for theta_4 using one of the other
# cells in rot_mat_3_6. We'll use the second row, third column.
# cos(theta_4)sin(theta_5) = rot_mat_3_6[1,2]
# cos(theta_4) = rot_mat_3_6[1,2] / sin(theta_5)
# theta_4 = arccosine(rot_mat_3_6[1,2] / sin(theta_5))
theta_4 = np.arccos(rot_mat_3_6[1,2] / np.sin(theta_5))
print(f'\nTheta 4 = {theta_4} radians')

# Check that the angles we calculated result in a valid rotation matrix
r11 = -np.sin(theta_4) * np.cos(theta_5) * np.cos(theta_6) - np.cos(theta_4) * np.sin(theta_6)
r12 = np.sin(theta_4) * np.cos(theta_5) * np.sin(theta_6) - np.cos(theta_4) * np.cos(theta_6)
r13 = -np.sin(theta_4) * np.sin(theta_5)
r21 = np.cos(theta_4) * np.cos(theta_5) * np.cos(theta_6) - np.sin(theta_4) * np.sin(theta_6)
r22 = -np.cos(theta_4) * np.cos(theta_5) * np.sin(theta_6) - np.sin(theta_4) * np.cos(theta_6)
r23 = np.cos(theta_4) * np.sin(theta_5)
r31 = -np.sin(theta_5) * np.cos(theta_6)
r32 = np.sin(theta_5) * np.sin(theta_6)
r33 = np.cos(theta_5)

check_rot_mat_3_6 = np.array([[r11, r12, r13],
                              [r21, r22, r23],
                              [r31, r32, r33]])

# Original matrix
print(f'\nrot_mat_3_6 = {rot_mat_3_6}')

# Check Matrix
print(f'\ncheck_rot_mat_3_6 = {check_rot_mat_3_6}\n')

# Return if Original Matrix == Check Matrix
rot_minus_check_3_6 = rot_mat_3_6.round() - check_rot_mat_3_6.round()
zero_matrix = np.array([[0, 0, 0],
                        [0, 0, 0],
                        [0, 0, 0]])
matrices_are_equal = np.array_equal(rot_minus_check_3_6, zero_matrix)

# Determine if the solution is valid or not
# If the solution is valid, that means the end effector of the robotic 
# arm can reach that target location.
if (matrices_are_equal):
  valid_matrix = "Yes"
else:
  valid_matrix = "No"  
print(f'Is this solution valid?\n{valid_matrix}\n')

Here is the output:

References

Sodemann, Dr. Angela 2020, RoboGrok, accessed 14 October 2020, <http://robogrok.com/>

Inverse Kinematics Using the Pseudoinverse Jacobian Method (Numerical Approach)

Let’s take a look at a numerical approach to inverse kinematics. There are a number of approaches, but in this section will explore one that is quite popular in industry and academia: the Pseudoinverse Jacobian Method.

This method is called “numerical” because it involves iteration to calculate the joint angles from the desired end effector position.

We’ll take a look at a SCARA-like robotic arm below that has 2 degrees of freedom (two joints/motors and 4 links).

To extend this example to a 6DOF robotic arm, just add 4 more joints and the appropriate number of links. Make sure before you do this step, that you’ve drawn the kinematic diagram. On this page, I have several examples of kinematic diagrams for 6DOF robotic arms.

Full Code in Python

Here is the full code. I named the program inv_kinematics_using_pseudo_jacobian.py. I’ll explain each piece step-by-step below.

#######################################################################################
# Progam: Inverse Kinematics for a Robotic Arm Using the Pseudoinverse of the Jacobian
# Description: Given a desired end position (x, y, z) of the end effector of a robot, 
#   calculate the joint angles (i.e. angles for the servo motors).
# Author: Addison Sears-Collins
# Website: https://automaticaddison.com
# Date: October 15, 2020
#######################################################################################

import numpy as np # Scientific computing library 

def axis_angle_rot_matrix(k,q):
    """
    Creates a 3x3 rotation matrix in 3D space from an axis and an angle.

    Input
    :param k: A 3 element array containing the unit axis to rotate around (kx,ky,kz) 
    :param q: The angle (in radians) to rotate by

    Output
    :return: A 3x3 element matix containing the rotation matrix
    
    """
    
    #15 pts 
    c_theta = np.cos(q)
    s_theta = np.sin(q)
    v_theta = 1 - np.cos(q)
    kx = k[0]
    ky = k[1]
    kz = k[2]	
	
    # First row of the rotation matrix
    r00 = kx * kx * v_theta + c_theta
    r01 = kx * ky * v_theta - kz * s_theta
    r02 = kx * kz * v_theta + ky * s_theta
    
    # Second row of the rotation matrix
    r10 = kx * ky * v_theta + kz * s_theta
    r11 = ky * ky * v_theta + c_theta
    r12 = ky * kz * v_theta - kx * s_theta
	
    # Third row of the rotation matrix
    r20 = kx * kz * v_theta - ky * s_theta
    r21 = ky * kz * v_theta + kx * s_theta
    r22 = kz * kz * v_theta + c_theta
	
    # 3x3 rotation matrix
    rot_matrix = np.array([[r00, r01, r02],
                           [r10, r11, r12],
                           [r20, r21, r22]])
						   
    return rot_matrix

def hr_matrix(k,t,q):
    '''
    Create the Homogenous Representation matrix that transforms a point from Frame B to Frame A.
    Using the axis-angle representation
    Input
    :param k: A 3 element array containing the unit axis to rotate around (kx,ky,kz) 
    :param t: The translation from the current frame (e.g. Frame A) to the next frame (e.g. Frame B)
    :param q: The rotation angle (i.e. joint angle)

    Output
    :return: A 4x4 Homogenous representation matrix
    '''
    # Calculate the rotation matrix (angle-axis representation)
    rot_matrix_A_B = axis_angle_rot_matrix(k,q)
	
    # Store the translation vector t
    translation_vec_A_B = t

    # Convert to a 2D matrix
    t0 = translation_vec_A_B[0]
    t1 = translation_vec_A_B[1]
    t2 = translation_vec_A_B[2]
    translation_vec_A_B = np.array([[t0],
                                    [t1],
                                    [t2]])
									
    # Create the homogeneous transformation matrix
    homgen_mat = np.concatenate((rot_matrix_A_B, translation_vec_A_B), axis=1) # side by side

    # Row vector for bottom of homogeneous transformation matrix
    extra_row_homgen = np.array([[0, 0, 0, 1]])

    # Add extra row to homogeneous transformation matrix	
    homgen_mat = np.concatenate((homgen_mat, extra_row_homgen), axis=0) # one above the other
 	    
    return homgen_mat

class RoboticArm:
    def __init__(self,k_arm,t_arm):
        '''
        Creates a robotic arm class for computing position and velocity.

        Input
        :param k_arm: A 2D array that lists the different axes of rotation (rows) for each joint.
        :param t_arm: A 2D array that lists the translations from the previous joint to the current joint
	                  The first translation is from the global (base) frame to joint 1 (which is often equal to the global frame)
	                  The second translation is from joint 1 to joint 2, etc.
        '''
        self.k = np.array(k_arm)
        self.t = np.array(t_arm)
        assert k_arm.shape == t_arm.shape, 'Warning! Improper definition of rotation axes and translations'
        self.N_joints = k_arm.shape[0]

    def position(self,Q,index=-1,p_i=[0,0,0]):
        '''
        Compute the position in the global (base) frame of a point given in a joint frame
        (default values will assume the input position vector is in the frame of the last joint)
        Input
        :param p_i: A 3 element vector containing a position in the frame of the index joint
        :param index: The index of the joint frame being converted from (first joint is 0, the last joint is N_joints - 1)

        Output
        :return: A 3 element vector containing the new position with respect to the global (base) frame
        '''
        # The position of this joint described by the index
        p_i_x = p_i[0]
        p_i_y = p_i[1]
        p_i_z = p_i[2]
        this_joint_position = np.array([[p_i_x],
                                        [p_i_y],
                                        [p_i_z],
									    [1]])

        # End effector joint
        if (index == -1):
          index = self.N_joints - 1
		
        # Store the original index of this joint		
        orig_joint_index = index

        # Store the result of matrix multiplication
        running_multiplication = None
		
        # Start from the index of this joint and work backwards to index 0
        while (index >= 0):
		  
          # If we are at the original joint index
          if (index == orig_joint_index):
            running_multiplication = hr_matrix(self.k[index],self.t[index],Q[index]) @ this_joint_position
          # If we are not at the original joint index
          else:	
            running_multiplication = hr_matrix(self.k[index],self.t[index],Q[index]) @ running_multiplication
		  
          index = index - 1
		
        # extract the points
        px = running_multiplication[0][0]
        py = running_multiplication[1][0]
        pz = running_multiplication[2][0]		
        
        position_global_frame = np.array([px, py, pz])
		
        return position_global_frame

    def pseudo_inverse(self,theta_start,p_eff_N,goal_position,max_steps=np.inf):
        '''
        Performs the inverse kinematics using the pseudoinverse of the Jacobian

        :param theta_start: An N element array containing the current joint angles in radians (e.g. np.array([np.pi/8,np.pi/4,np.pi/6]))
	    :param p_eff_N: A 3 element vector containing translation from the last joint to the end effector in the last joints frame of reference
        :param goal_position: A 3 element vector containing the desired end position for the end effector in the global (base) frame
        :param max_steps: (Optional) Maximum number of iterations to compute 

        Output
        :return: An N element vector containing the joint angles that result in the end effector reaching xend (i.e. the goal)
        '''
        v_step_size = 0.05
        theta_max_step = 0.2
        Q_j = theta_start # Array containing the starting joint angles
        p_end = np.array([goal_position[0], goal_position[1], goal_position[2]]) # desired x, y, z coordinate of the end effector in the base frame
        p_j = self.position(Q_j,p_i=p_eff_N)  # x, y, z coordinate of the position of the end effector in the global reference frame
        delta_p = p_end - p_j  # delta_x, delta_y, delta_z between start position and desired final position of end effector
        j = 0 # Initialize the counter variable
        
        # While the magnitude of the delta_p vector is greater than 0.01 
        # and we are less than the max number of steps
        while np.linalg.norm(delta_p) > 0.01 and j<max_steps:
            print(f'j{j}: Q[{Q_j}] , P[{p_j}]') # Print the current joint angles and position of the end effector in the global frame
            
            # Reduce the delta_p 3-element delta_p vector by some scaling factor 
            # delta_p represents the distance between where the end effector is now and our goal position.			
            v_p = delta_p * v_step_size / np.linalg.norm(delta_p) 

            # Get the jacobian matrix given the current joint angles
            J_j = self.jacobian(Q_j,p_eff_N)
             
            # Calculate the pseudo-inverse of the Jacobian matrix
            J_invj = np.linalg.pinv(J_j)
			
            # Multiply the two matrices together
            v_Q = np.matmul(J_invj,v_p)

            # Move the joints to new angles
            # We use the np.clip method here so that the joint doesn't move too much. We
            # just want the joints to move a tiny amount at each time step because 
            # the full motion of the end effector is nonlinear, and we're approximating the
            # big nonlinear motion of the end effector as a bunch of tiny linear motions.
            Q_j = Q_j + np.clip(v_Q,-1*theta_max_step,theta_max_step)#[:self.N_joints]

            # Get the current position of the end-effector in the global frame
            p_j = self.position(Q_j,p_i=p_eff_N)

            # Increment the time step			
            j = j + 1

            # Determine the difference between the new position and the desired end position
            delta_p = p_end - p_j

        # Return the final angles for each joint
        return Q_j


    def jacobian(self,Q,p_eff_N=[0,0,0]):
        '''
        Computes the Jacobian (just the position, not the orientation)

        :param Q: An N element array containing the current joint angles in radians
        :param p_eff_N: A 3 element vector containing translation from the last joint to the end effector in the last joints frame of reference

        Output
        :return: A 3xN 2D matrix containing the Jacobian matrix
        '''
        # Position of the end effector in global frame
        p_eff = self.position(Q,-1,p_eff_N)
        
        first_iter = True
		
        jacobian_matrix = None
		
        for i in range(0, self.N_joints):
          if (first_iter == True):

            # Difference in the position of the end effector in the global frame
            # and this joint in the global frame
            p_eff_minus_this_p = p_eff - self.position(Q,index=i)
			
            # Axes
            kx = self.k[i][0]
            ky = self.k[i][1]
            kz = self.k[i][2]
            k = np.array([kx, ky, kz])
			
            px = p_eff_minus_this_p[0]
            py = p_eff_minus_this_p[1]
            pz = p_eff_minus_this_p[2]
            p_eff_minus_this_p = np.array([px, py, pz])
			
            this_jacobian = np.cross(k, p_eff_minus_this_p)	
 
            # Convert to a 2D matrix
            j0 = this_jacobian[0]
            j1 = this_jacobian[1]
            j2 = this_jacobian[2]
            this_jacobian = np.array([[j0],
                                      [j1],
                                      [j2]])			
            jacobian_matrix = this_jacobian
            first_iter = False
          else:
            p_eff_minus_this_p = p_eff - self.position(Q,index=i)
			
            # Axes
            kx = self.k[i][0]
            ky = self.k[i][1]
            kz = self.k[i][2]
            k = np.array([kx, ky, kz])
			
            # Difference between this joint's position and end effector's position
            px = p_eff_minus_this_p[0]
            py = p_eff_minus_this_p[1]
            pz = p_eff_minus_this_p[2]
            p_eff_minus_this_p = np.array([px, py, pz])
			
            this_jacobian = np.cross(k, p_eff_minus_this_p)	

            # Convert to a 2D matrix
            j0 = this_jacobian[0]
            j1 = this_jacobian[1]
            j2 = this_jacobian[2]
            this_jacobian = np.array([[j0],
                                      [j1],
                                      [j2]])			
            jacobian_matrix = np.concatenate((jacobian_matrix, this_jacobian), axis=1) # side by side  		    

        return jacobian_matrix

def main():
  '''Given a two degree of freedom robotic arm and a desired end position of the end effector,
     calculate the two joint angles (i.e. servo angles).
  '''

  # A 2D array that lists the different axes of rotation (rows) for each joint
  # Here I assume our robotic arm has two joints, but you can add more if you like.
  # k = kx, ky, kz 
  k = np.array([[0,0,1],[0,0,1]])
	
  # A 2D array that lists the translations from the previous joint to the current joint
  # The first translation is from the base frame to joint 1 (which is equal to the base frame)
  # The second translation is from joint 1 to joint 2
  # t = tx, ty, tz
  # These values are measured with a ruler based on the kinematic diagram
  # This tutorial teaches you how to draw kinematic diagrams:
  # https://automaticaddison.com/how-to-assign-denavit-hartenberg-frames-to-robotic-arms/
  a1 = 4.7
  a2 = 5.9
  a3 = 5.4
  a4 = 6.0
  t = np.array([[0,0,0],[a2,0,a1]])

  # Position of end effector in joint 2 (i.e. the last joint) frame
  p_eff_2 = [a4,0,a3]
	
  # Create an object of the RoboticArm class
  k_c = RoboticArm(k,t)

  # Starting joint angles in radians (joint 1, joint 2)
  q_0 = np.array([0,0])
	
  # desired end position for the end effector with respect to the base frame of the robotic arm
  endeffector_goal_position = np.array([4.0,10.0,a1 + a4])

  # Display the starting position of each joint in the global frame
  for i in np.arange(0,k_c.N_joints):
    print(f'joint {i} position = {k_c.position(q_0,index=i)}')

  print(f'end_effector = {k_c.position(q_0,index=-1,p_i=p_eff_2)}')
  print(f'goal = {endeffector_goal_position}') 

  # Return joint angles that result in the end effector reaching endeffector_goal_position
  final_q = k_c.pseudo_inverse(q_0, p_eff_N=p_eff_2, goal_position=endeffector_goal_position, max_steps=500)
	
  # Final Joint Angles in degrees	
  print('\n\nFinal Joint Angles in Degrees')
  print(f'Joint 1: {np.degrees(final_q[0])} , Joint 2: {np.degrees(final_q[1])}')

if __name__ == '__main__':
  main()

Here is the output:

axis_angle_rot_matrix(k,q)

There are multiple ways to describe rotation in a robotic system. In previous tutorial, I have covered some of those methods:

Another way to describe rotation in a robotic system is to use the Axis-Angle representation. With this representation, any orientation of a robotic system in three dimensions is equivalent to a rotation about a fixed axis k through angle θ.

For example, suppose you have a base frame of a robotic arm. This frame is Frame 0 (Joint 1).

The next frame (i.e. the next joint or servo motor) would be Frame 1 (Joint 2). Frame 1 of the robotic arm would be rotated around axis k by angle θ₁.

In most of my robotics work, I’ve assumed that rotation at any joint is around the z axis. So, in this code, I’ve assumed that axis k is the following vector.

k = (k_x, k_y, k_z) = (0, 0, 1)

With the rotation around that axis being θ (e.g. θ₁, θ₂, θ₃, etc.).

Given axis-angle representation, the equivalent 3×3 rotation matrix R_k(θ) is as follows:

This equation above is what you see implemented in the code.

hr_matrix(k,t,q)

This code creates the homogeneous representation (also called “transformation”) matrix. A homogeneous rotation matrix combines the rotation and translation (i.e. displacement) of one coordinate frame relative to another coordinate frame into a single 4 row x 4 column matrix.

class RoboticArm

This class represents the robotic arm that we want to control.

init(self,k_arm,t_arm)

This piece of code is the constructor for the robotic arm class. This constructor initializes the data fields of the robotic arm of interest. These data fields include the number of joints, the rotation axes for each joint, and the measured lengths (i.e. translations) between each joint.

position(self,Q,index=-1,p_i=[0,0,0])

This piece of code uses hr_matrix(k,t,q) to calculate the position in the global (base) frame of a point given in a joint frame.

pseudo_inverse(self,theta_start,p_eff_N,goal_position,max_steps=np.inf)

This piece of code performs inverse kinematics using the pseudoinverse of the Jacobian. What does that mean? Let me explain now how this process works.

You start off with the joints of the robotic arm at arbitrary angles (by convention I typically set them to 0 degrees).

The end effector of the robotic arm is located at some arbitrary position in 3D space.

You then iterate. At each step of the iteration, you

1. Calculate the distance between the current end effector position and the desired goal end effector position. This value (which is a vector…a list of numbers) is often called “delta p”.

2. Reduce the delta p vector by some scaling factor so that it is really small. We only want the end effector to move by a small amount at each time step because, for small motions, we can approximate the displacement of the end effector as linear. Doing this makes the math easier, enabling us to use the Jacobian matrix. The Jacobian matrix is, at its core, is a matrix of partial derivatives.

Remember, forward kinematics (i.e. the motion of revolute joints like servo motors) is nonlinear and would typically involve sines and cosines like we saw in the Analytical IK method), but in this case, we make linear approximations to that nonlinear motion.

3. Calculate the Jacobian matrix J.

4, Calculate the pseudoinverse of the Jacobian matrix. The pseudoinverse of the Jacobian matrix is calculated because the regular inverse (i.e. J^-1 which we looked at in a previous tutorial) fails if a matrix is not square (i.e. a square matrix is a matrix with the same number of columns and rows). The pseudoinverse can invert a non-square matrix.

5. Multiply the pseudoinverse of the Jacobian matrix and the scaled delta p vector together to calculate the desired tiny change in the joint values.

6. Update each joint angle by the amount of that tiny change.

7. The resulting motion from updating the joint values should bring the end effector just a little bit closer to your desired goal position.

Keep repeating the steps above again and again until the end effector has reached the desired goal position (or rather “close enough”).

In a nutshell, at each iteration, we have the end effector take tiny steps towards the goal. We only stop when the end effector reaches the goal position.

I like to set the maximum number of iterations around 500, but you’ll often get the end effector to reach the desired goal position long before that many iterations.

Once the end effector is close enough, return the final joint angles. For a real robotic arm, these are the angles that each servo motor would need to be for the end effector of the arm to be at the goal position.

jacobian(self,Q,p_eff_N=[0,0,0])

In this piece of code, we calculate the Jacobian matrix (position part on the top half…not the orientation part on the bottom half).

d⁰_nis the displacement from the origin of the global coordinate frame to the origin of the end of the end effector.

For a revolute joint (like a servo motor), the change in the linear velocity of the end effector is the cross product of the axis of revolution k (which is made up of 3 elements, k_x, k_y, and k_z) and a 3-element position vector from the joint to the end effector.

For example, given a robotic arm with two joints (i.e. two servo motors), the Jacobian is calculated as follows:

J_v = [J_v1, J_v2]

Where:

J_v1 is the first column of the Jacobian matrix.
J_v2 is the second column of the Jacobian matrix.
k₁is the 3-element axis for the first joint (e.g. (0, 0, 1)).
k₂is the 3-element axis for the second joint (e.g. (0, 0, 1)).
p_effis a 3-element vector that represents the position of the end effector in the base frame of the robotic arm.
p^B₁is the position of joint 1 relative to the base frame.
p^B₂is the position of joint 2 relative to the base frame.

main()

Given a two degree of freedom robotic arm and a desired end position of the end effector, we calculate the two joint angles (i.e. servo angles).

If you see this line of code, you’ll see that I want the robotic arm to go to x = 4.0, y = 10.0.

endeffector_goal_position = np.array([4.0,10.0,a1 + a4])

In fact, these coordinates are the same goal coordinates I set in this inverse kinematics project, where I did an analytical approach to inverse kinematics on a real robotic arm.

In that post, I set the goal destination to x = 4.0 and y = 10.0. If you uncomment the Arduino code on that tutorial and run it, you’ll see that the Serial Monitor outputs the final joint angles (that get the end effector of the robotic arm to the goal destination) as:

θ₁ = 42.8

θ₂ = 50.3

This is pretty much equal to what we get with the Pseudoinverse Jacobian approach from our Python code earlier in this section.

Conclusion

What I’ve shown in this tutorial are two popular methods for calculating the inverse kinematics of a robotic arm. There are other methods out there like Cyclic Coordinate Descent, the Damped Least Squares method, and the Jacobian Transpose method. What method you choose for your project depends on your personal preference and what you’re trying to achieve.

That’s it! Keep building!

The Ultimate Guide to Jacobian Matrices for Robotics

In this tutorial, we’ll learn about the Jacobian matrix, a useful mathematical tool for robotics.

Real-World Applications

There are some use cases (e.g. robotic painting) where we want to control the velocity of the end effector (i.e. paint sprayer, robotic hand, gripper, etc.) of a robotic arm. One way to do this is to use a library to set the desired speed of each joint on a robotic arm. I did exactly this in this post.

1-Robotic-spray-painting-experiment — A car painting robot. If the robotic arms move too fast, they will do a poor job painting the vehicle. We can use Jacobian matrices to help control the speed of the arm. Image Source: ResearchGate.

Another way we can do this is to use a mathematical tool called the Jacobian matrix. The Jacobian matrix helps you convert angular velocities of the joints (i.e. joint velocities) into the velocity of the end effector of a robotic arm.

For example, if the servo motors of a robotic arm are rotating at some velocity (e.g. in radians per second), we can use the Jacobian matrix to calculate how fast the end effector of a robotic arm is moving (both linear velocity x, y, z and angular velocity roll ω_x, pitch ω_y, and yaw ω_z).

Prerequisites

It is helpful if you know how to do the following:

Overview of the Jacobian Matrix

For a robot that operates in three dimensions, the Jacobian matrix transforms joint velocities into end effector velocities using the following equation:

q with the dot on top represents the joint velocities (i.e. how fast the joint is rotating for a revolute joint and how fast the joint is extending or contracting for a prismatic joint) .

The size of this vector is n x 1, with n being equal to the number of joints (i.e. servo motors or linear actuators) in your robotic arm.
Note that q can represent either revolute joints (typically represented as θ) or prismatic joints (which I usually represent as d, which means displacement)

J is the Jacobian matrix. It is an m rows x n column matrix (m=3 for two dimensions, and m=6 for a robot that operates in three dimensions). n represents the number of joints.

The matrix on the left represents the velocities of the end effector.

x, y, and z with the dots on top represent linear velocities (i.e. how fast the end effector is moving in the x, y, and z directions relative to the base frame of a robotic arm)

ω_x, ω_y, and ω_zrepresent the angular velocities (i.e. how fast the end effector is rotating around the x, y, and z axis of the base frame of a robotic arm).

Consider this SCARA robotic arm:

The equation using the Jacobian would be as follows:

Looking closer at the Jacobian, this term can be divided into two parts, where the first three rows are used for linear velocities, and the bottom three rows are used for angular velocities.

Taking a look at a real-world example, you’ll see that the SCARA robot that I’ve been building (see my YouTube video here) can only rotate around the z axis of the base frame (i.e. straight upwards towards the ceiling out of the board). There is no rotation around the x or y axes of the base frame.

How To Find the Jacobian Matrix for a Robotic Arm

To fill in the Jacobian matrix, we use the following tables:

The upper half of the matrix is used to determine the linear components of the velocity, while the bottom part is used to determine the rotational components of the velocity.

Example 1: Cartesian Robot

Consider the following robot that has three prismatic joints.

Here is the equation with the Jacobian matrix:

Which is the same as:

Notice that the q’s were replaced with d’s, which represent “displacement” of the prismatic joint (i.e. linear actuator).

To fill in the Jacobian matrix, we have to come one column at a time from left to right. Each column represents a single joint.

Let’s start with Joint 1. Making i = 1, we get:

Now let’s fill in the second column of the matrix which represents Joint 2. Making i = 2, we get:

Finally, let’s fill in column 3, which represents Joint 3.

The R in the matrix above stands for “rotation matrix.” For example, R⁰₁stands for the rotation matrix from frame 0 to frame 1.

Now to fill in the rotation matrices, we need to add reference frames to the joints. This tutorial shows you how to do that. I’m going to use that same graphic here.

Let’s start with R⁰₀. Since we have no rotation from frame to frame 0, R⁰₀is the identity matrix. Anything that is multiplied by the identity matrix will remain itself, so our matrix now looks like this:

What is R⁰₁? We derived this matrix here.

So now we have:

Which reduces to:

What is R⁰₂? We know that:

R⁰_{2 =}R⁰₁R¹₂

Here is R¹₂:

Therefore:

Our Jacobian becomes:

Which reduces to:

Writing this a bit more clearly, we have:

Our original equation then becomes:

We can do matrix multiplication on the right side of this equation above to yield six equations:

If we look at the kinematic diagram, we can see if the equations above make sense. You can see that the speed of the end effector in the x₀direction is determined by the displacement of joint 2 (i.e. d₂).

You can see that the speed of the end effector in the y₀direction is determined by the negative displacement of joint 3 (i.e. d₃).

The speed of the end effector in the z₀direction is determined by the positive displacement of joint 1 (i.e. d₁).

The last three equations tell us that the end effector will never be able to rotate around the x, y, and z axes of frame 0, the base frame of the robot. This makes sense because all of the joints are prismatic joints. We would need to have revolute joints in order to get rotation.

Example 2: Articulated Robot

Now let’s take a look at another example, an articulated robot that has the following kinematic diagram.

This robotic arm will have three columns since it has three joints (i.e. servo motors). As usual, it will have six rows since we’re working in three dimensions.

Our equation that relates joint velocity to the velocity of the end effector takes the following form:

The Jacobian matrix J (the 6×3 matrix above with the squares) takes the following form since we have all revolute joints:

So using the form above, our Jacobian matrix for the articulated robotic arm is (n is the number of joints):

You’ll see that each column (i = 1, 2, and 3) uses the Jacobian for revolute joints since each joint is a revolute joint.

The x in the above matrix stands for the cross product.

Now, let’s fill in this Jacobian matrix, going one column at a time from left to right.

R⁰₀ is the identity matrix since there is no rotation from frame 0 to itself.

We now need to find d⁰₃. d⁰₃comes from the upper right of the homogeneous transformation matrix from frame 0 to frame 3 (i.e. homgen_0_3). That is, it comes from the T part of the matrix of the following form:

To get the homogeneous transformation matrix from frame 0 to frame 3, we first have to fill in the Denavit-Hartenberg table for the articulated robotic arm. I show you how to do this in this tutorial.

Here was the table we got:

Now, we take a look at that table and go one row at a time, starting with the Joint 1 row. To find the homogeneous transformation matrix from frame 0 to frame 1, we use the values from the first row of the table to fill in this…

We then look at the second row of the D-H table and fill in the homogeneous transformation matrix from frame 1 to 2…

We then look at the third row of the D-H parameter table to fill in the homogeneous transformation matrix from frame 2 to 3…

Then, to find the homogeneous transformation matrix from the base frame (frame 0) to the end-effector frame (frame 3), we would multiply those three transformation matrices together.

homgen_0_3 = (homgen_0_1)(homgen_1_2)(homgen_2_3)

d⁰₃is a three element vector that is made up of the values of the first three rows of the rightmost column of homgen_0_3. I won’t go through all the matrix multiplication for finding homgen_0_3, but if you follow the method I’ve described above, you will be able to get it.

d⁰₀will always be 0, so our Jacobian will look like this at this stage:

Going to the second column of the matrix, d⁰₁is a three element vector that is made up of the values of the first three rows of the rightmost column of homgen_0_1 (which you found earlier).

R⁰₁is below (see this tutorial for how to find it):

Going to the third column, d⁰₂is a three element vector that is made up of the values of the first three rows of the rightmost column of homgen_0_2. Since you know homgen_0_1 and homgen_1_2, you can multiply those two matrices together to get homgen_0_2.

homgen_0_2 = (homgen_0_1)(homgen_1_2)

R⁰₂(i.e. rot_mat_0_2) is found by multiplying the rotation matrices from frame 0 to frame 1 and from frame 1 to frame 2.

rot_mat_0_2 = (rot_mat_0_1)(rot_mat_1_2)

We know rot_mat_0_1. Rot_mat_1_2 is the following:

Now in the top half your Jacobian matrix you will need to go column by column and take the cross product of two three-element vectors.

In a real-world setting, you would use a Python library like NumPy to perform this operation. If you do a search for “numpy.cross” you will see how to do this. For example, here is some Python code of how to take the cross product of two three-element vectors:

x = [1, 2, 3]
y = [4, 5, 6]
np.cross(x, y)
Output:
array([-3,  6, -3])

Now going to the bottom half of the matrix, R⁰₀ is the identity matrix.

You can fill in the rest of the bottom half of the Jacobian matrix because we know R⁰₁ and R⁰₂.

Once you have the Jacobian, you will then have the following setup:

From the setup above, you can find six equations, one for each velocity variable (on the far left). These equations enable us to calculate the velocities of the end effector of the articulated robotic arm given the joint velocities.

Inverse Jacobian Matrix

In the previous section, we looked at how to calculate the velocities of the end effector of a robotic arm given the joint velocities. What if we want to do the reverse? We want to calculate the joint velocities given desired velocities of the end effector?

To solve this problem, we must use the inverse of the Jacobian matrix.

A matrix multiplied by its inverse is the identity matrix I. The identity matrix is the matrix version of the number 1.

You can only take the inverse of a square matrix. A square matrix is a matrix where the number of rows is equal to the number of columns.

So how would we find J^-1? Let’s take a look at an example.

Example

Suppose we have the following two degrees of freedom robotic arm.

We have the following equation where the matrix with the 12 squares is J, the Jacobian matrix.

We only have two servo motors. These two servo motors control the velocity of the end effector in only the x and y directions (e.g. we have no motion in the z direction).

Suppose the only thing that matters to us is the linear velocity in the x direction and the linear velocity in the y direction. We can simplify our equation accordingly to this, where the matrix with the squares is J:

Let’s replace those squares with some variables.

Since…

To get the J^-1, we use the following formula:

To get J^-1, we first need to find J. J has two revolute joints. Revolute joints take the following form:

So the equation for our two degree of freedom robotic arm will look like this:

R⁰₀is the identity matrix.

To calculate d⁰₂, we need to find the homogeneous transformation matrix from frame 0 to frame 2. We did exactly this on this tutorial.

homgen_0_2 = (homgen_0_1)(homgen_1_2)

When you do this multiplication, you get the following:

Based on the homogeneous matrix above, here is d⁰₂.

Plugging that into this expression below and performing the cross product with the vector [0 0 1], we get:

And since:

We know that:

R⁰₁ is the 3×3 matrix in the upper left of homgen_0_1.
d⁰₁is the 3×1 vector in the upper right of homgen_0_1.
We already know d⁰₂. We found that earlier in this tutorial.
We can therefore fill in the rest of the upper half of the Jacobian matrix to get this:

J is that big matrix above. Since we are only concerned about the linear velocities in the x and y directions, this:

Becomes…

J is that big matrix above. It takes the form:

To get the inverse of J (i.e. J^-1), we do the following:

The (J₁₁J₂₂ – J₁₂J₂₁) in the denominator is known as the determinant. I’ll call 1/(J₁₁J₂₂ – J₁₂J₂₁) the reciprocal of the determinant.

Once we know J^-1, we can use the following expression to solve the joint velocities given the velocities of the end effector in the x and y directions.

Which is the same as:

Which is:

Programming the Jacobian on a Real Robot

Let’s take a look at all this math we’ve done so far by implementing it on a real-world robot.

To do this section, you need to have assembled the two degrees of freedom SCARA robot here.

We want to calculate the rotation velocity of the two joints (i.e. servo motors) based on desired linear velocities of the end effector.

We will program our robotic arm so that the end effector of the robotic arm moves in a straight line at a constant linear velocity.

This equation will come in handy. That big nasty matrix below is the Jacobian. We’ll use it to calculate the inverse Jacobian matrix in our code.

Open the Arduino IDE and create a new sketch named jacobian_based_path_planning_v2.ino.

Write the following code. It is helpful to write the code line by line rather than copying and pasting so that you understand what is going on.

/*
Program: Jacobian-Based Path Planning
File: jacobian_based_path_planning_v1.ino
Description: This program moves the end effector of a SCARA robotic arm based on your
  desired x and y linear velocities. 
  Motion is calculated using the Jacobian matrix which relates joint velocities
  to end effector velocities.
  Note that Servo 0 = Joint 1 and Servo 1 = Joint 2.
Author: Addison Sears-Collins
Website: https://automaticaddison.com
Date: October 14, 2020
*/
 
#include <VarSpeedServo.h> 

// Define the number of servos
#define SERVOS 2

// Conversion factor from degrees to radians
#define DEG_TO_RAD 0.017453292519943295769236907684886

// Conversion factor from radians to degrees
#define RAD_TO_DEG 57.295779513082320876798154814105

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

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

const int std_delay = 10; // Delay in milliseconds

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

// Angle of the first servo
float theta_1 = 0;
float theta_1_increment = 0; 
float theta_1_dot = 0; // rotational velocity of the first servo

// Angle of the second servo
float theta_2 = 0;
float theta_2_increment = 0;
float theta_2_dot = 0; // rotational velocity of the second servo

// Linear velocities of the end effector relative to the base frame
// Units are in centimeters per second
// If x_dot = 0.0, the end effector will move parallel to the y axis
// Play around with these numbers, and observe the motion of the end effector
// relative to the x and y axes of the base frame of the robotic arm.
float x_dot = 0.0;
float y_dot = 1.0;

// Jacobian variables
float reciprocal_of_the_determinant;
float J11;
float J12;
float J21;
float J22;

// Inverse Jacobian variables
float J11_inv;
float J12_inv;
float J21_inv;
float J22_inv;

// Link lengths in centimeters
// You measure these values using a ruler and the kinematic diagram
float a2 = 5.9;
float a4 = 6.0;

void setup() {

  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 the angle of the first servo.
  theta_1 = 0.0;

  // Set the angle of the second servo.
  theta_2 = 90.0;
  
  // Set initial servo positions 
  myservo[0].write(theta_1, default_speed, true);  
  myservo[1].write(theta_2, default_speed, true);

  // Let servos get into position
  delay(3000);
  
}
 
void loop() { 

    // Make sure the servos stay within their 180 degree range
    while (theta_1 <= 180.0 && theta_1 >= 0.0 && theta_2 <= 180.0 && theta_2 >= 0.0) { 

      // Convert from degrees to radians
      theta_1 = theta_1 * DEG_TO_RAD;
      theta_2 = theta_2 * DEG_TO_RAD;

      // Calculate the values of the Jacobian matrix
      J11 = -a4 * sin(theta_1) * cos(theta_2) - a4 * cos(theta_1) * sin(theta_2) - a2 * sin(theta_1);
      J12 = -a4 * sin(theta_1) * cos(theta_2) - a4 * cos(theta_1) * sin(theta_2);
      J21 = a4 * cos(theta_1) * cos(theta_2) - a4 * sin(theta_1) * sin(theta_2) + a2 * cos(theta_1);
      J22 = a4 * cos(theta_1) * cos(theta_2) - a4 * sin(theta_1) * sin(theta_2);
       
      reciprocal_of_the_determinant = 1.0/((J11 * J22) - (J12 * J21));

      // Calculate the values of the inverse Jacobian matrix
      J11_inv = reciprocal_of_the_determinant * (J22);
      J12_inv = reciprocal_of_the_determinant * (-J12);
      J21_inv = reciprocal_of_the_determinant * (-J21);      
      J22_inv = reciprocal_of_the_determinant * (J11);
      
      // Set the rotational velocity of the first servo
      theta_1_dot = J11_inv * x_dot + J12_inv * y_dot; 

      // Set the rotational velocity of the second servo
      theta_2_dot = J21_inv * x_dot + J22_inv * y_dot; 

      // Convert rotational velocity in radians per second to X radians in std_delay milliseconds
      // Note that 1 second = 1000 milliseconds and each delay is std_delay milliseconds
      theta_1_increment = (theta_1_dot) * (1/1000.0) * std_delay;

      // Convert rotational velocity in radians per second to X radians in std_delay milliseconds
      // Note that 1 second = 1000 milliseconds and each delay is std_delay milliseconds
      theta_2_increment = (theta_2_dot) * (1/1000.0) * std_delay;

      theta_1 = theta_1 + theta_1_increment; 
      theta_2 = theta_2 + theta_2_increment;

      // Convert the new angles from radians to degrees
      theta_1 = theta_1 * RAD_TO_DEG;
      theta_2 = theta_2 * RAD_TO_DEG;

      Serial.println(theta_1);
      Serial.println(theta_2);
      Serial.println(" ");
     
      myservo[0].write(theta_1, default_speed, true); 
      myservo[1].write(theta_2, default_speed, true);
      
      delay(std_delay); // Delay in milliseconds      
    }    
}

Load the code to your Arduino.

Wire up your system as shown on this pdf.

Set the voltage limit on your DC power supply to 6V and the current limit to 2A.

Run the code. You should see the end effector of your robotic arm move parallel to the y axis in a straight line because we set:

After a few seconds, the robotic arm will stop moving in a straight line. The reason for this is that the robotic arm can only move a fixed distance in the positive y direction due to the lengths of the links. The servos, however, continue to rotate until either the first servo or the second servo (or both) reach the limit of their range of rotation (i.e. 0 to 180 degrees).

That’s it! Keep building!

References

Sodemann, Dr. Angela 2020, RoboGrok, accessed 14 October 2020, <http://robogrok.com/>

Review of Denavit-Hartenberg for Robotic Arms

In this tutorial, we will review the fundamentals of Denavit-Hartenberg parameters, which are a convention for assigning coordinate frames to robotic arms. We assign coordinate frames to robotic arms so that we can get them to do useful work in the real world.

Prerequisites

It is helpful if you’ve already gone through the following tutorials.

Guidelines for Drawing Denavit-Hartenberg Frames

1. If J is the number of joints and F is the number of coordinate frames on a robotic manipulator, the following relation must hold:

F > J

2. One coordinate frame must be on the end effector.

3. There are six ways to draw an axis of a coordinate frame.

Rules for Assigning Denavit-Hartenberg Frames

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

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

For step-by-step examples of assigning Denavit-Hartenberg Frames, check out my tutorial here.

Examples

Here are D-H frames for a Scara robotic arm.

Here are the D-H frames for a six degree of freedom robot like the Universal Robots UR5.

Here are the D-H frames for a six degree of freedom industrial robot like the FANUC LRMate 200iD.

Guidelines for Creating the Denavit-Hartenberg Parameter Table

Once you’ve finished drawing the Denavit-Hartenberg frames for a robotic arm, you then create the Denavit-Hartenberg parameter table. This table takes the following form:

Number of Rows = Number of Frames – 1
Number of Columns = 4: Two columns for rotation and two columns for displacement

The Denavit-Hartenberg parameter tables consist of four variables:

The two variables used for rotation are θ and α.
The two variables used for displacement are r and d.

Consider two coordinate frames: Frame 1 and Frame 2. Let Frame 0 = Frame n-1 and Frame 1 = Frame n.

θ is the angle from x_n-1 to x_n around z_n-1.
α is the angle from z_n-1 to z_n around x_n.
r (sometimes you’ll see the letter ‘a’ instead of ‘r’) is the distance between the origin of the n-1 frame and the origin of the n frame along the x_n direction.
d is the distance from x_n-1 to x_n along the z_n-1 direction.

See step-by-step examples of how to create Denavit-Hartenberg Parameter tables.

Guidelines for Creating Homogeneous Transformation Matrices Using the Denavit-Hartenberg Parameter Table

Once we have filled in the Denavit-Hartenberg (D-H) parameter table for a robotic arm, we find the homogeneous transformation matrices (also known as the Denavit-Hartenberg matrix) by plugging the values into the matrix of the following form, which is the homogeneous transformation matrix for joint n (i.e. the transformation from frame n-1 to frame n).

homgen_n-1_n =

where R is the 3×3 submatrix in the upper left that represents the rotation from frame n-1 (e.g. frame 0) to frame n (e.g. frame 1), and T is the 3×1 submatrix in the upper right that represents the translation (or displacement) from frame n-1 to frame n.

For example, suppose we have a SCARA robotic arm.

We draw the kinematic diagram for this arm.

We then use that diagram to find the Denavit-Hartenberg parameters:

We now want to find the homogeneous transformation matrix from frame 0 to frame 1, so we look at the first row of the Denavit-Hartenberg parameter table labeled ‘Joint 1’ (remember that a joint in our case is either a servo motor that produces rotational motion or a linear actuator that produces linear motion).

In this case, n=1, and we would look at the first row of the D-H table (Joint 1) and plug in the appropriate values into the following matrix:

For the homogeneous transformation matrix from frame 1 to 2, we would look at the second row of the D-H table (Joint 2) and plug in the corresponding values into the following matrix.

For the homogeneous transformation matrix from frame 2 to 3, we would look at the third row of the table (Joint 3) and plug in the corresponding values into the following matrix.

Then, to find the homogeneous transformation matrix from the base frame (frame 0) to the end-effector frame (frame 3), we would multiply all the transformation matrices together.

homgen_0_3 = (homgen_0_1)(homgen_1_2)(homgen_2_3)

See step-by-step examples of how to create Homogeneous Transformation Matrices Using Denavit-Hartenberg.

References

Sodemann, Dr. Angela 2020, RoboGrok, accessed 5 October 2020, <http://robogrok.com/>