# The six degrees of freedom manipulator forward kinematics and the geometric solution of inverse kinematics

This article describes forward kinematics of the six degrees of freedom manipulator and its geometric inverse kinematics geometric.
The algorithm described in this article has not been strictly tested. Take your own risk while using it

The robot structure is shown in the figure below:

## Forward Kinematics

The general solution for the forward kinematics of the manipulator can be found Here

#### The initial state of the robot

Suppose the initial state of the manipulator that all nodes are shown on the same line as shown in following figure.

And we define all the joints to rotate only around the Z axis or the Y axis, so that this initial state all the rotation matrix ${\mathbf{B}}_{i}$ is all unit matrix.

#### Forward Kinematic Solution

Define the base axis ${\mathbf{P}}_{0}$; The length of the base to the first joint is ${l_0}$, The link length from ${J_1}$ to ${J_2}$ is ${l_1}$. The link length from ${J_2}$ to ${J_3}$is ${l_2}$ etc.
$${\mathbf{A}}_{i} = {\begin{bmatrix} {\mathbf{X}}_{i} & {\mathbf{Y}}_{i} & {\mathbf{Z}}_{i} \end{bmatrix}}$$
According to the method of forward kinematics, we can get:
$${\mathbf{P}}_{1} = {\mathbf{P}}_{0}+ {{\mathbf{A}}_{0}}{\begin{bmatrix} 0\\0\\{l_0} \end{bmatrix}}$$
$${\mathbf{P}}_{2} = {\mathbf{P}}_{1}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{\begin{bmatrix} 0\\0\\{l_1} \end{bmatrix}}$$
$${\mathbf{P}}_{3} = {\mathbf{P}}_{2}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{\begin{bmatrix} 0\\0\\{l_2} \end{bmatrix}}$$
$${\mathbf{P}}_{4} = {\mathbf{P}}_{3}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{{\mathbf{A}}_{3}}{\begin{bmatrix} 0\\0\\{l_3} \end{bmatrix}}$$
$${\mathbf{P}}_{5} = {\mathbf{P}}_{4}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{{\mathbf{A}}_{3}}{{\mathbf{A}}_{4}}{\begin{bmatrix} 0\\0\\{l_4} \end{bmatrix}}$$
$${\mathbf{P}}_{6} = {\mathbf{P}}_{5}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{{\mathbf{A}}_{3}}{{\mathbf{A}}_{4}}{{\mathbf{A}}_{5}}{\begin{bmatrix} 0\\0\\{l_5} \end{bmatrix}}$$
$${\mathbf{P}}_{E} = {\mathbf{P}}_{6}+ {{\mathbf{A}}_{0}}{{\mathbf{A}}_{1}}{{\mathbf{A}}_{2}}{{\mathbf{A}}_{3}}{{\mathbf{A}}_{4}}{{\mathbf{A}}_{5}}{{\mathbf{A}}_{6}}{\begin{bmatrix} 0\\0\\{l_6} \end{bmatrix}}$$

## Geometry Solution of Inverse Kinematics

The inverse kinematics of the 6-dof manipulator requires a complete endpoint manipulator position and pose.
First, assume the position of the manipulator is ${\mathbf{P}}_E$ and the rotation matrix is ${\begin{bmatrix} {\mathbf{X_6}} & {\mathbf{Y_6}} & {\mathbf{Z_6}} \end{bmatrix}}$
The range of joint 2,3 and 5 is $\left [0,\frac{\pi}{2} \right ]$, The range of joint 1,4,6 is $\left [-\pi,\pi \right ]$
The method to solve the rotation angle of each joint is as follows.

#### 1. The position of ${J_5}$

Known the manipulator rotation matrix ${\begin{bmatrix} {\mathbf{X_6}} & {\mathbf{Y_6}} & {\mathbf{Z_6}} \end{bmatrix}}$,as the rotation angle of $J_6$ is not related to manipulator, so we can construct vector
$${\vec{{J_5}E}} = ({l_5}+{l_6}){\mathbf{Z_6}}$$
The vector is shown in the simplified diagram of the mechanical below.

To calculate the position of $J_5$ ${\mathbf{P}}_5$, we only need minus ${\vec{{J_5}E}}$ from the endpoint position
$${\mathbf{P}_5} = {{\mathbf{P}}_E} – {\vec{{J_5}E}}$$