In the previous two tutorials we examined methods of forward kinematics for a six axis robot model where we computed the real world co-ordinates of the tool tip for specified joint angles. In this tutorial we consider the problem in the opposite direction, that is we know the intended co-ordinates and orientation of the tool tip and from this information **compute the joint angles**. This procedure is called **inverse kinematics**.

Unlike forward kinematics, there is no generic solution for inverse kinematics that can be applied to all configurations of robot manipulators. Text books generally state three categories of solutions:

- geometric
- analytic
- numerical

**Geometric solutions** are straightforward where there are a limited number of links. We present an example using our robot model to give a complete solution for a limited range of positions of the tool tip.

**Analytic solutions** exist in varying degrees of complexity, depending on the number of joints but there is no single generic solution for a six axis robot. We will outline a method using transformation matrices that is valid under certain conditions.

**Numerical solutions** apply iterative mathematical processes requiring complex software and are beyond the scope of these tutorials.

#### Six axis robot model

Figs.1(a) and (b) below shows the robot model with six revolute joints used in this tutorial. This model is similar to one used in previous tutorials for forward kinematics, but in this model there are no offsets between joints. Also, the rotational axes of joints 4, 5 and 6 intersect to provide a **spherical wrist** configuration for these joints. This arrangement simplifies the analytical method for inverse kinematics presented below.

X_{0}, Y_{0} and Z_{0} are the real world co-ordinates of the robot base. The datum pose for the robot is a horizontal extension aligned with the X_{0} axis. All co-ordinate frames and rotations of joints are in accordance with the right hand rule. Thus counter-clockwise rotations are positive and clockwise rotations negative

#### Geometric solution for inverse kinematics

The basis for this example, originally drawn in CAD, is the pose shown above in Fig.2. The known co-ordinates of the origin of frame 5 relative to the origin of the base frame* are (in unspecified length units):

- x
_{5}= 695 - y
_{5}= 781 - z
_{5}= - 401

**In previous tutorials these real world co-ordinates would be designated by a twin suffix x _{50}, y_{50}, z_{50}. In this application we omit the 0 suffix for simplicity.*

The tool tip is 110 units vertically below the origin of frame 5. In this configuration joint 4 and joint 6 do not rotate. Consequently joint angle θ_{5} is determined by joint angles θ_{2} and θ_{3}. In effect, this configuration has three degrees of freedom.

Figs.3(a) and 3(b) below illustrate the geometry of the robot in the specified configuration. Note that the "elbow" at joint 3 in Fig.3(b) is in the up position. The inverse kinematic problem is to find joint angles θ_{1}, θ_{2}, θ_{3} and θ_{5}.

Directions of rotation of joints 2, 3 and 5 illustrated in Fig.3(b) are determined from the horizontal datum position.

**To find θ _{1}**

From Fig.3(a): θ_{1} = tan^{-1}(z_{5} / x_{5}) = tan^{-1}(401/ 695): gives **θ _{1} = 29.98°**. Rotation is +ve (counter-clockwise) with the Y

_{0}axis coming out the page on Fig.3(a).

**To find θ _{2}**

From Fig.3(a): θ_{2} = (φ + λ_{3})

Also from Fig.3(a): r = z_{5} / sin θ_{1} = (401) / sin (29.98°) = 802

From the robot model: a_{1} = 160

From Fig.3(b): tan^{-1}φ = tan^{-1}(y_{5} - a_{1}) / r) = tan^{-1}(621 / 802) gives: φ = 37.74°

Also: (L_{1})^{2} = r^{2} + (y_{5} - a_{1})^{2} = (802)^{2} + (781 - 160)^{2}

gives L_{1} = √[(802)^{2} + (621)^{2} ] = 1014

From the robot model: L_{2} = 400, L_{3} = 630

Using the law of cosines for triangle with sides L_{1}, L_{2}, L_{3} and internal angles λ_{1}, λ_{2}, λ_{3} gives:

(L_{3})^{2} = (L_{1})^{2} + (L_{2})^{2} - 2L_{1}.L_{2}.cos λ_{3}

Thus θ_{2} = (φ + λ_{3}) = (37.74° + 12.47°)

gives **θ _{2} = 50.21°** Rotation is +ve (counter-clockwise) with the Z

_{0}axis coming out the page on Fig.3(b).

