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
You are NOT ALLOWED to copy this article

The robot structure is shown in the figure below:

6 Degree of Freedom Robot Structure
Figure 1. 6 Degree of Freedom Robot Structure


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.

The initial state and rotation direction of the 6 DOF manipulator.
Figure 2. The initial state and rotation direction of the 6 DOF manipulator.

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.
Before solving, the Euler Angle must be transform into a rotation matrix. For more information about the transform between Euler Angle and rotation matrix, Please refer to this article
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.

Simplified Diagram of 6DOF manipulator
Figure 3. Simplified Diagram of 6DOF manipulator

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}} $$

2. The angle of ${J_1}$ ${\theta_1}$

From a mechanical arm structure, the ${J_1} $to ${J_5} $ is on the same plane. The projection of ${J_1} $${J_5} $ to the x-y plane, as shown in the figure below. The angle between the projection line and X axis is ${\theta_1} $.

x-y plane projection
Figure 4. x-y plane projection

Here we assume ${\mathbf{P}_5} = {\begin{bmatrix} {x_5} \\ {y_5} \\{z_5} \end{bmatrix}}$
So,
$${\theta_1} = \operatorname{atan2}({y_5},{x_5}) $$

3. The rotation angle of ${J_3}$ ${\theta_3}$

Because we know the position of ${J_5}$ and the position of ${J_2}$ ${\mathbf{P}_2}$is fixed, connect ${J_2}$and ${J_5}$to make a triangle, as shown in the figure below.

Figure 5. Joint 2,3

We can calculate $\vec{{J_2}{J_5}} = {\mathbf{P}_5} – {\mathbf{P}_2}$
By the law of cosines:
$${\cos{\theta_3}} = \frac{{\left \| \vec{{J_2}{J_5}} \right \|}^2 – {l_2^2} -{l_3^2}}{2{l_2}{l_3}} $$
So:
$${\theta_3} = \arccos{\left ( \frac{{\left \| \vec{{J_2}{J_5}} \right \|}^2 – {l_2^2} -{l_3^2}}{2{l_2}{l_3}} \right )} $$

4. Calculate the transform angle of ${J_2}$ ${\theta_2}$

As shown in figure before, draw a line cross ${J_5} $be perpendicular to the extension line of ${J_2} {J_3} $. The extension cord is $S $
From ${J_2} $in the plane of ${\bigtriangleup} {J_2} {J_3} {J_5} $draw a perpendicular line to the Z axis, and make a perpendicular line across the ${J_5}$perpendicular to this line at $T $

Connect ${J_2}{J_5}$ and name this line$A$
In ${\bigtriangleup}{J_2}{J_3}{J_5}$, apply Pythagorean theorem, we can get:
$$A = \sqrt{(({l_3}+{l_4})\sin{\theta_3})^2 + (({l_3}+{l_4})\sin{\theta_3}+{l_2})^2}$$
$$\beta = \arcsin{\left ( \frac{{\mathbf{P}_5}*{\mathbf{Z}_0} – {l_1}}{A} \right )} $$
$$\alpha = \arccos{\left ( \frac{{l_2^2}+{A^2}-{(l_3+l_4)}^2}{2A{l_2}}\right )}$$
So:
$${\theta_2} = \frac{ \pi}{2} – \alpha – \beta$$

So far, the rotation of the joint 1,2,3 has been calculated. We can use forward kinematics to calculate the position of ${J_3}$ ${\mathbf{P}}_3$and its rotation matrix $\begin{bmatrix} {{\mathbf{X}}_3} & {{\mathbf{Y}}_3} &{{\mathbf{Z}}_3} \end{bmatrix}$

5. Calculate the transform angle of ${J_5}$ ${\theta_5}$

From the forward solution of the rotation matrix of ${\theta_5}$, we can know:
$${\mathbf{Z}}_5 = \sin{\theta_5}{\mathbf{X}_4} + \cos{\theta_5}{\mathbf{Z}}_4 $$
And ${\mathbf{X}_4} \perp {\mathbf{Z}_4}, \left \| {\mathbf{Z}_4} \right \| =1$. Multiply both sides of the above equation by ${\mathbf{Z}_4}$, we can get:
$$\cos{\theta_5} = {\mathbf{X}_4}\cdot{\mathbf{Z}_4}$$

Because of the rotation of the joint 4,6 around the Z axis,
$${\mathbf{Z}}_5 = {\mathbf{Z}}_6 , {\mathbf{Z}_4} ={\mathbf{Z}}_3 $$
$$\cos{\theta_5} = {\mathbf{X}_3}\cdot{\mathbf{Z}_6}$$
$${\theta_5} = \arccos{{\mathbf{X}_3}\cdot{\mathbf{Z}_6}}$$

6. Calculate the transform angle of${J_4}$ ${\theta_4}$

From the forward solution of the rotation matrix of ${\theta_4}$:
$${\mathbf{X}}_4 = \cos{\theta_4}{\mathbf{X}}_3 + \sin{\theta_4}{\mathbf{Y}_3} $$
Substitute the above equation into the ${\mathbf{Z}}_5$.
$${\mathbf{Z}}_5 = \sin{\theta_5}(\cos{\theta_4}{\mathbf{X}}_3 + \sin{\theta_4}{\mathbf{Y}_3}) + \cos{\theta_5}{\mathbf{Z}}_3$$
And ${\mathbf{X}_3} \perp {{\mathbf{Y}}_3}, {\mathbf{X}_3} \perp {\mathbf{Z}_3} \left \| {\mathbf{X}_3} \right \| =1$
Multiply both sides of the above equation by ${\mathbf{X}_3}$:
$${{\mathbf{Z}}_5}\cdot {{\mathbf{X}}_3} = \sin{\theta_5} \cos{\theta_4}$$
Multiply both sides of the above equation by ${\mathbf{Y}_3}$::
$${{\mathbf{Z}}_5}\cdot {{\mathbf{Y}}_3} = \sin{\theta_5} \sin{\theta_4}$$
Make fraction of two equation:
$${\theta_4} = \operatorname{atan2}{\left ( {{{\mathbf{Z}}_6} \cdot {{\mathbf{Y}}_3}},{{{\mathbf{Z}}_6} \cdot {{\mathbf{X}}_3}} \right )} $$

At this point, the rotation matrix at the end of the fifth joint can be obtained according to the rotation Angle of the 1st to 5th joint. $\begin{bmatrix} {{\mathbf{X}}_5} & {{\mathbf{Y}}_5} &{{\mathbf{Z}}_5} \end{bmatrix}$

7. Calculate the transform angle of${J_6}$ ${\theta_6}$

From the forward solution of the rotation matrix of ${\theta_6}$:
$${\mathbf{X}}_6 = \cos{\theta_6}{\mathbf{X}}_5 + \sin{\theta_6}{\mathbf{Y}_5} $$
Multiply both sides of the above equation by ${\mathbf{Y}_5}$, and $\left \| {\mathbf{Y}_5} \right \| =1$
So:
$${\theta_6} = \operatorname{atan2}{({{\mathbf{X}}_6}\cdot {{\mathbf{Y}}_5} , {{\mathbf{X}}_6}\cdot {{\mathbf{X}}_5} )} $$