The Closed Form Solution of the Inverse

Kinematics of a 6-DOF Robot

Mustafa Jabbar Hayawi

Computer Science Department

Education College
Thi - Qar University
mustafahayawi@yahoo.ca

Abstract:-

The kinematics of manipulator is a central problem in the

automatic control of robot manipulators. Theoretical background of
the analysis of the MA-2000 educational robot arm kinematics is
presented in this paper. The revolute robot consists of six rotary
joints (6-DOF) with base, shoulder, elbow, wrist pitch, wrist yaw
and wrist roll. The kinematics problem is defined as transformation
from the cartesian space to the joint space. The DenavitHarbenterg (D-H) model of representation is used to model robot
links and joints in this study. Both forward and inverse kinematics
solutions for this robot are presented. Four sets of exact solution
for the vector of the joint angles { i } pertaining to the inverse
kinematics problem of a MA-2000 robot with two different kinds of
gripper configurations. An effective method is suggested to
decrease multiple solutions in inverse kinematics.
Keyword: Forward kinematics robot, inverse kinematics robot,
closed form solution
1.Introduction:
A serial link manipulator consists of a sequence of
mechanical links connected together by actuated joints. Such a
structure forms a kinematics chain and may be analyzed by
methods developed by Denavite and Hartenberg (D-H)[1]. The
results of this analysis are the matrix equations expressing
manipulator end-effector Cartesian position and orientation in
terms of the joint coordinates. These equations may be obtained
for any manipulator independent of the number of links or degree
of freedom.
The problem of the inverse kinematics for robot manipulators
has always been a challenging problem in their design. Essentially,
the problem is to find the vector of the joint angles, say for an naxis revolute manipulator, given the position and orientation of the

end- effector or the gripper [2].

Piper and Roth[3], were the first to derive a set of solutions
for a number of special robot manipulators. They also devoted a
portion of their work to the iterative type algorithms to numerically
solved the inverse kinematics problem. Lumelsky [4] presented
iterative solutions for the inverse kinematics problem for one type
of a robot manipulator. Paul and Mayer [5] presented the basis for
all advanced manipulator control which is relationship between the
Cartesian coordinates of the end-effector and manipulator joint
coordinate. Dieh, Z. [6] proposed a closed form solution algorithm
for solving the inverse position and orientation problem for the
NASHI-750 robot by imposing a castrating condition to the problem
and projection the tool frame on subspace of the robot. Duffy [7]
further found some special geometric manipulators, which can
solve the same problem analytically. Since then, many methods
have been presented to solve the IK problem.
2.Coordinate frame:
A serial link manipulator consists of a sequence of links
connected together by actuated joints. For n-degrees of freedom
manipulator, there will be n-links and n-joints. The base of the
manipulator is link 0 and is not considered one of the six links.
Link 1 is connected to the base link by joint 1. There is not joint
at the end of the final link. The only significance of links is that they
maintain a fixed relationship between the manipulator joints at
each end of the link. Any link can be characterized by two
dimensions. The common normal distance ai and i the angle
between the axes in a plane perpendicular to ai .It is customary to
call ai the length and i the twist of the link (see Fig.-1).
Generally, two links are connected at each joint axis.
The axis will have two normal connected to it, one for each
link. The relative position of two such connected links is given by di
the distance between the normal along the joint n-axis and i the
angle between the normal measured in a plane normal to the
axis.[8]

Fig. -1: Link parameter d, a, and

The coordinates for the generalized link coordinate system are
chosen as follows:
1. Numbering:
Number the joints from 1 to n starting with the base and
ending with tool Yaw, Pitch and Roll in that order.
2- Base frame:
Assign a right-handed orthonormal coordinate frame O0 to

the robot base, making sure that Z 0 aligns with the axis of joint 1.
Set i=1

Z i Axis

3-

Align Z i with the axis of joint i+1

4- Oi Origin:

If Z i Z i 1 then

Locate Oi at the point of intersection.

Else

Locate Oi at the intersection of Z i with a common normal

between Z i

and Z i 1 .

End if

5- X i Axis:

If Z i // Z i 1 then

Point X i away from Z i 1

Else

Establish X i perpendicular on both Z i and Z i 1 .

End if

6- Y i Axis:

Establish Y i to from a right handed orthonormal coordinate frame.

7- Decision:
Set i=i+1
If i < n then
Go to step 3
Else
Go to step 8
End if
8- Tool frame:
- Locate On at the tool tip.

- Align Z n

- Align Y n with the sliding vector

- Align X n
Stop

with the approach vector a .

s.

with the normal vector n .

constitute a minimally d i , i , i , a i The four parameters ( )

sufficient set to determine the kinematics configuration of each link
of the robotic arm.
Note that for a plane revolute joint, generally (
) are
all constant while i varies as link i rotate about the axis of joint i.
on the other hand, for a prismatic joint ( i , i , ai ) are constant while
Thus,
d varies as link i slides along the axis of joint i.
i and ai i
depend
for both cases are generally constant and
on the design of the robot. Once the Denavit-Harbenterg (D-H)
coordinate system for each link is established a homogenous
transformation matrix can be easily developed relating the i-th
coordinate frame to the (i-1)th coordinate frame. The following
homogenous transformation matrices:

d i , i , a i

1- joint variable qi:

set i=1
qi i i (1 i ) d i

..(1)

joint i revolute

joint i prismatic

2- HTMs of two adjacent frames:

From the HTMs between two adjacent link frames by
substituting the link parameters from the link parameter table into:
Ci
S
i 1
Ti (q ) i
0

Si Ci

Si S i

Ci Ci
S i

Ci Ci
Ci

..(2)
3- Decision:
Set i=i+1
If i n then
Go to step 1
Else
Got to step 4
End if

ai Ci
ai Si
di

4- Manipulator matrices:
The wrist matrix:
The hand matrix:
The arm matrix:
(3)
Stop

Base

TWrist (q ) 0 T3 (q3 ) 0 T1 (q1 )1T2 (q 2 ) 2 T3 (q3 )

Wrist

TTool (q ) 3Tn (q ) 3T4 (q 4 ) 4 T5 (q5 ).........n 1 Tn (q n )

Base

TTool (q) Base TWrist (q) Wrist TTool (q ) 0 Tn (q n )

Kinematics equations:
The task of a robot usually specified in Cartesian space
0
coordinate, X T (Cartesian position and orientation of the tool
frame T relative to a fixed frame 0), the controllers require joint
space coordinate , to control the motion of the robot. Therefore,
coordinate transformation from one space to another is important.
Such transformations are often referred to as forward (direct) and

inverse kinematics. The forward kinematic problem transforms on

0
one side, joint space coordinate , into task space coordinate, X T
, via nonlinear transforms, f , determine by the homogenous
0
transformation matrices TT ,such that [9].
0

X T f ( )

0 RT
TT T
03

....(4)
0

PT

.(5)

Where 0 T0 is zero vector, 0TT is homogenous transformation

0

matrix, RT and PT are the rotation matrix and position vector of

frame T relative to frame 0, respectively.
The description of the end-effector, link coordinate frame 6, with
respect to link coordinate frame n-1 is given by Un:
U n An An 1 ........A6

(6)
The position and orientation of the end-effector respect to
base, known as T6 is given by U1:
T6 U 1 A1 * A2 * A3 * A4 * A5 * A6

0 R6
T
03

P6

..(7)
For the first version of MA-2000 educational robot, we
consider the gripper axis of motion to be co directional with the
axis of wrist roll (Fig.-2). For the second version of this same robot
we consider the gripper axis of motion to be perpendicular to the
axis of wrist roll (Fig.-3).
A. First case:
Consider (Fig.-2) we can construct the following table for the
joint parameter (Table- 1).

X2

Y6

X6

Y5

X5

Y4

X3

a3

Z2

Z1

Y2

Y1

X4

Z5

Z6

Z3

a4

Z4

Y3

a2

X1
d1
Z0
X0

Y0

Fig-2: Link frame assignment for MA-2000 Robot with first version
of gripper.

Joint
1
2
3
4
5
6

i
1
2
3
4
5
6

ai

di

90
0
0
90
90
0

d1

a2
a3
a4

0
0

0
0
0
0

d6

Table-1 : joint parameter for 6-DOF robot with first version

C1
S
0
T1 1
0

S1

0 C1
1
0
0

.(8)

0
0
d1

C 2
S
1
T2 2
0

C 3
S
2
T3 3
0

C 4
S
3
T4 4
0

C 5
S
4
T5 5
0

C 6
S
5
T6 6
0

S2
C2
0

0 a2C2
0 a 2 S 2
1
0

0
1

.(9)
S3
C3

