IK

Summary

ED7220C 5-DOF Articulated Manipulator
Forward Kinematics (Exact Solution)
Inverse Kinematics (Exact Solution ingnored Singularity)

Dimensions of ED7220C

001

DH Parameters

Joint Coordinates Definition

002

DH Parameters (Standard form)

$i$	$\alpha_i$ [deg]	$a_i$ [mm]	$\theta_i$	$d_i$ [mm]	HOME [deg]	RANGE [deg]	REMARK
1	$\alpha_1 = -90$	$a_1 = 22$	$\theta_1$	$d_1 = 140$	$0$	-155, +155
2	$\alpha_2 = 0$	$a_2 = 218$	$\theta_2$	$d_2 = 0$	$-90$	-125, +45
3	$\alpha_3 = 0$	$a_3 = 218$	$\theta_3$	$d_3 = 0$	$+90$	-130, +130
4	$\alpha_4 = -90$	$a_4 = 0$	$\theta_4$	$d_4 = 0$	$0$	-40, +220	$J_3$ and $J_4$ are on same position.
5	$\alpha_5 = 0$	$a_5 = 0$	$\theta_5$	$d_5 = 140$	$-90$	-180, +180

Check Validity of DH Parameters & Home Position

003

(Using MATLAB & Robotics toolbox by P.Corke)

Forward Kinematics

General Transformation Matrix

$T^i_{i-1} = \begin{bmatrix} cos \theta_i & -sin \theta_i cos \alpha_i & sin \theta_i sin \alpha_i & \alpha_i cos \theta_i \\ sin \theta_i & cos \theta_i cos \alpha_i & -cos \theta_i sin \alpha_i & \alpha_i sin \theta_i \\ 0 & sin \alpha_i & cos \alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} C \theta_i & -S \theta_i C \alpha_i & S \theta_i S \alpha_i & \alpha_i C \theta_i \\ S \theta_i & C \theta_i C \alpha_i & -C \theta_i S \alpha_i & \alpha_i S \theta_i \\ 0 & S \alpha_i & C \alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Transformation Matrix for each joints

Represent to $sin \theta_1 = S_1 ...$

$T^1_{0} = \begin{bmatrix} C_1 & 0 & -S_1 & a_1 C_1 \\ S_1 & 0 & C_1 & a_1 S_1 \\ 0 & -1 & 0 & d_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T^2_{1} = \begin{bmatrix} C_2 & -S_2 & 0 & a_2 C_2 \\ S_2 & C_2 & 0 & a_2 S_2 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T^3_{2} = \begin{bmatrix} C_3 & -S_3 & 0 & a_3 C_3 \\ S_3 & C_3 & 0 & a_3 S_3 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T^4_{3} = \begin{bmatrix} C_4 & 0 & -S_4 & 0 \\ S_4 & 0 & C_4 & 0 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$T^5_{4} = \begin{bmatrix} C_5 & -S_5 & 0 & 0 \\ S_5 & C_5 & 0 & 0 \\ 0 & 0 & 1 & d_5 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Transformation Matrix from $J_1$ to $J_3$ (Base to Wrist)

$T^3_{1} = \begin{bmatrix} C_1 C_2 C_3 -C_1 S_2 S_3 & -C_1 C_2 S_3 -C_1 S_2 C_3 & -S_1 & C_1 C_2 a_3 C_3 -C_1 S_2 a_3 S_3 +C_1 a_2 C_2 +a_1 C_1\\ S_1 C_2 C_3 -S_1 S_2 S_3 & -S_1 C_2 S_3 -S_1 S_2 C_3 & C_1 & S_1 C_2 a_3 C_3 -S_1 S_2 a_3 S_3 +S_1 a_2 C_2 +a_1 S_1 \\ -S_2 C_3 -C_2 S_3 & S_2 S_3 -C_2 C_3 & 0 & -S_2 a_3 C_3 -C_2 a_3 S_3 -a_2 S_2 +d_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Represent to $sin ( \theta_1 +\theta_2 ) = S_{12} ...$

And apply some trigonometric formulas :

$S_{12} = S_1 C_2 + C_1 S_2$

$C_{12} = C_1 C_2 - S_1 S_2$

So we can take more simple form :

Check Validity

When ED7220C is on HOME POSITION,

$q = \begin{bmatrix} \theta_1 \\ \theta_2 \\ \theta_3 \end{bmatrix} = \begin{bmatrix} 0 \\ -90 \\ 90 \end{bmatrix}$

$T_1^3 = \begin{bmatrix} C_1 C_{23} & -C_1 S_{23} & -S_1 & C_1 ( a_3 C_{23} + a_2 C_2 + a_1 ) \\ S_1 C_{23} & -S_1 S_{23} & C_1 & S_1 ( a_3 C_{23} +a_2 C_2 +a_1 ) \\ -S_{23} & -C_{23} & 0 & -( a_3 S_{23} +a_2 S_2 -d_1 ) \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & a_3 +a_1 \\ 0 & 0 & 1 & 0 \\ 0 & -1 & 0 & a_2 +d_1 \\ 0 & 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 240 \\ 0 & 0 & 1 & 0 \\ 0 & -1 & 0 & 358 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

The normal vector of Wrist is :

$\underline{n} = \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}$

The orientation vector of Wrist is :

$\underline{o} = \begin{bmatrix} 0 \\ 0 \\ -1 \end{bmatrix}$

The approach vector of Wrist is :

$\underline{a} = \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix}$

And the position vector of Wrist is :

$\underline{p} = \begin{bmatrix} 240 \\ 0 \\ 358 \end{bmatrix}$

Total Transformation Matrix from $J_1$ to $J_5$ (Base to Tool)

$T_1^5 = \begin{bmatrix} \underline{n} & \underline{o} & \underline{a} & \underline{p} \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$\underline{n} = \begin{bmatrix} ( ( C_1 C_2 C_3 -C_1 S_2 S_3 ) C_4 + ( -C_1 C_2 S_3 -C_1 S_2 C_3 ) S_4 ) C_5 + S_1 S_5 \\ ( ( S_1 C_2 C_3 -S_1 S_2 S_3 ) C_4 + ( -S_1 C_2 S_3 -S_1 S_2 C_3 ) S_4 ) C_5 -C_1 S_5 \\ ( ( -S_2 C_3 -C_2 S_3 ) C_4 + ( S_2 S_3 -C_2 C_3 ) S_4 ) C_5 \end{bmatrix}$

$\underline{o} = \begin{bmatrix} -( ( C_1 C_2 C_3 -C_1 S_2 S_3 ) C_4 + ( -C_1 C_2 S_3 -C_1 S_2 C_3 ) S_4 ) S_5 + S_1 C_5 \\ -( ( S_1 C_2 C_3 -S_1 S_2 S_3 ) C_4 + ( -S_1 C_2 S_3 -S_1 S_2 C_3 ) S_4 ) S_5 -C_1 C_5 \\ -( ( -S_2 C_3 -C_2 S_3 ) C_4 + ( S_2 S_3 -C_2 C_3 ) S_4 ) S_5 \end{bmatrix}$

$\underline{a} = \begin{bmatrix} -( C_1 C_2 C_3 -C_1 S_2 S_3 ) S_4 + ( -C_1 C_2 S_3 - C_1 S_2 C_3 ) C_4 \\ -( S_1 C_2 C_3 -S_1 S_2 S_3 ) S_4 + ( -S_1 C_2 S_3 -S_1 S_2 C_3 ) C_4 \\ -( -S_2 C_3 -C_2 S_3 ) S_4 + ( S_2 S_3 - C_2 C_3 ) C_4 \end{bmatrix}$

$\underline{p} = \begin{bmatrix} ( -(C-1 C_2 C_3 -C_1 S_2 S_3 ) S_4 + ( -C_1 C_2 S_3 -C_1 S_2 C_3 ) C_4 ) d_5 + C_1 C_2 a_3 C_3 -C_1 S_2 a_3 S_3 + C_1 a_2 C_2 + a_1 C_1 \\ ( -(S_1 C_2 C_3 -S_1 S_2 S_3 ) S_4 + ( -S_1 C_2 S_3 - S_1 S_2 C_3 )c_4 ) d_5 + S_1 C_2 a_3 C_3 - S_1 S_2 a_3 S_3 +S_1 a_2 C_2 +a_1 S_1 \\ ( -( -S_2 C_3 -C_2 S_3 ) S_4 +( S_2 S_3 -C_2 C_3 ) C_4 ) s_5 -S_2 a_3 C_3 - C_2 a_3 S_3 -a_2 S_2 +d_1 \end{bmatrix}$

And apply some trigonometric formulas :

$C_{123} = C_1 C_2 C_3 - S_1 S_2 C_3 - C_1 S_2 S_3 - S_1 C_2 S_3$

$S_{123} = S_1 C_2 C_3 + C_1 S_2 C_3 + C_1 C_2 S_3 - S_1 S_2 S_3$

So we can take more simple form :

$\underline{n} = \begin{bmatrix} C_1 C_{234} C_5 + S_1 S_5 \\ S_1 C_{234} C_5 - C_1 S_5 \\ -S_{234} C_5 \end{bmatrix}$

$\underline{o} = \begin{bmatrix} -C_1 C_{234} S_5 + S_1 S_5 \\ -S_1 C_{234} S_5 - C_1 C_5 \\ S_{234} C_5 \end{bmatrix}$

$\underline{a} = \begin{bmatrix} -C_1 S_{234} \\ -S_1 S_{234} \\ -C_{234} \end{bmatrix}$

$\underline{p} = \begin{bmatrix} C_1 ( -d_5 S_{234} + a_3 C_{23} + a_2 C_2 + a_1 ) \\ S_1 ( -d_5 S_{234} + a_3 C_{23} + a_2 C_2 + a_1 ) \\ -d_5 C_{234} - a_3 S_{23} - a_2 S_2 + d_1 \end{bmatrix}$

Check Validity

When ED7220C is on HOME POSITION,

$q = \begin{bmatrix} \theta_1 \\ \theta_2 \\ \theta_3 \\ \theta_4 \\ \theta_5 \\ \end{bmatrix} = \begin{bmatrix} 0 \\ -90 \\ 90 \\ 0 \\ -90 \end{bmatrix}$

$T_1^5 = \begin{bmatrix} \underline{n} & \underline{o} & \underline{a} & \underline{p} \\ 0 & 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 0 & 1 & 0 & 240 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & -1 & 218 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Inverse Kinematics (Exact Solution)

Tool configuration vector $\omega$

