Inverse Kinematics

Inverse Kinematics

Inverse kinematics are used when solving for the joint variables in terms of the position of the given end effector position and orientation.


Some of the applications of inverse kinematics are relevant to game programming and 3D animation, where a common use is making sure game characters connect physically to the world, such as feet landing firmly on top of terrain.[1]

Solution Types[2]

Closed Form Solution
-analytical solution, nonlinear equations
-fixed number of steps
-in the past this was the only way
-real time control
-returns all possible solutions
Note: Pieper's Theorem: Closed-form solution always exists for any 6 dof robot with a spherical wrist.

Numerical Solution
-iterative process, requires more processing power
-Newton-Raphson method
-single solution return
-can be applied to all robots

Solved Closed Form Solution Process [2]

The general inverse kinematics solution applies to robot manipulators with up to 6 degrees of freedom. Setup the inverse process by breaking the robot links into arm/positioning parts and the wrist/orienting parts. The arm/positioning part is used to determine the first three joint parameters of the robot manipulator (q1, q2, and q3), and positions its wrist center. The wrist/orienting part is used to determine the last three joint parameters of the robot manipulator (q4, q5, and q6), if applicable. The division of the closed-form solution is used to meet the required position and orientation.

1) Start with the given tool pose To6. With the the top left 3x3 is the orientation matrix and the 4th column is the position.

\begin{align} \left [ \begin{array}{cccc} r11 & r12 & r13 & xp \\ r21 & r22 & r23 & yp \\ r31 & r32 & r33 & zp \\ 0 & 0 & 0 & 1 \end{array} \right ] \end{align}

2) Solve forward kinematics for To3 and R36. Normally R36 is a spherical wrist, so it controls the orientation of the end effector. To3 will work on satisfying the position requirement.
Forward Kinematics

3) Find the location of the wrist center, pc

\begin{align} pc = \bigl(\begin{smallmatrix} x' \\ y' \\ z' \end{smallmatrix}\bigr) = \bigl(\begin{smallmatrix} x \\ y \\ z \end{smallmatrix}\bigr) - z6*d6 \end{align}


\begin{align} pc = \bigl(\begin{smallmatrix} x' \\ y' \\ z' \end{smallmatrix}\bigr) = \bigl(\begin{smallmatrix} x \\ y \\ z \end{smallmatrix}\bigr) - \bigl(\begin{smallmatrix} r13 \\ r23 \\ r33 \end{smallmatrix}\bigr) \end{align}

4) Set pc equal to the last column of To3 from the forward kinematics. Solve these equations for the 1st 3 joint parameters.

5) From the forward kinematics Ro3 and Ro6
Get R36 (given) = Ro3 (transpose) * Ro6
remember that Ro6 = Ro3 * R36

6) Solve for the final joint parameters by setting the orientation matrix equal to R36.
remember to keep track of the number of solutions


Continuing the example from the forward kinematics.

First define To2 as the arm and T23 as the wrist.

Looking at the last column of To2

\begin{bmatrix} s1d2 \\ -c1d2 \\ d1 \end{bmatrix} = \begin{bmatrix} x \\ y \\ z \end{bmatrix} -d3* \begin{bmatrix} r13 \\ r23 \\ r33 \end{bmatrix} = \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix}

Now solve for $\theta$1 and d2. Start by squaring and adding row 1 and 2. This will give 2 solutions for d2.

\begin{equation} s1^{2}d2^{2}+c1^{2}d2^{2}=x'^{2}+y'^{2} \end{equation}
\begin{equation} d2^{2}=x'^{2}+y'^{2} \end{equation}
\begin{align} d2=\frac{+}{-} \sqrt[]{x'^{2}+y'^{2}} \end{align}

(2 solutions)

\begin{equation} theta1=atan2(x',-y') \end{equation}

(1 solution)

Now to solve for the wrist
from given

\begin{align} R^0_2'*R^0_3= \begin{bmatrix} r11' & r12' & r13' \\ r21' & r22' & r23' \\ r31' & r32' & r33' \end{bmatrix}\\ \end{align}

from kinematics

\begin{align} R^2_3= \begin{bmatrix} c3 & -s3 & 0 \\ s3 & c3 & 0 \\ 0 & 0 & 1 \end{bmatrix}\\ \end{align}

Set equal and solve for theta3

\begin{equation} theta3=atan2(r21',r22') \end{equation}

(1 solution)

This gives a total of 2 solutions.

1. Inverse Kinematics Wikipedia <>
2. Canfield, Stephen. Lecture notes: “Inverse Kinematics” Fall 2009
Add a New Comment
Unless otherwise stated, the content of this page is licensed under Creative Commons Attribution-ShareAlike 3.0 License