**To find θ _{3}**

Using the law of cosines for triangle with sides L_{1}, L_{2}, L_{3} and internal angles λ_{1}, λ_{2}, λ_{3} gives:

(L_{1})^{2} = (L_{2})^{2} + (L_{3})^{2} - 2L_{2}.L_{3}.cos λ_{1}

giving: θ_{3} = (180° - λ_{1} ) = (180° - 159.66°)

Thus **θ _{3} = - 20.34°** Rotation is -ve (clockwise) with the Z

_{0}axis coming out the page on Fig.3(b)

**To find θ _{5}**

From Fig.3(b): θ_{5} = (90° + β)

Also from Fig.3(b) β = (θ_{2} - θ_{3}) = (50.21° - 20.34°) = 29.87°

Giving θ_{5} = (90° + 29.87°)

Thus **θ _{5} = 119.87°** Rotation is -ve (clockwise) with the Z

_{0}axis coming out the page on Fig.3(b).

**Alternative solution for θ _{2}, θ_{3}, and θ_{5}**

The above solution for joint angles θ_{2}, θ_{3}, and θ_{5} is not unique. Fig.3(c) below illustrates another pose providing the same tool tip co-ordinates where elbow joint 3 points down.

In the elbow down configuration angles are computed as follows (elbow up configuration in brackets).

**θ _{2} = 25.27° +ve ** (50.21° +ve)

**θ _{3} = 20.34° +ve ** (20.34° -ve)

**θ _{5} = 119.87° -ve ** (135.61° -ve)

**Consideration of offsets**

Other variable aspects of the robot configuration, such as offsets between links, introduce multiple inverse kinematic solutions. The robot model in the examples above has no offsets but consider Fig.4 below where links between joints are offset.

When revolute joint 1 rotates towards the mid-way angular target point from opposite directions with clockwise or counter-clockwise rotations the joint rotations θ_{1} are not the same. As a consequence inverse kinematics must define the direction of rotation, normally stated as right hand (counter-clockwise) or left hand (clockwise). In this example rotations to a mid-way target point from starting points 90° apart are θ_{1(RH)} = 53° and θ_{1(LH)} = 37°

#### Analytic solution for inverse kinematics

The analytic method described below considers an inverse kinematics solution for a six axis robot in two parts.

- Find a geometric solution for joint angles θ
_{1}, θ_{2}and θ_{3}. For this example we apply the results from the geometric method outlined above for θ_{1}, θ_{2}and θ_{3}. We then derive the rotational transformation matrix R^{0}_{3}(see previous tutorial) using these values. - The second part requires joints 4, 5 and 6 to be in a spherical wrist configuration (see above). Because of this, rotation matrix R
^{3}_{6}alone defines transformations from co-ordinate frame 6 to co-ordinate frame 3. The elements of R^{3}_{6}are expressed in terms of sine and cosine functions of joint angles θ_{4}, θ_{5}and θ_{6}. Note that the pose of joints 4, 5, and 6 in this example are not the same as applied in the previous example,specifically joints 4 and 6 are rotated.

We also specify the orientation of the co-ordinate frame for joint 6 in the specified pose which determines the rotational transformation matrix R^{0}_{6}from frame 6 to the base frame.

The method is based on the following clever piece of linear algebra:

R^{0}_{6}= R^{0}_{3}• R^{3}_{6}

multiply both sides by (R^{0}_{3})^{-1}(the inverse matrix of R^{0}_{3})

gives (R^{0}_{3})^{-1}• R^{0}_{6}= (R^{0}_{3})^{-1}• R^{0}_{3}• R^{3}_{6}

and as (R^{0}_{3})^{-1}• R^{0}_{3}= I (the identity matrix) it follows:

**R**.^{3}_{6}= (R^{0}_{3})^{-1}• R^{0}_{6}

This relationship is the basis of the procedure outlined below to resolve joint angles θ_{4}, θ_{5}and θ_{6}where we compute**(R**and use the equivalence with known matrix^{0}_{3})^{-1}• R^{0}_{6}**R**to solve for θ^{3}_{6}_{4}, θ_{5}and θ_{6}.

**Worked example of analytic method**

We will work through this procedure using joint angles θ_{1}, θ_{2} and θ_{3} from the previous worked example (elbow up configuration). For convenience joint angles are rounded to:

- θ
_{1}= 30.0° (+ve) - θ
_{2}= 50.0° (+ve) - θ
_{3}= 20.0° (-ve)

*Note that joint angles θ _{4}, θ_{5} and θ_{6} are different from the joint angles applied in the previous example.*

**Step 1 - derive rotation matrix R ^{0}_{3}**

R^{0}_{1} is rotation of +30° about the Y axis; R^{1}_{2} is rotation of +50° about the Z axis; R^{2}_{3} is rotation of -20° about the Z axis;

The inverse of a rotation matrix is its transpose, thus:

**Step 2 - derive rotation matrix R ^{0}_{6}**

Fig.5 below shows the horizontal datum position for the robot model as shown in Fig.1(b). Co-ordinates X_{0}, Y_{0}, Z_{0} represent the real world co-ordinate frame of the robot base.

Fig.6 below shows the specified tool tip pose with the co-ordinate system for frame 6. For simplicity the transformation of frame 6 is a single (clockwise) rotation of -90° about the Y_{0} axis. Fig.7 below provides a more detailed view of the tool tip at the extremity of joint 6 in datum and specified pose conditions.

In a previous tutorial we found the rotational transformation matrix R_{y(θ)} about the Y axis to be:

which for θ = -90° gives:

**Step 3 - compute rotation matrix R ^{3}_{6}**

**Step 4 - derive rotation matrix R ^{3}_{6} from first principles**

Firstly, note that:

- R
^{3}_{4}is rotation of θ_{4}about the X axis - R
^{4}_{5}is rotation of θ_{5}about the Z axis - R
^{5}_{6}is rotation of θ_{6}about the X axis

Using rotational transformation matrices R_{x(θ)}, R_{y(θ)} R_{z(θ)}

This operation is not entirely straightforward since multiple solutions are possible for each element.

First consider the element r_{11} (row 1, column 1).

Gives: cosθ_{5} = - 0.433

There are two possible solutions: θ_{5} = (i) cos^{-1}(-0.433) = 115.7° or (ii) cos^{-1}(-0.433) = -115.7°

Choose **θ _{5} = -115.7°** giving sinθ

_{5}= - 0.901

Next consider element r_{12 (row 1, column 2)}

Gives: - (sinθ_{5}.cosθ_{6}) = 0.5

Giving cosθ_{6} = - 0.5 / (- 0.901) = 0.555

Thus θ_{6} = (i) cos^{-1}(0.555) = 56.3° or (ii) cos^{-1}(0.555) = - 56.3°

Choose **θ _{6} = 56.3°** giving sinθ

_{6}= 0.832

As a check compute element r_{13} (row1, column 3)

sinθ_{5}.sinθ_{6} = (- 0.901).(0.832) = - 0.75 (correct)

θ_{6} = - 56.3° would be incorrect as sin( - 56.3°) is -ve, making r_{13} = +0.75

Next consider element r_{21} (row 2, column 1) = cosθ_{4}.sinθ_{5}

Gives: cosθ_{4} = 0.25 / ( - 0.901) = - 0.277

Thus θ_{4} = cos^{-1}(- 0.277) = (i) 106.1° or (ii) cos^{-1}(- 0.277) = - 106.1°

Choose **θ _{4} = - 106.1°** giving sinθ

_{4}= - 0.961

As a check compute element r_{31} (row 3, column 1)

sinθ_{4}.sinθ_{5} = (- 0.961) (- 0.901) = 0.866 (correct)

θ_{4} = 106.1° would be incorrect as sin(106.1°) is +ve, making r_{31} = -0.866

Thus the complete solution for rotation angles to effect the specified robot pose is:

θ_{1} |
+30.0° |

θ_{2} |
+50.0° |

θ_{3} |
-20.0° |

θ_{4} |
+106.1° |

θ_{5} |
-115.7° |

θ_{6} |
+56.3° |

*Calculations of matrix elements r _{22}, r_{23}, r_{32} and r_{33} provide additional checks to validate this solution.*

It can be shown that a valid solution also occurs with θ_{5} = + 115.7°

the resulting joint angles are:

θ_{4} |
+73.9° |

θ_{5} |
+115.7° |

θ_{6} |
-123.7° |

This completes the short series of tutorials on kinematics of robot manipulators.

Reurn to: Robotics content

I welcome feedback at: