Forward Kinematics in Robotics Using Screw Theory

Can you recall a time when you were frustrated when attempting to compute the forward kinematics for robotic systems? Did you find it frustrating to assign coordinaNte frames to each link when robots become more sophisticated? 

If your answer to these questions is yes, and you’re tired of cumbersome methods like Denavit-Hartenberg to calculate the forward kinematics of robotic chains, this lesson is for you. 

Rather than going into mathematical details first, let’s get right to the point with an educational example.

Example: Forward Kinematics of a 3 DOF Planar Open Chain Robot Arm

Consider a three dof planar open-chain robot arm modeled using STEM building blocks. Here is what it will look like once the coordinate frames, joints, and link lengths are identified:

L₁, L₂, and L₃ are link lengths. {0} is the fixed frame with the origin located at the base joint. {4} is the end-effector frame attached to the tip of the third link. We do not need all the frames to derive the forward kinematics using the product of exponentials formula, but we depicted them for educational purposes. Since the motion is on the plane, the ẑ-axes are parallel and out of the page. But again, to reiterate, we don’t need all the coordinate frames.

In order to find the forward kinematics of a 3-dof planar open-chain robot, we will follow the following simple steps:

Step 1: Write the vector of joint angles.

Since all the joints, in this case, are revolute, the vector of joint angles can be written as:

Step 2: Draw the robot in its home or zero position when all joint angles are set to zero and assign the base frame and the end-effector frame.

The 3-dof planar open-chain robot in its zero position can be depicted as the figure below:

Step 3: Calculate the matrix M, which is the position and orientation of frame {4} relative to frame {0} in the robot’s zero position.

For our 3-dof planar open-chain robot, the matrix representing the position and orientation of the frame {4} relative to the frame {0} can be written using the knowledge that we gained in the transformation matrices lesson.

The rotational part is the identity matrix since the orientation of frame {4} is the same as the frame {0}, and the position of the origin of frame {4} is L₁ + L₂ + L₃ units in the direction of the x-axis of the {0} frame.

Step 4. Calculate the coordinates of 3D arbitrary points on the joint axes at zero configuration with respect to the coordinates of the fixed base frame.

For our 3-dof planar open-chain robot, we go back to the figure of the robot in the zero position. We can take the three arbitrary points on the joint axes as the following points:

Then we can write the coordinates of these points with respect to the base frame as:

The first point is on the origin of the base frame, and that’s why its coordinates are all zero. The second point is located at L₁ units from the x-axis of the base frame, and similarly, the third point is at L₁ + L₂ units from the x-axis of the base frame.

Step 5. Calculate the screw axis of each joint expressed in the base frame in the robot’s zero position.

For our 3-dof planar robot, we can say that each revolute joint axis is a zero-pitch screw axis (we know this from the previous lessons) by holding the first two joint angles at zero position:

The third link goes through a screw motion, and the screw axis corresponding to rotating about the third joint expressed in frame {0} consists of two angular and linear parts as we saw in the lesson about screw motion in robotics:

As we saw before, the angular part is the angular velocity when the angular speed is one, and the linear part is the linear velocity of the origin when the angular speed is one θ̇ = 1:

where a₃ is any point on the screw axis expressed in frame {0} that we calculated in the previous step.

Therefore, the linear part of the screw axis can be calculated by the cross-product of the angular part of the screw axis and the selected point on the screw axis:

From this, we can see that the linear part of the screw axis is in the opposite direction of the y-axis of the {0} frame. This is also expected from the circular motion of the origin of joint 3 and the fact that the linear velocity is tangent to the circular path.

To learn more about circular motions and why the linear velocity is tangent to the circular path, please refer to the exponential coordinates lesson.

Therefore, the screw axis of joint 3 can be written by stacking up the angular part and the linear part:

Now, considering only the screw motion about the third joint, we can write the configuration of the end-effector in the base frame as the following transformation matrix:

The bracket notation shows the se(3) matrix form of the screw axis that we learned all about it and how to calculate it in the screws in robotics lesson.

Now for θ₁ = 0 and fixed (but arbitrary) θ₃, rotation about joint 2 can be viewed as applying a screw motion to the rigid link 2, and link 3 pair:

Then, the configuration of the end-effector in the base frame can be updated as the following transformation matrix:

And using the same method as before, the angular and linear components of the second screw axis can be calculated as follows:

then the second screw axis can be calculated by stacking up the angular component and the linear component:

Finally, θ₂ and θ₃ are fixed, and we consider the rotation about joint 1. This is equivalent to applying a screw motion to the entire rigid three-link assembly.

For arbitrary values of (θ₁,θ₂,θ₃), the final configuration of the end-effector in the base frame can be updated as the following transformation matrix:

Where the first screw axis can be written as the following equation:

Note that here the arbitrary point is at the center of the base frame and thus the linear part is zero. At the end of this lesson, we will provide you with a code that you can easily find the position and orientation of the end-effector frame in the base frame using the PoE formula.

Using the code provided in this lesson, we can find the position and orientation of the end-effector frame in the base frame as the following equations:

Note that x, y, and φ provide the position and orientation of the end-effector frame with respect to the base frame given the joint values, and we call it the forward kinematics of this robotic arm.

The steps taken to find the forward kinematics of the 3-dof planar robot are applicable to all open-chain manipulators, and we will see the general formula shortly. 

By now, we’ve become familiar with the tools required to express robot motions. Starting with this lesson, we will use the foundation we gained over the past eighteen lessons to better understand real robotics topics. In this lesson, we started with forward kinematics of serial manipulators, and our approach is based on screw theory in robotics. If you are unfamiliar with screw theory in robotics, you might want to read the previous lessons first. 

Now let’s learn about the general form of forward kinematics of serial manipulators using screw theory in robotics.

If you take a look at the literature on robot kinematics, you will figure out that most approaches are based on Denavit-Hartenberg (D-H) parameters presented first by guess what two people named Denavit and Hartenberg in an article in 1955.

For reasons you’ll learn in this lesson, we prefer Brockett’s 1983 Product of Exponentials (PoE) formula based on screw theory to represent forward kinematics of open-chain robot arms. You can refer to the references section for the article. 

Forward kinematics calculates the position and orientation of the robot end-effector from the joint coordinates q. Therefore, it is a map that takes the joint values to the position and orientation of the end-effector frame or {b} frame with respect to the fixed reference frame {s}.

If you do not know what the body frame or the fixed frame is, please read this lesson.

Product of Exponentials Formula (PoE)

Now that we have seen how the PoE formula is used to find the forward kinematics of a three-link planar robot arm, let’s look at a more general description.

Let’s consider the figure below, which consists of n one-dof joints connected serially in an n-link spatial open chain:

The key concept behind the PoE formula is that every joint applies a screw motion to all outward links.

In order to find the configuration of the end-effector frame relative to the base frame, we first choose a fixed frame {s} and the end-effector frame {b} attached to the last link.

Then, we should place the robot in its zero position, which means setting all joint values to zero with the direction of positive displacement (rotation for revolute joints, translation for prismatic joints) for each joint. Then find M ∈ SE(3), which is the configuration of the end-effector frame with respect to the fixed base frame when the robot is in its zero position.

Now suppose that the joint n is displaced to some joint value q_n. The end-effector frame M undergoes a displacement of the form:

T is the new configuration of the end effector frame, and S_n is the screw axis of joint n expressed in the fixed base frame. Here we can have two cases:

If joint n is revolute, that corresponds to a screw motion of zero pitch since there is no linear motion, then the angular part of the screw axis S_ω ∈ ℝ³ is the unit vector in the positive direction of the joint axis n, and the linear part of the screw axis can be calculated by the cross product of the angular part and a_n, S_v = -S_ω×a_n where a_n is any arbitrary point on joint axis n written in coordinates of the fixed base frame. Here, q_n = θ_n is the joint rotation.

If joint n is prismatic, then the angular part of the screw axis is zero S_ω = 0, and the linear part S_v ∈ ℝ³ is the unit vector in the direction of the positive translation. Here, q_n = d_n is the prismatic extension/retraction.

To understand the different types of robot joints, please refer to the lesson on the degrees of freedom of a robot.

Now assume that joint n-1 is allowed to vary. Then this has the effect of applying a screw motion to link n-1 (and by extension to link n because link n is connected to link n-1 via joint n).

Then the end effector frame undergoes a displacement of the form:

Continuing this reasoning and now allowing all the joints (q₁ … q_n) to vary, we can find the configuration of the end-effector frame relative to the base frame as the following matrix:

This is called the Product of Exponentials formula that describes the forward kinematics of an n-dof open chain. This representation is the space form of the PoE formula because the screw axes are expressed in the fixed space frame. Consequently, the forward kinematics is a product of matrix exponentials, each corresponding to a screw motion. 

Note that there is also the body form of the PoE formula where the joint axes are screw axes that are expressed in the end-effector or body frame. We will not study this in this lesson since we think that it is basically the same as the space form, and one can easily deduce that by learning the space form.

To summarize, in order to calculate the forward kinematics of an open-chain manipulator using the space form of the PoE formula, we should first find the following elements:

• the joint variables q₁ … q_n
• the end-effector configuration M ∈ SE(3) when the robot is at its home position, meaning that all the joint variables are set to zero
• the screw axes S₁ … S_n expressed in the fixed base frame, corresponding to the joint motions when the robot is at its home position.

Now let’s talk about why we prefer the PoE formula to other approaches like Denavit-Hartenberg.

