Mastering Forward Kinematics for 6-DOF Robots: A Complete Guide

Karan Bhakuni
Jan 12
5 min read

Ever wondered how a robotic arm knows exactly where its hand (or claw, or gripper) is at any given moment? Enter the world of Forward Kinematics (FK)—the mathematical magic that turns joint angles into precise end-effector positions.

Let’s break it down, step by step.

Forward Kinematics in Action

So, What Exactly is Forward Kinematics?

In simple terms, FK is like asking your robot,If I give you these joint angles, can you tell me exactly where your hand will be?"

It’s all about computing the position and orientation of the end-effector using the robot's joint parameters. You feed in the joint angles, and out comes the exact location in space.

But here’s a cool twist: FK is a one-to-one function.

👉 For every unique joint angle configuration, there’s only one possible position and orientation for the end effector. This will blow your mind later when we dive into Inverse Kinematics (IK), where the same end-effector position could have multiple joint configurations. (Spoiler alert: IK is a one-to-many problem. Stay tuned for that!)

The 6-DOF Robot and Its Modified DH Parameters

To keep things exciting, we’re working with a 6-DOF robotic manipulator (6R type). This means six rotating joints that work together to place the end-effector exactly where you want it—like a futuristic surgeon’s scalpel or a pick-and-place wizard.

But how does it all work? Drumroll... Modified Denavit-Hartenberg (DH) Parameters to the rescue! If you’re not familiar with DH parameters, they’re the unsung heroes of robotics, simplifying the complex relationships between joints and links into a set of matrices.

Now, we’ve already solved the Modified DH Parameters for this robot, so no need to re-invent the wheel here. If you’re curious about the detailed derivation, check out this blog post where I dive deep into the math. For now, let’s skip to the good part: the final parameter table.

How Does This All Come Together?

The beauty of FK lies in its simplicity:

You stack transformations for each joint (using the DH matrices).
Multiply them all together, and voilà—you’ve got the final position and orientation of the end-effector in the robot’s workspace.

Here’s How the Actual and Modified DH Frames Shape My Robot

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

Ready to Have Some Fun with FK Math?

In Fig. 2, you can clearly see that on right side i have Modified DH frames assigned and we are using ZYX transformation formula

We will just find the transformation between each frame from frame 0 to last frame by putting values of 4 variables from the Table and then multiply them in order to get the final transformation matrix from base to last frame.

a) Transformation between Frame 1 and Base Frame

b) Transformation between Frame 2 and Base Frame

c) Transformation between Frame 3 and Base Frame

d) Transformation between Frame 4 and Base Frame

e) Transformation between Frame 5 and Base Frame

f) Transformation between Frame 6 and Base Frame

T60 is the transformation matrix of frame 6 wrt base frame. Now we have to just feed the values of theta in the last equation, and we get the coordinates of the last frame.

Let's answer some of the questions arising to curious minds...

1. Why are we not getting the end effector position but only the position of the last frame?

We must be aware of the end effector position wrt the last frame of the robot, and if we multiply that matrix with T60, we will get TE0:

TE0 = TE6*T60

No, we have the transformation matrix for the end effector position, but the task is not done yet. We have to give output in terms of x,y,z,r,p,y. So :

x = TE0[0][3]

y = TE0[1][3]

z = TE0[2][3]

roll,pitch,yaw = convertRotToEuler(TE0[:3,:3])

In this way we can easily get x, y and z positions and roll, pitch, and yaw angles for the end effector wrt base frame.

2. How did we get the transformation matrix formulae? Is it same for every case?

As we are using Modified DH parameters for defining frames we have used this matrix which is derived as below. But if we use some other conventions say old DH paramteter than this matrix will change. By multiplying these matrices we get the Transformation matrix between any two frames.

Turning FK Theory Into Code:

import sympy as sp

# Initialize symbolic variables (ensure l1, l3, l5, l6, le are defined elsewhere)
l1, l3, l5, l6, le, l7 = sp.symbols('l1 l3 l5 l6 le l7')
theta1, theta2, theta3, theta4, theta5, theta6 = sp.symbols('theta1 theta2 theta3 theta4 theta5 theta6')

def transform_sym(n):
    # Define alpha, a, d
    alpha = [0, sp.pi/2, 0, 0, -sp.pi/2, sp.pi/2, 0]
    a = [0, 0, -l3, -l5, 0, 0, l7]
    d = [l1, 0, 0, l6, 0, 0, le]
    theta = [theta1, theta2, theta3, theta4, theta5, theta6]

    # Adjust theta
    theta[0] += sp.pi/2
    theta[1] -= sp.pi/2
    theta[3] += sp.pi/2
    theta[5] = theta[5] + sp.pi/2

    # Initialize transformation matrix
    Ttemp = sp.eye(4)

    # Loop to calculate the transformation matrix
    for i in range(n):
        Tim = sp.Matrix([
            [sp.cos(theta[i]), -sp.sin(theta[i]), 0, a[i]],
            [sp.sin(theta[i]) * sp.cos(alpha[i]), sp.cos(theta[i]) * sp.cos(alpha[i]), -sp.sin(alpha[i]), -d[i] * sp.sin(alpha[i])],
            [sp.sin(theta[i]) * sp.sin(alpha[i]), sp.cos(theta[i]) * sp.sin(alpha[i]), sp.cos(alpha[i]), d[i] * sp.cos(alpha[i])],
            [0, 0, 0, 1]
        ])
        Ttemp = Ttemp * Tim

    # Define the transformation matrix T_end_effector_to_wrist
    T_end_effector_to_wrist = sp.Matrix([
        [-1,  0,  0,  l7],
        [ 0, -1,  0,  0],
        [ 0,  0,  1,  le],
        [ 0,  0,  0,  1]
    ])

    # Uncomment this if you want transformation matrix from base to last frame not the end effector
    # T_end_effector_to_wrist = sp.eye(4)

    # Multiply the transformation matrix with Ttemp before returning
    Ttemp = Ttemp * T_end_effector_to_wrist

    return Ttemp

# Test the function for symbolic values
Te0_symbolic = sp.simplify(transform_sym(6))
display(Te0_symbolic)

# Test the function for Numerical Values
values = {
    l1: 0.2,
    l3: 0.3,
    l5: 0.32,
    l6: 0.25,
    l7: 0.18,
    le: 0.1,
    theta1: 0,
    theta2: 0,
    theta3: 0,
    theta4: 0,
    theta5: 0,
    theta6: 0,
}

T_36_numeric = Te0_symbolic.subs(values)
display(T_36_numeric)

In the above code snippet, you just have to give an HTM(Homogeneous Transformation Matrix) that is actually a matrix given by the user, and you will be able to get the final transformation matrix between the base and end effector.

Wrapping It Up: The Power of Forward Kinematics

And there you have it – the backbone of robotic movement, Forward Kinematics! Understanding how the position and orientation of your robot’s end effector are determined by its joint angles is essential for precise control. Whether you're designing a robotic arm to perform delicate surgeries or programming a drone to navigate through complex terrains, Forward Kinematics gives you the foundation you need to make these movements predictable and reliable.

As we delve deeper into the world of robotics, remember that while Forward Kinematics tells us where we’re going, it’s the inverse kinematics that might be the real game-changer when it comes to actually getting there. Keep experimenting, keep learning, and watch your robot come to life—one calculation at a time!

Stay tuned for the next chapter, where we’ll tackle the mysterious world of inverse kinematics. 🚀🤖