0 a3C 3
0 a3 S 3
1
0

0
1

0
0

.(10)
0 S4
0 C4
1
0

0
0

(11)
S5

0 C5
1

(12)
S6
C6
0
0

a4C4
a 4 S 4
0

0
0
0

0
0 0
1 d6

0 1

(13)
Multiplying the matrices T1 through T6 and setting the result equal
0

to T6 yields the following twelve equations for the determination of

the vector of the joint angle.
n x C1 (C 234 C 5 C 6 S 234 S 6 ) S1 S 5 S 6

....(14)

nY S1 (C 234 C 5 C 6 S 234 S 6 ) S1 S 5 S 6

(15)

n Z S 234 C 5 C 6 C 234 S 6

(16)

o x C1 (C 234 C 5 C 6 S 234 S 6 ) S1 S 5 S 6

(17)

o x S1 (C 234 C 5 C 6 S 234 S 6 ) S1 S 5 S 6

(18)

o Z S 234 C 5 C 6 C 234 S 6

.(19)

a x C1C 234 S 5 S1C 5

(20)

aY S1C 234 S 5 C1C 5

(21)

a Z S 234 S 5

(22)

p x C1 (d 6 S 5 C 234 a 4 C 234 a 2 C 2 a3 C 23 ) S1C 5 d 6

...(23)

p y S1 (d 6 S 5 C 234 a 4 C 234 a 2 C 2 a3 C 23 ) S1C 5 d 6

..(24)

p Z d1 d 6 S 234 S 5 a3 S 23 a 2 S 2 a 4 S 234

..(25)

Multiplying Eq.(24) by C1 and Eq.(23) by S1 and substituting the

resulting equations, as the following is obtained:
C1 0 p y 6 S1 0 p x 6 d 6 C 5

(26)
Multiplying Eq.(20) by S1 and Eq.(21) by C1 and substituting the
resulting equations, as the following is obtained:
C 5 C1 a y S1 a x

(27)
From Eqs.(26 and 27) ,we can obtain:
C1 0 p y 6 S1 0 p x 6 d 6 C1 a y d 6 a x S1

.. (28)
Hence,

