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

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.

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

• θ2 = tan-1(y/x)
• d3 = sqrt(x2 + y2) – a3 – a4

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

We can see from the above image that:

• d1 = z – a1 – a2

## 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, z6 will point in the same direction as z0. Accordingly, if you can imagine z6 pointing straight upwards, x6 would be oriented in the opposite direction as x0, and y6 would be oriented in the opposite direction as y0. 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).
# 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)

# 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])

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

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

# 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: