Forward Kinematics Problem

# Forward Kinematics

In the forward kinematics problem, the transformation describing the position and orientation of a tool, or end effector, is determined by known joint variables. Joint variables are associated with a particular axis, or joint, and are denoted by qi. The two common types of joints used in robot manipulators are revolute and prismatic joints. For a revolute joint, qi is the angle of rotation ($\Theta$i), while for a prismatic joint, qi is the joint displacement (di).

## The Denavit-Hartenberg Representation

The Denavit-Hartenberg convention (DH convention) is commonly used to define reference frames for robotic serial arms. This convention is based on two assumptions:

1. DH1 : The z-axis will lie along the joint. According to Spong, axis zi-1 lies along joint i. For example, the axis x1 is perpendicular to the axis zo .
2. DH2 : The x-axes lie along the common normals between the joint axes. According to Spong, axis zi-1 lies along joint i. For example, the axis x 1 intersects the axis z0 .

Link i $\alpha$i a i d i $\Theta$ i
1 Angle between zo and z1 measured about x1 Distance from zo to z1 measured along x1 Distance from xo to x1 measured along zo Angle between xo and x1 measured about zo
2 Angle between z1 and z2 measured about x2 Distance from z1 to z2 measured along x2 Distance from x1 to x2 measured along z1 Angle between x1 and x2 measured about z1
n Angle between zn and zn+1 measured about xn+1 Distance from zn to zn+1 measured along xn+1 Distance from xn to xn+1 measured along zn Angle between xn and xn+1 measured about zn

A reference frame must first be established for the manipulator. Frames are also assigned for the remaining joints of the manipulator and the end effector. Each frame i moves with link i and is attached to the end of the link. It is necessary to interpret the locations of the frame origins, because they do not always lie at the middle of the joint. To perform a transformation from frame i-1 to frame i:

1. Rotate about axis zi-1 by $\theta$i
2. Translate along axis zi-1 by di
3. Translate along axis xi by ai
4. Rotate about axis xi by $\alpha$i

Figure 1: Robot Manipulator

In the execution the transformation, the step order is important for rotation, but not for translation. The above four steps can be summarized by the following notation:
A = Rotz,$\theta$ Transz,d Transx,a Rotx,$\alpha$

From the above equation, A can be written in matrix form as

(1)
\begin{align} A = \left[ { \begin{array}{cc} R _{\rm 1}^{0} & o _{\rm 1}^{0}\\ 0 & 1\\ \end{array} } \right] \end{align}

where R1o is the orientation frame 1 with respect to frame 0 (the base frame), and o1o is the position of frame 1 with respect to the base frame. The matrix A can be rewritten using the corresponding transformations for the rotations and translations of the frame.

(2)
\begin{align} A _{\rm i} = \left[ { \begin{array}{cccc} c \theta _{\rm i} & -s \theta _{\rm i} & 0 & 0\\ s \theta _{\rm i} & c \theta _{\rm i} & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \left[ { \begin{array}{cccc} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & d _{\rm i}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \left[ { \begin{array}{cccc} 1 & 0 & 0 & a _{\rm i}\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \left[ { \begin{array}{cccc} 1 & 0 & 0 & 0\\ 0 & c \alpha _{\rm i} & -s \alpha _{\rm i} & 0\\ 0 & s \alpha _{\rm i} & c \alpha _{\rm i} & 0\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}

The most common representation of the forward kinematics problem is based on the D&H convention and is given below,

(3)
\begin{align} T _{\rm i}^{i-1} = \left[ { \begin{array}{cccc} c \theta _{\rm i} & -s \theta _{\rm i} c \alpha _{\rm i} & s \theta _{\rm i} s \alpha _{\rm i} & a _{\rm i} c \theta _{\rm i}\\ s \theta _{\rm i} & c \theta _{\rm i} c \alpha _{\rm i} & -c \theta _{\rm i} s \alpha _{\rm i} & a _{\rm i} s \theta _{\rm i}\\ 0 & s \alpha _{\rm i} & c \alpha _{\rm i} & d _{\rm i}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}

with D&H parameters assigned according to the Spong et. al. convention (frame i on joint i-1). The transformation Tii-1 is the product of the rotation and translation matrices of matrix Ai. The upper left 3x3 matrix of the transformation corresponds to the orientation of the end effector with respect to the base frame. The three columns of this matrix give the direction of the x-, y-, and z-axes, from left to right. The 3x1 matrix in the upper right corner shows the location of the end effector with respect to the base frame.

## Summary of Deriving Robot Forward Kinematics

The general procedure for deriving the forward kinematics of a serial robot manipulator is given in the following steps:

1. Label all joints i = 1 to n.
2. Assign z-axes for joints 0 to n-1 (zo along joint 1, etc.).
3. Assign xo normal to zo.
4. Assign x1 through xn-1, which lie at the common normals between zo to zn-1.
5. Establish y1 to yn-1 to complete each frame.
6. Assign zn freely (but carefully) and define xn.
7. Create the table of DH link parameters, defining $\alpha$i, ai, di, and $\theta$i for each joint.
8. Create Tii-1 for i = 1 to n.
9. Solve Tno = T1o * T21 * … * Tnn-1.
10. Show the position as the last column of Tno and the orientation as the first three columns of Tno.

## Forward Kinematics Example

Figure 2: Robot Manipulator

For a revolute joint, the joint is the axis of rotation. For a prismatic joint, the joint is on the motion path of the joint.

Figure 3: Robot Manipulator with Joint and Frame Assignments

### DH Table

Link i $\alpha$i a i d i $\Theta$i Ti i-1
1 90 0 d1 $\Theta$ 1* T1o
2 0 0 d2* 0 T21
3 0 0 d3 $\Theta$ 3* T32

Note: * denotes variable joint parameters. For each link, only one unknown joint variable can exist for each joint axis.

### Transformations

The resulting transformation matrices for the manipulator are given below:

(4)
\begin{align} T _{\rm 1}^{0} = \left[ { \begin{array}{cccc} c _{\rm 1} & 0 & s _{\rm 1} & 0\\ s _{\rm 1} & 0 & -c _{\rm 1} & 0\\ 0 & 1 & 0 & d _{\rm 1}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}
(5)
\begin{align} T _{\rm 2}^{0} = T _{\rm 1}^{0} T _{\rm 2}^{1} \end{align}
(6)
\begin{align} T _{\rm 2}^{0} = \left[ { \begin{array}{cccc} c _{\rm 1} & 0 & s _{\rm 1} & 0\\ s _{\rm 1} & 0 & -c _{\rm 1} & 0\\ 0 & 1 & 0 & d _{\rm 1}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \left[ { \begin{array}{cccc} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & d _{\rm 2}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] = \left[ { \begin{array}{cccc} c _{\rm 1} & 0 & s _{\rm 1} & s _{\rm 1} d _{\rm 2}\\ s _{\rm 1} & 0 & -c _{\rm 1} & -c _{\rm 1} d _{\rm 2}\\ 0 & 1 & 0 & d _{\rm 1}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}
(7)
\begin{align} T _{\rm 3}^{0} = T _{\rm 1}^{0} T _{\rm 2}^{1} T _{\rm 3}^{2} = T _{\rm 2}^{0} T _{\rm 3}^{2} \end{align}
(8)
\begin{align} T _{\rm 3}^{0} = \left[ { \begin{array}{cccc} c _{\rm 1} & 0 & s _{\rm 1} & s _{\rm 1} d _{\rm 2}\\ s _{\rm 1} & 0 & -c _{\rm 1} & -c _{\rm 1} d _{\rm 2}\\ 0 & 1 & 0 & d _{\rm 1}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \left[ { \begin{array}{cccc} c _{\rm 3} & -s _{\rm 3} & 0 & 0\\ s _{\rm 3} & c _{\rm 3} & 0 & 0\\ 0 & 0 & 1 & d _{\rm 3}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}
(9)
\begin{align} T _{\rm 3}^{0} = \left[ { \begin{array}{cccc} c _{\rm 1} c _{\rm 3} & -c _{\rm 1} s _{\rm 3} & s _{\rm 1} & s _{\rm 1} (d _{\rm 2} + d _{\rm 3})\\ s _{\rm 1} c _{\rm 3} & -s _{\rm 1} s _{\rm 3} & -c _{\rm 1} & -c _{\rm 1} (d _{\rm 2} + d _{\rm 3})\\ s _{\rm 3} & c _{\rm 3} & 0 & d _{\rm 1}\\ 0 & 0 & 0 & 1\\ \end{array} } \right] \end{align}

The rotation matrix (first 3 columns of T3o) is given as

(10)
\begin{align} T _{\rm 3}^{0} = \left[ { \begin{array}{ccc} c _{\rm 1} c _{\rm 3} & -c _{\rm 1} s _{\rm 3} & s _{\rm 1}\\ s _{\rm 1} c _{\rm 3} & -s _{\rm 1} s _{\rm 3} & -c _{\rm 1}\\ s _{\rm 3} & c _{\rm 3} & 0\\ \end{array} } \right] \end{align}

while the translation matrix (4th column) is

(11)
\begin{align} T _{\rm 3}^{0} = \left[ { \begin{array}{ccc} s _{\rm 1} (d _{\rm 2} + d _{\rm 3})\\ -c _{\rm 1} (d _{\rm 2} + d _{\rm 3})\\ d _{\rm 1}\\ \end{array} } \right] \end{align}

MATLAB can also be used to determine transformations by a user-defined symbolic toolbox. This symbolic toolbox contains the variable parameters for the robot manipulator. A MATLAB code for Example 1 is available below. The code can be modified and expanded to determine transformations of serial arms with six degrees of freedom. (See Serial Arm Manipulator Analysis for an analysis of a 6 DOF serial manipulator.)

#### MATLAB Code: Transformation

%Define symbolic variables
syms theta1 theta3 l2 d1 d2 d3

%Define transformations:
T1_0 = [cos(theta1), 0, sin(theta1), 0;
sin(theta1), 0, -cos(theta1), 0;
0, 1, 0, d1;
0, 0, 0, 1];

T2_1 = [1, 0, 0, 0;
0, 1, 0, 0;
0, 0, 1, d2;
0, 0, 0, 1];

T3_2 = [cos(theta3), -sin(theta3), 0, 0;
sin(theta3), cos(theta3), 0, 0;
0, 0, 1, d3;
0, 0, 0, 1];

%Determine transformation
T2_0 = T1_0 * T2_1;
T2_0 = simple(T2_0)

T3_0 = T2_0*T3_2;
T3_0 = simple(T3_0)

After executing the program, the output should be the following:

T2_0 =

[ cos(theta1), 0, sin(theta1), sin(theta1)*d2]
[ sin(theta1), 0, -cos(theta1), -cos(theta1)*d2]
[ 0, 1, 0, d1]
[ 0, 0, 0, 1]

T3_0 =

[ cos(theta1)*cos(theta3), -cos(theta1)*sin(theta3), sin(theta1), sin(theta1)*(d3+d2)]
[ sin(theta1)*cos(theta3), -sin(theta1)*sin(theta3), -cos(theta1), -cos(theta1)*(d3+d2)]
[ sin(theta3), cos(theta3), 0, d1]
[ 0, 0, 0, 1]

## References

Bibliography
1. Spong, et.al. Robot Modeling and Control.