# Robotics Kinematics and Dynamics/Serial Manipulator Position Kinematics

## Forward Position Kinematics

The forward position kinematics problem can be stated as follows: given the different joint angles, what is the position of the end-effector? With the previous sections in mind, the answer is rather simple: construct the different transformation matrices and combine them in the right way, the result being ${\displaystyle _{bs}^{ee}T\,}$, where ${\displaystyle \{bs\}}$ is the base frame of the robot manipulator.

### Solution

Suppose the mutual orientation matrices between adjacent links are known. (As the fixed parameters of each link are known, and the joint angles are a given to the problem, these can be calculated. One possible way to do this would be to make use of the Denavit-Hartenberg convention.) The transformation that relates the last and first frames in a serial manipulator arm, and thus, the solution to the forward kinematics problem, is then represented by the compound homogeneous transformation matrix. The axes are moving, thus, the compound homogeneous transformation matrix is found by premultiplying the individual transformation matrices:

${\displaystyle _{bs}^{ee}T=\,_{0}^{N}T=\,_{0}^{1}T\,_{1}^{2}T\ldots \,_{N-2}^{N-1}T\,_{N-1}^{N}T}$

### Examples

The equations below use 3 × 3 pose matrices, as this is just a 2-dimensional case (cf. the figure on the right).

The pose of the first link, relative to the reference frame, is given by (recall the elementary rotation about the z-axis from the previous section):

${\displaystyle T_{1}(\theta _{1})={\begin{pmatrix}\cos(\theta _{1})&-\sin(\theta _{1})&0\\\sin(\theta _{1})&\cos(\theta _{1})&0\\0&0&1\end{pmatrix}}}$

The pose of the second link, relative to the first link, is given by:

${\displaystyle T_{2}(\theta _{2})={\begin{pmatrix}\cos(\theta _{2})&-\sin(\theta _{2})&l_{1}\\\sin(\theta _{2})&\cos(\theta _{2})&0\\0&0&1\end{pmatrix}}}$

This corresponds to a rotation by an angle ${\displaystyle \theta _{2}}$ and a translation by a distance ${\displaystyle l_{1}}$, where ${\displaystyle l_{1}}$ is the length of the first link.

The pose of the third link, relative to the second link, is given by:

${\displaystyle T_{3}(\theta _{3})={\begin{pmatrix}\cos(\theta _{3})&-\sin(\theta _{3})&l_{2}\\\sin(\theta _{3})&\cos(\theta _{3})&0\\0&0&1\end{pmatrix}}}$

The pose of the end effector, relative to the third link, is given by:

${\displaystyle T_{4}={\begin{pmatrix}1&0&l_{3}\\0&1&0\\0&0&1\end{pmatrix}}}$

The solution to the forward kinematics problem is then:

${\displaystyle _{bs}^{ee}T=\,_{0}^{4}T(\theta _{1},\theta _{2},\theta _{3})={\begin{pmatrix}c_{123}&-s_{123}&l_{1}c_{1}+l_{2}c_{12}+l_{3}c_{123}\\s_{123}&c_{123}&l_{1}s_{1}+l_{2}s_{12}+l_{3}s_{123}\\0&0&1\end{pmatrix}}}$

Hence:

${\displaystyle {\begin{pmatrix}x\\y\\1\end{pmatrix}}=T{\begin{pmatrix}0\\0\\1\end{pmatrix}}}$

The resulting kinematic equations are:

${\displaystyle {\begin{array}{l}x=l_{1}\cos \theta _{1}+l_{2}\cos(\theta _{1}+\theta _{2})+l_{3}\cos(\theta _{1}+\theta _{2}+\theta _{3})\\y=l_{1}\sin \theta _{1}+l_{2}\sin(\theta _{1}+\theta _{2})+l_{3}\sin(\theta _{1}+\theta _{2}+\theta _{3})\end{array}}}$

## Inverse Position Kinematics

The inverse kinematics problem is the opposite of the forward kinematics problem and can be summarized as follows: given the desired position of the end effector, what combinations of the joint angles can be used to achieve this position?

Two types of solutions can be considered: a closed-form solution and a numerical solution. Closed-form or analytical solutions are sets of equations that fully describe the connection between the end-effector position and the joint angles. Numerical solutions are found through the use of numerical algorithms, and can exist even when no closed-form solution is available. There may also be multiple solutions, or no solution at all.

The inverse kinematics problem for this 2D manipulator can quite easily be solved algebraically.

From the earlier results (for simplicity, the displacement over the distance ${\displaystyle l_{3}}$ shall be omitted here):

${\displaystyle _{0}^{3}T={\begin{pmatrix}c_{123}&-s_{123}&0&l_{1}c_{1}+l_{2}c_{12}\\s_{123}&c_{123}&0&l_{1}s_{1}+l_{2}s_{12}\\0&0&1&0\\0&0&0&1\\\end{pmatrix}}}$

Now assume a given end-effector orientation in the following form:

${\displaystyle _{bs}^{ee}T={\begin{pmatrix}c_{\phi }&-s_{\phi }&0&x\\s_{\phi }&c_{\phi }&0&y\\0&0&1&0\\0&0&0&1\\\end{pmatrix}}}$

Equating the two previous expressions results in:

${\displaystyle {\begin{array}{lll}c_{\phi }&=&c_{123}\\s_{\phi }&=&s_{123}\\x&=&l_{1}c_{1}+l_{2}c_{12}\\y&=&l_{1}s_{1}+l_{2}s_{12}\\\end{array}}}$

As:

${\displaystyle {\begin{array}{lll}c_{12}&=&c_{1}c_{2}-s_{1}s_{2}\\s_{12}&=&c_{1}s_{2}+s_{1}c_{2}\\\end{array}}}$,

squaring both the expressions for ${\displaystyle x}$ and ${\displaystyle y}$ and adding them, leads to:

${\displaystyle x^{2}+y^{2}=l_{1}^{2}+l_{2}^{2}+2l_{1}l_{2}c_{2}}$

Solving for ${\displaystyle c_{2}}$ leads to:

${\displaystyle c_{2}={\frac {x^{2}+y^{2}-l_{1}^{2}-l_{2}^{2}}{2l_{1}l_{2}}}}$,

while ${\displaystyle s_{2}}$ equals:

${\displaystyle s_{2}=\pm {\sqrt {1-c_{2}^{2}}}}$,

and, finally, ${\displaystyle \theta _{2}}$:

${\displaystyle \theta _{2}={\mbox{Atan2}}(s_{2},c_{2})}$

Note: The choice of the sign for ${\displaystyle s_{2}}$ corresponds with one of the two solutions in the figure above.

The expressions for ${\displaystyle x}$ and ${\displaystyle y}$ may now be solved for ${\displaystyle \theta _{1}}$. In order to do so, write them like this:

${\displaystyle {\begin{array}{lll}x&=&k_{1}c_{1}-k_{2}s_{1}\\y&=&k_{1}s_{1}+k_{2}c_{1}\\\end{array}}}$

where ${\displaystyle k_{1}=l_{1}+l_{2}c_{2}}$, and ${\displaystyle k_{2}=l_{2}s_{2}}$.

Let:

${\displaystyle {\begin{array}{lll}r&=&{\sqrt {k_{1}^{2}+k_{2}^{2}}}\\\gamma &=&{\mbox{Atan2}}(k2,k1)\\\end{array}}}$

Then:

${\displaystyle {\begin{array}{lll}k_{1}&=&r\cos \gamma \\k_{2}&=&r\sin \gamma \\\end{array}}}$

Applying these to the above equations for ${\displaystyle x}$ and ${\displaystyle y}$:

${\displaystyle {\begin{array}{lll}x/r&=&\cos \gamma \,c_{1}+\sin \gamma \,s_{1}\\y/r&=&\cos \gamma \,s_{1}+\sin \gamma \,c_{1}\\\end{array}}}$,

or:

${\displaystyle {\begin{array}{lll}\cos(\gamma +\theta _{1})&=&{\frac {x}{r}}\\\sin(\gamma +\theta _{1})&=&{\frac {y}{r}}\\\end{array}}}$

Thus:

${\displaystyle \gamma +\theta _{1}={\mbox{Atan2}}(y,x)}$

Hence:

${\displaystyle \theta _{1}={\mbox{Atan2}}(y,x)-{\mbox{Atan2}}(k_{2},k_{1})}$

Note: If ${\displaystyle x=y=0}$, ${\displaystyle \theta _{1}}$ actually becomes arbitrary.

${\displaystyle \theta _{3}}$ may now be solved from the first two equations for ${\displaystyle s_{\phi }}$ and ${\displaystyle c_{\phi }}$:

${\displaystyle \theta _{3}=\phi -\theta _{1}-\theta _{2}={\mbox{Atan2}}(s_{\phi },c_{\phi })-\theta _{1}-\theta _{2}}$