The Reverse Game: Unpacking Inverse Kinematics for 6-DOF Robots

Imagine you're a robot arm, tasked with picking up a cup of coffee. You know exactly where the cup is, but how do you figure out which joints to bend and by how much to reach it? Sounds tricky, right? That’s exactly what Inverse Kinematics (IK) helps solve!

Inverse Kinematics in Action

While Forward Kinematics (FK) told us where the end effector would be based on joint angles, IK flips the script—it tells us the joint angles needed to reach a desired position and orientation.  Basically you are provided with Target position(x,y,z) and Target Orientation(roll,pitch,yaw), and we have to calculate the joint configuration corresponding to given position and orientation.

Fig1-Inverse Kinematics FlowChart

Why a 6-DOF Robot?

Ever heard the phrase, "The right tool for the job"? In robotics, a 6-DOF (Degree of Freedom) robot is just that—perfectly suited to solve the IK puzzle. Let’s dive into why:

1. Our Space Has 6 Degrees of Freedom: Imagine moving freely in 3D space. You can translate (move) in three directions—x, y, and z—and rotate around these axes (roll, pitch, and yaw). A 6-DOF robot matches this freedom, allowing us to address all possible movements in our space.

2. 6 Equations for 6 Unknowns: In IK, we solve for 6 unknowns: the joint angles. For this, we need 6 independent equations. A 6-DOF robot provides just the right amount of control to uniquely determine these solutions for a specific end-effector pose (position + orientation).

3. What About 7-DOF Robots?While a 7-DOF robot is also popular, there’s a twist: infinite solutions! This redundancy can complicate finding the "best" joint configuration. In contrast, a 6-DOF robot offers a more straightforward approach—one pose, one solution whereas with 7 dof robot we can have access to more confined spaces but that comes with lot of complexities so lets take it later on.
   

Fig. 2: Actual Joints(left) and Modified DH Frames(right) of 6-axis Manipulator

Fig 2 actually shows the actual joint placements(left) and joint placements according to Modified DH Parameters.

1. To start with we will get an input from user as a 6D array with x,y,z,r,p,y values:

For eg :[0.2,0.4,0.3,1.57,1.2,0.5]

2. Now first we have to do Decoupling. Now you might wonder what decoupling is? Let me first explain what decoupling is which you often heard of while solving any manipulator.

Decoupling in the context of solving the inverse kinematics (IK) of a 6-DOF manipulator refers to separating the problem into two simpler, independent subproblems:

1. Position IK – Determining the first three joint angles to position the wrist (usually the wrist center).

2. Orientation IK – Determining the last three joint angles to orient the end effector.

How Decoupling Works

Now as you got some idea on decoupling that is basically helps to divide the problem into two subparts such that we can solve the problem with ease without getting into the non linearity of the problem. Directly solving all six joint angles simultaneously is mathematically complex due to nonlinear relationships between joint angles and the end effector's pose. 

Now lets jump onto some real Mathematics………

1. We will start with forward kinematics to get the equations of wrist:

Below given is the final matrix that will come after solving forward kinematics. To learn this go to the Forward Kinematics(Provide link for the blog) blog.

2. In the matrix i m only showing the last column as we require that only to get first three joint positions. As the matrix is very large i have not written the entire matrix. 

x_wrist =  l3​sin(θ1​)sin(θ2​)+l5​sin(θ1​)sin(θ2​+θ3​)+l6​cos(θ1​)

y_wrist = −l3​sin(θ2​)cos(θ1​)−l5​sin(θ2​+θ3​)cos(θ1​)+l6​sin(θ1​)

z_wrist = l1​+l3​cos(θ2​)+l5​cos(θ2​+θ3​)

Using Above equations to solve for all the angles as below:

Now we have the values for theta1, theta2, theta3. Now we can actually divide our transformation matrix from base to wrist into 2 parts i.e. base to shoulder and shoulder to wrist (in other words from base to joint3 and joint3 to joint6). As we have the complete matrix from base to wrist from the user and we have just calculated our angles theta1, theta2, theta3. We can now easily calculate the transformation matrix from base to joint 3. So  from this we can now calculate the transformation matrix from joint 3 to wrist and that will help us to calculate theta4, theta5 and theta6.

Things to Remember

1. I have used s2,s3,s5 because for those joints i have multiple solutions as you know that inverse kinematics in not a one to one mapping. So for each of the end effector configuration we have 8 solutions possible. And to handle that i have used these three variable so that we can make 2^3 values with +/- combinations.

2. Using above technique we will get values from -180 to 180 as we are using atan2 function in python or C++. To get full complete solutions we have to deal with roll over that happens in euler angels and depending on the joint limits of your robot you can select the most feasible solution between 0 to 360.

 To handle roll over we have to just add 360 if the difference between current solution and last solution is below -180 and subtract 360 if the difference between current solution and last solution is above 180.

References:

1. ChatGpt

2. Quillbot

3. Robotics and Control by RK Mittal and IJ Nagrath.