As you may know, Denavit-Hartenberg (D-H) is another convention for calculating the FK of open-chain robots, which requires a minimum number of parameters. For an n-joint robot, 3n numbers are required to describe the robot’s structure, and n numbers to describe the joint values.

PoE representation, however, isn’t minimal. Besides the n joint values, 6n numbers are required to describe the n screw axes. There are, however, advantages that outweigh the only disadvantage.

As opposed to D-H representation, link reference frames do not need to be defined, which is a huge advantage. All we need is the base frame and M. On the other hand, revolute and prismatic joints are treated the same, and the joint axes can intuitively be interpreted geometrically as screws.

A major disadvantage of D-H parameters is that link frames are assigned differently, and both link frames and D-H parameters should be given together to fully describe the robot’s forward kinematics.

We can conclude that there’s no compelling reason to prefer D-H parameters over PoE representation unless using a small number of parameters is very important.

We will see more advantages of PoE representation when studying velocity kinematics. 

Let’s now see some more examples.

Example: Forward Kinematics of UR5e 6R Robot Arm from Universal Robots

Consider the UR5e 6R robot arm from Universal Robots shown in the figure below:

Our goal is to determine the forward kinematics of this open-chain collaborative industrial robot. By following the steps outlined in this lesson, we can easily accomplish this.

All joints in this robot are revolute joints, so the vector of joint angles can be written as:

Below is a figure of this robot in its zero position with the base and end-effector frames attached:

The screw axes in the robot’s zero position are chosen based on the positive rotation of the joints based on the right-hand rule (see the video version for the joint movements and their positive rotations). You can learn about the right-hand rule in the lesson below.

Note that W₁ is the distance between the anti-parallel axes of joints 1 and 5.

Now it’s time to calculate M, which is the position and orientation of the tool frame relative to the base frame in the robot’s zero position:

The next step is to calculate the coordinates of 3D arbitrary points on the joint axes at zero configuration with respect to the coordinates of the fixed base frame:

Note that these points can easily be found on the screw axes using simple geometry.

Next, we will calculate the screw axis of each joint expressed in the base frame in the robot’s zero position. First, we calculate the rotation parts of the screw axes:

Note that, for example, the rotational part of the screw axis of joint 4 is in the opposite direction of the y-axis of the base frame, and it is a unit vector.

Now that we have the rotational parts and the arbitrary points on the screw axes, we can find the linear parts of the screw axes by finding the cross product of the rotational part and the 3D arbitrary point:

You can easily calculate this cross product using MATLAB. For instance, for the linear part of the joint 3 axis, we can write:

syms L1 L2 H1 H2 W1 W2
a3 = [-L1;0;H1];
Sw3 = [0;-1;0];
Sv3 = cross(a3,Sw3)

Stacking up the rotational part and the linear part will give us the screw axis.

Now we have all the necessary elements to find the forward kinematics of the UR5e robotic arm representing the position and orientation of the end-effector with respect to the base frame as:

As an example for a visual representation of the forward kinematics of this robot, suppose that θ₂ = -π/2 , and θ₅ = π/2 with all other joint angles to be zero. Also, suppose that W₁ = 109mm, W₂ = 82mm, L₁ = 425mm, L₂ = 392, H₁ = 89mm, and H₂ = 95mm. Using the MATLAB function at the end of this lesson, we can find the configuration of the end-effector frame in the base frame as:

The visualization of this configuration is as follows (see the video version for the demo):

A question arises here: will we get the same configuration if the base frame is different?

To answer this question, we will use RoboDK to show that we’ll get the same configuration as above by selecting a different base frame (see the video version for the demo). We begin by selecting the following reference frame as the base frame:

Using the same robot joint angles, the matrix representing the configuration of the end-effector frame relative to the base frame can be calculated as follows:

This matrix gives the same configuration as the previous base frame (you can also confirm this by drawing both frames by hand):

This example shows that the choice of the base frame is arbitrary when calculating the forward kinematics. Similarly, the end-effector frame can also be chosen arbitrarily as long as it follows the right-hand rule. Typically, frames are chosen with their z-axis aligned in the direction of the joint’s positive movement.

Here, it’s noteworthy that 6-dof robotic arms are commonly referred to as general purple manipulators, and they are important for robotics and industrial applications since they have a minimum number of joints, allowing the end-effector to move a rigid body in all its degrees of freedom subject to only limits on the robot’s workspace (do you remember from the degrees of freedom lesson that a rigid body in space has 6 degrees of freedom?)

Let’s take a look at an example in which a prismatic joint is used.

Example: Forward Kinematics of KUKA KR5 SCARA R550 Z200

Consider the RRPR KUKA SCARA robot depicted in the figure below:

As shown in the video version, this robot has three rotational degrees of freedom and one translational degree of freedom, which makes it a useful pick-and-place robot. The set of joint variables are q = (θ₁,θ₂,d₃,θ₄).