$\omega (q) = \begin{bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \\ \omega_4 \\ \omega_5 \\ \omega_6 \\ \end{bmatrix} = \begin{bmatrix} \underline{p} \\ \underline{a} \cdot e^{ \frac{q_5}{\pi} } \end{bmatrix} = \begin{bmatrix} C_1 ( -d_5 S_{234} + a_3 C_{23} + a_2 C_2 + a_1 \\ S_1 ( -d_5 S_{234} + a_3 C_{23} + a_2 C_2 + a_1 \\ -d_5 C_{234} - a_3 S_{23} -a_2 S_2 + d_1 \\ -C_1 S_{234} e^{ \frac{q_5}{\pi} } \\ -S_1 S_{234} e^{ \frac{q_5}{\pi} } \\ -C_{234} e^{ \frac{q_5}{\pi} } \end{bmatrix}$

NOTE : $e^{ \frac{q_5}{\pi} }$ is the roll information of tool.

Find $q_1$

Represent to $q_1 = \theta_1 ...$

$\frac{\omega_2}{\omega_1} = \frac { S_1 ( -d_5 S_{234} + a_3 C_{23} + a_2 C_2 + a_1 ) } { C_1 ( -d_5 S_{234} + a_3 C_{23} + a_2 C_2 + a_1 ) } = \frac{S_1}{C_1}$

$atan2 ( \omega_2 , \omega_1 ) = q_1$

Find $q_3$

$q_{234} = q_2 + q_3 + q_4$ : This is global tool pitch angle.

$C_1 \omega_4 + S_1 \omega_5 = -C_1^2 S_{234} e^{ \frac{q_5}{\pi} } - S_1^2 S_{234} e^{ \frac{q_5}{\pi} } = -S_{234} e^{ \frac{q_5}{\pi} }$

$\frac{\omega_5}{S_1} = -S_{234} e^{ \frac{q_5}{\pi} }$

$\therefore q_{234} = atan2 ( \frac{\omega_5}{S_1} , \omega_6 )$

( This is not a good solution, what if $S_1 = 0$ )

Define as ( to find $q_3$ ) :

$b_1 = C_1 \omega_1 + S_1 \omega_2 -a_1 + d_5 S_{234}$

$b_2 = d_1 - d_5 C_{234} - \omega_3$

$b_1$ and $b_2$ are all known, because we know $q_1$ and $q_{234}$ .

$b_1 = ( C_1^2 + S_1^2 ) ( -d_5 S_{234} + a_3 C_{23} + a_2 C_2 + a_1 ) -a_1 + d_5 S_234 = a_2 C_2 + a_3 C_{23}$

$b_2 = d_1 - d_5 C_{234} - ( -d_5 C_{234} - a_3 S_{23} - a_2 S_2 + d_1 ) = a_2 S_2 + a_3 + S_{23}$

Now use …

$b_1^2 + b_2^2 = ( a_2^2 C_2^2 + a_3^2 C_{23}^2 + 2 a_2 a_3 C_2 C_{23} ) + ( a_2^2 S_2^2 + a_3^2 S_{23}^2 + 2 a_2 a_3 S_2 S_{23} ) = a_2^2 + a_3^2 + 2 a_2 a_3 ( C_2 C_{23} + S_2 S_{23} ) = a_2^2 + a_3^2 + 2 a_2 a_3 C_3$

$q_3 = \pm acos ( \frac{b_1^2 + b_2^2 - a_2^2 - a_3^2}{2 a_2 a_3} )$ : 2 solutions by elbow up&down

Find $q_2$

Now we know $q_3$ , so we can solve

$b_1 = a_2 C_2 + a_3 C_{23} = a_2 C_2 + a_3 ( C_2 C_3 - S_2 S_3 )$

$b_2 = a_2 S_2 + a_3 + S_{23} = a_2 S_2 + a_3 ( S_2 C_3 + C_2 S_3 )$

$b_1 = ( a_2 + a_3 C_3 ) C_2 - ( a_3 S_3 ) S_2$

$b_2 = ( a_3 S_3 ) C_2 + ( a_2 + a_3 C_3 ) S_2$

$C_2 = \frac {( a_2 + a_3 C_3 ) b_1 + a_3 S_3 b_2} {( a_2 + a_3 C_3 )^2 + a_3^2 S_3^2}$

$S_2 = \frac {( a_2 + a_3 C_3 ) b_2 - a_3 S_3 b_1} {( a_2 + a_3 C_3 )^2 + a_3^2 S_3^2}$

$atan2 ( S_2 , C_2 ) = q_2$

Find $q_4$

Remember $q_{234} = q_2 + q_3 + q_4$ ,

$q_4 = q_{234} - q_2 - q_3$ : Tool pitch angle

Find $q_5$

$q_5 = \frac{\pi}{2} ln ( \omega_4^2 + \omega_5^2 + \omega_6^2 )$ : Tool roll angle

Solution Summary

JOINT	SOLUTION	VALID RANGE [deg]
1	$q_1 = atan2 ( \omega_2 , \omega_1 )$	-180 ~ +180
2	$q_2 = atan2 ( S_2 , C_2 )$	-90 ~ +90
3	$q_3=\pm acos ( \frac{b_1^2 + b_2^2 - a_2^2 - a_3^2}{2 a_2 a_3} )$	-0 ~ +180
4	$q_4 = q_{234} - q_2 - q_3$	-270 ~ +90
5	$q_5 = \frac{\pi}{2} ln ( \omega_4^2 + \omega_5^2 + \omega_6^2 )$	-180 ~ +180

Notice

This solution should be evaluated in simulation or real test.
ED does not guarantee this solution’s safety & complete perfection.
Use this solution only for educational purpose.
This document was wrote for who has studied fundamental robotics.
This document does not consider singularity problems, path planning, differential kinematics, dynamics, digital controls, and etc.

References

Corke. Robotics Toolbox for MATLAB (Release 6) . pic@cat.csiro.au, 2001
Desrochers. Notes of FUNDAMENTALS OF ROBOTICS . http://www.ecse.rpi.edu/Courses/F04/ECSE4490/Roboticsoutline04.htm, 2004
Choi. Presentation of Implementation of Inverse Kinematics and Application . kjchoi@graphics.snu.ac.kr, 2007
Craig. Introduction to robotics : mechanics and control . Pearson, 2005
김진오. 강의자료 : 산업용 로봇 기구설계 기초. 산업인력양성프로젝트, 2007

Revisions

Rev-1.0 : First distribution
Rev-1.1 : Fixed some wrong words

Author

by DymaxionKim

IK

2008, Personal Study

IK