1 a tan 2(( p y d 6 a y ), ( p x d 6 a x )

(29)
From eq(27), hence,
S 5 1 C 52

(30)

5 a tan 2( S 5 , C 5 )

(31)
Now multiplying Eq.(20) by C1 and Eq.(21) by S1 and adding the
resulting equations yields:
S 5 C 234 (a x C1 a y S1 )

(32)

From Eq.(22) and Eq.(32) find that:

234 a tan 2( a z , (a x C1 a y S1 ))

and,

(33)

for 5 > 0

234 234

for 5 < 0

(34)
Multiplying Eq.( 23) by C1 and Eq.(24) by S1 and adding the
resulting equations yields:
P1 C1 p x 6 S1 p y 6 d 6 S 5 C 234 a 4 C 234

Where,
P1 a 2 C 2 a3 C 23

(35)
Rearranging eq.(25) yields:
P2 p z 6 d1 d 6 S 5 S 234 a 4 S 234

Where,
P2 a3 S 23 a 2 S 2

(36)
Thus, from eqs.(35) and (36) we find:
a3 C 23 P1 a 2 C 2

(37)

a3 S 23 P2 a 2 S 2

(38)
Hence from Eqs(37) and (38) we have:
a32 ( P1 a 2 C 2 ) 2 ( P2 a 2 S 2 ) 2

(39)
Which yield the following:
P1C 2 P2 S 2

P12 P22 a 22 a32

N
2a 2

.(40)
Then it clearly follows that:

2 a tan 2( P2 , P1 ) a tan 2( P12 P22 N , N )

(41)

Consequently from (37) and (38):

23 a tan 2( P2 a 2 S 2 , P1 a 2 C 2 )

.(42)

3 23 2

and finally,

4 234 23

.(43)

.(44)
Multiplying Eq.( 15) by C1 and Eq.(14) by S1 and adding the
resulting equations yields:
S 5 C 6 n y C1 n x S1

..(45)
Multiplying Eq.( 17) by S1 and Eq.(18) by S1 and adding the
resulting equations yields:
S

(o x S

Hence,

o yC

(46)

6 a tan 2((o x S1 o y C1 ), (n y C1 n x S1 )) for 5 0

... (47)

for 5 0

6 6

... (48)
5 0
These are the complete solutions for the inverse kinematics
problem of a robot when (nondegenerate).
one of 5 0, n If S5=0(
) the arm geometry becomes
a degenerate kind because 234 becomes
essentially
234 k arbitrary based on Eq.(22). For the sake of
calculation let us assume that
1 a tan 2(( p y d 6 a y ), ( p x d 6 a x )

(49)
2 a tan 2( P2 , P1 ) a tan 2( P12 P22 N , N )

(50)

23 a tan 2( P2 a 2 S 2 , P1 a 2 C 2 )

(51)

3 23 2
4 k 23
5 0

(52)
(53)

(54)
From Eqs.(16) and (19) and further yield:
C 6 n z S 234 C 5 o z C 234

... (55)

S 6 o z S 234 C 5 n z C 234

(56)

6 a tan 2( S 6 , C 6 )

(57)

These are complete solutions for one of the degenerate case of a

robot with a gripper of the first case.
B. Second case:
Consider (Fig.-3) we can construct the following table for the joint
parameter (Table-2).
X2

Z6
Y6

X6

Y5

X5
Z5

Z4

a4
Y4

Y3

Z3
X3

a3

Z2

Z1

X4

Y2

Y1

a2

X1
d1
Z0
X0

Y0

Fig-3: Link frame assignment for MA-2000 Robot with second

version of gripper.
i
ai
di
Joint i
1
d1
1
90
0
2
a2
2
0
0
3
a3
3
0
0
4
a4
4
90
0
5
5
90
0
0
6
d6
6
90
0
Table-2: joint parameter for 6-DOF robot with second version

Now bases on above table the homogenous transformation

matrices will be the same as before expect for T6 which becomes:

C 6
S
5
T6 6
0

0 S6
0 C6
1
0
0

0
0
d6

(59)
The homogenous transformations of the end-effector to the base
can be multiplying the above six matrices and setting the results
equal to 0T6:
n x C1 (C 234 C 5 C 6 S 234 S 6 ) S1 S 5 S 6

....(60)

nY S1 (C 234 C 5 C 6 S 234 S 6 ) S1 S 5 S 6

(61)

n Z S 234 C 5 C 6 C 234 S 6

(62)

o x C1C 234 S 5 S1C 5

(63)

o y S1C 234 S 5 C1C 5

o Z S 234 S 5

(64)
.(65)

a x C1 (C 234 C 5 C 6 S 234 S 6 ) S1 S 5 S 6

(66)

a x C1 (C 234 C 5 C 6 S 234 S 6 ) C1 S 5 S 6

(67)

a Z S 234 C 5 S 6 C 234 C 6

.(68)

p x C1 (d 6 S 5 C 234 a 4 C 234 a 2 C 2 a3 C 23 ) S1C 5 d 6

...(69)

p y S1 (d 6 S 5 C 234 a 4 C 234 a 2 C 2 a3 C 23 ) S1C 5 d 6

...(70)

p Z d1 d 6 S 234 S 5 a3 S 23 a 2 S 2 a 4 S 234

...(71)

Note that the structures of these equations are similar to the

structure of previous (Eqs.(14) through (25)) except that the roles
of o-vector and the roles of a-vector are exactly replaced
respectively by.

1 a tan 2(( p y d 6 a y ), ( p x d 6 a x )

(72)
2 a tan 2( P2 , P1 ) a tan 2( P12 P22 N , N )

...(72)

23 a tan 2( P2 a 2 S 2 , P1 a 2 C 2 )

(73)

3 23 2

(74)
Multiplying eq.(63) by S1 and eq.(64) by C1,
C 5 S1o x C1o y
S 5 1 C 52

5 a tan 2( S 5 , C 5 )

(75)
Now multiplying eq.(63)by C1 and eq.(64)by S1 and adding the
resulting equations yield:
S 5 C 234 o x C1 o y S1

(76)
From eq.(65) and eq.(76) find that:
234 a tan 2(o z , (o x C1 o y S1 ))

And,

(77)

234 234
4 234 23

(78)

for 5 0

for 5 0

...(79)
Multiplying eq.(60) by C1 and eq.(61) by S1 and adding the
resulting equations yield:
S 5 C 6 n y C1 n x S1

...(80)
Multiplying eq.(66) by S1 and eq.(67) by C1 and adding the
resulting equations yield:
S 5 S 6 a x S1 a y C1

Hence,

...(81)

6 a tan 2( S 6 , C 6 )

And,

(82)

6 6

(83)

for 5 0

for 5 0

These are complete solutions for inverse kinematics problem of a

5 0 . For the
degenerate case
5 0, n robot for
we find that:
when ( ) and assume ( 234 k ),
1 a tan 2(( p y d 6 a y ), ( p x d 6 a x )

..(84)
2 a tan 2( P2 , P1 ) a tan 2( P12 P22 N , N )

..(85)

23 a tan 2( P2 a 2 S 2 , P1 a 2 C 2 )
3 23 2
4 k 23
5 0

..(86)

..(87)
..(88)

..(89)
From eqs.(62) and (68) and further yield:
C 6 n z S 234 C 5 a z C 234

.. (90)

S 6 a z S 234 C 5 n z C 234

..(91)

6 a tan 2( S 6 , C 6 )

..(92)
These are complete solutions for one of the degenerate case of a
robot with a gripper of the second case.
Results and discussion:
An analysis technique was introduced to reduce the multiple
solutions in inverse kinematics. Forward and inverse kinematics
solution are generated and implemented by program (Fig.4). For
the numerical confirmation of the exact solutions the following
procedure has been adopted. First the values of the structure of a
robot a 2 , a3 , a 4 , d1 , d 6 and i are specified (Table-3). Then a set of i s
are chosen to calculate position and orientation of the end effector
by forward kinematics. The values of the attitude matrix are then
employed to calculate the joint angles by inverse kinematics. The
inverse kinematics problem is solved by closed form solution
because the kinematic equations in general have multiple
solutions; having closed form solution allows one to develop rules
for choosing a particular solution among several. The results
obtained for the components of the vector of the joint angles are
compared with the chosen values of i s. (Table4 and 5). They are

in fairly excellent agreement with the chosen values for i s.

Joint
1
2
3
4
5
6

ai

di

90
0
0
90
90
0

0
15
15
10
0
0

15
0
0
0
0
15

Table-3: D-H parameters of MA-2000 Robot arm

Joint

i Chosen

1
2
3
4
5
6

25
45
30
40
20
30

Calculated
25
44.99998
30.001
39.998
19.999
30.0003

Table-4: the results for the non degenerate case1

Joint

i Chosen

Calculated
1
25
25
2
45
45.002
3
30
29.997
4
40
40
5
0
0
6
30
29.897
Table-5: the results for the degenerate case1

Joint
Angles
i

Compare
joint angles

Forward
Kinematics and
generate
transformation
Ai

Determine
position and
orientation A6

Calculate Joint
Angles i by
Inverse Kinematics
Equations

Fig(4): Block diagram for the numerical confirmation

of the exact solution for MA-2000 Robot
Conclusions:
Two different gripper configurations were used to find exact
solution of the joint angles. Two sets pertaining to the degenerate
cases were nonunique and the other two sets pertaining to the
nondegenerate cases were unique for each gripper
configurations.
The numerical calculation by forward kinematics showed that
the calculated exact solutions were in prefect agreement with the
chosen vector of the joint angle for each gripper configuration.
References:

1. Jing-Shan Zhao, Kai Zhou, Zhi-Jing Feng, A theory of degrees of

freedom for

mechanisms, Mechanism and Machine Theory 39 (6) (2004) 621643.

2. Coiffet, P., Chirouze, M.: An Introduction to Robot Technology. Kogan
Page, London, 1983.
3. Piper,D.L. and Roth,B. the kinematics of manipulator under computer
control proc.2nd Int.cong.on the theory of mach.and mech. Vol.2,1969.
4. Lumelsky,V.J.Iterative procedure for inverse coordinate transformation
for one class of robots. Report general electric company Feb.1983.
5.Paul P.R. and Mayer G.kinematics control equations for simple
manipulator IEEE Trans. On systems Man. And Cyberntics VolSNC-11
No.6 June 1995.
6.Dieh Z. Simulation and off-programming of a 5-DOF welding robot
M.Sc. Thesis, Baghdad University Dept. of Mech.Eng.1998.
7. Duffy, J., 1980. Analysis of Mechanism and Robot Manipulator. Wiley,
NY, USA.
8. Craid J.J Introduction to robotics: mechanics and control Ad isonWesely.1989.

9.Baki Koyuncu Application of 6 axes robot arm by using inverse

kinematics
equations Journal of computer engineering Vol.1,No.1,2007.