From the robot’s technical specs, we can calculate ℓ₁ = 325mm, ℓ₂ = 225mm, and ℓ₀ = 46mm. Now following the steps to calculate the forward kinematics of this robot using screw theory, we can write the matrix M and the screw axes as:

Note that since one of the joints (3rd joint) is prismatic, the linear part of the screw axis is the unit vector representing the linear velocity when the linear speed is one. The direction of this linear velocity is in the z-axis direction of the base frame.

For educational purposes and using our MATLAB code, the configuration of the end-effector relative to the base frame for the set of joint variables q = (0,π/2,10mm,-π/2) can be found as:

This configuration can be visualized as the following figure (see the video version for the demo):

The following code helps you to calculate the forward kinematics using the PoE formula:

function [R,p]=FK_PoE(theta,a,vel,jt,M)
    % this code caclulate the forward kinematics using the product of
    % exponentials PoE formula
    
    % function inputs:
    % theta: vector of joint angles
    % a: matrix of 3D arbitrary points on the joint axes at zero configuration
        % written in coordinates of the fixed base frame where each row belongs
        % to one point
    % vel: matrix of 3D joint actuation velocities at zero configuration, where each
    % row belongs to one joint and each row is a unit vector. For rotational
    % joints this is rotational velocity and for prismatic joints, this is
    % linear velocity
    % jt: a string of letters showing joint types, e.g. 'RRP'
    % M: Homogenous transformation matrix of end-effector w.r.t. base at zero configuration
    
    % outputs:
    % R is the rotation matrix of the end-effector w.r.t. base
    % p is the position of the end-effector w.r.t. base 
    %-------------------------------------------------------------------------    
    T=POE(theta,a,vel,jt);
    Tf=T*M;
    R=Tf(1:3,1:3);
    p=Tf(1:3,4);
end
% utility function ----------------------------------------------------
function T=POE(theta,a,vel,jt)
    T=eye(4,4);
    % number of joints
    n=length(theta);
    for ii=n:-1:1
        if jt(ii)=='R'
            vel_hat=[0, -vel(ii,3), vel(ii,2);...
                        vel(ii,3), 0, -vel(ii,1);...
                        -vel(ii,2), vel(ii,1), 0];
            e_vel_hat=eye(3,3)+vel_hat*sin(theta(ii))+vel_hat*vel_hat*(1-cos(theta(ii)));
        elseif jt(ii)=='P'
            vel_hat=zeros(3,3);
            e_vel_hat=eye(3,3);
        end
        if (jt(ii)=='R') & (ii>1)
            Sv=-transpose(cross(vel(ii,:),a(ii,:)));
        elseif (jt(ii)=='R') & (ii==1)
            Sv=[0;0;0];
        elseif jt(ii)=='P'
            Sv=vel(ii,:)';
        end
        t=(eye(3,3)*theta(ii)+(1-cos(theta(ii)))*vel_hat+(theta(ii)-sin(theta(ii)))*vel_hat*vel_hat)*Sv;
        e_zai=[e_vel_hat t;0 0 0 1];
        T=e_zai*T;
    end
end

For instance, for the UR5e forward kinematics at the given joint angles above, you can use the code as follows:

t1 = 0;
t2 = -pi/2;
t3 = 0;
t4 = 0;
t5 = pi/2;
t6 = 0;
theta = [t1;t2;t3;t4;t5;t6];
W1 = 109/1000;
W2 = 82/1000;
L1 = 425/1000;
L2 = 392/1000;
H1 = 89/1000;
H2 = 95/1000;
a = [0 0 0;
     0 0 H1;
     -L1 0 H1;
     -L1-L2 0 H1;
     -L1-L2 -W1 0;
     -L1-L2 0 H1-H2];
vel = [0 0 1;
       0 -1 0;
       0 -1 0;
       0 -1 0;
       0 0 -1;
       0 -1 0];
jt = 'RRRRRR';
M = [1 0 0 -L1-L2;
     0 0 -1 -W1-W2;
     0 1 0 H1-H2;
     0 0 0 1];
[R,p]=FK_PoE(theta,a,vel,jt,M)

That’s going to do it for today’s lesson. We hope that by now, you have a good understanding of calculating forward kinematics using screw theory for open-chain manipulators.

References:

• Modern Robotics: Mechanics, Planning, and Control by Frank Park and Kevin Lynch
• A Mathematical Introduction to Robotic Manipulation by Murray, Lee, and Sastry
• Denavit, Jacques, and Richard S. Hartenberg. “A kinematic notation for lower-pair mechanisms based on matrices.” (1955): 215-221.
• Brockett, R.W., 1984. Robotic manipulators and the product of exponentials formula. In Mathematical theory of networks and systems (pp. 120-129). Springer, Berlin, Heidelberg.
• RoboDK software