FORWARD AND INVERSE KINEMATIC SOLUTION FOR FUNUC M-10iA ROBOT

BY
SOLOMON EZEIRUAKU
AND
UCHE MARK EZEOBI

A PROJECT ON
Robotic Engineering II (ME 582)

1.0

INTRODUCTION

M-10iA Fanuc robot is a six degree of freedom, high performance industrial

robot. This is a small but mighty robot only weighs 130kg but provides 10kg
pay-load with highest wrist moments and inertia in its class. It can be
mounted on the floor, wall, or ceiling at any angle. The third axis can be
flipped over allowing the robot to work behind itself thereby creating a large
working space. It is an ideal solution for machine tending, material handling,
picking/packing, assembly, material removal, testing and sampling, and
dispensing.

1.1

1.2

TECHNICAL SPECIFICATION OF THE ROBOT

Degree of Freedom = 6
Type of joint = All rotary
Payload = 10kg at the wrist
Repeatability = 0.08 mm

FRAME ASSIGNMENT

We systematically established a coordinate system to each link using

Denavit-Hartenberg method. The Zi-axis of the entire joint is fixed in the
direction of rotation as followed by the right-hand rule for rotation. Here the
rotation about the Z-axis will be the joint variable. Then Xi axis is fixed
considering Zi-1 and Zi axes. And finally Yi axis is fixed to complete the right
handed orthonormal coordinate frame. Thus frame assignment of 6-DOF
robot was completed as shown in Figure 1. The link parameters is shown in
table 1 and the transformations between adjacent coordinate frames is
represented using 4x4 homogenous coordinate transformation matrix.

i-1

ai-1

di

-90

a1

a2

-90

a3

d4

90

-90

6
Table 1: Link parameters of the M-10iA Fanuc
Robot

Transformation matrix for link 1 is

Figure 1. Frame assignment of M-10iA Fanuc

Manipulator

T1

c1
s
1
0

s1
c1
0
0

0
0
1
0

0
0
0

(1.1)

Transformation matrix for link 2 is

T2

c2
0

s2

s2
0
c2
0

0 a1
1 0
0 0

0 1

(1.2)

Transformation matrix for link 3 is

T3

c3
s
3
0

s3
c3
0
0

0 a2
0 0
1 0

0 1

(1.3)

Transformation matrix for link 4 is

T4

c4
0

s4

s4
0
c4
0

0 a3
1 d 4
0 0

0 1

Transformation matrix for link 5 is

(1.4)

T5

c5
0

s5

s5
0
c5
0

0
0
0

0
1
0
0

(1.5)

Transformation matrix for link 6 is

T6

c6
0

s6

s6
0
c6
0

2.0

0
1
0
0

0
0
0

(1.6)

FORWARD KINEMATCS

We solved for Forward kinematic solution of the robot to determine the

position and orientation of the end-effector given the values for joint
variables of the robot. That is computation of position and orientation of the
end effector relative to the base. In this case, our joint variables are the
angles between the links. The homogenous matrix 0T6 which specifies the
location of the end effector with respect to the base is gotten by matrix
multiplications below.
To form our 0T6 matrix, we multiple out all the individual matrices of each
link:

T6 =

T5 . 5T6

Then, we have:

c5 c 6
s
6
s5 c6

c5 c6
c6
s5 s6
0

s5
0
c5
0

0
0
0

(2.1)

T6 =

T3 =

T4 . 4T6

T2 . 2T3

c 4 c5 c 6 s 4 s 6

s5 c6

c 4 s 6 c5 c 6 s 4

c 2 c3 s 2 s3

c 4 s 3 c3 s 2

c 6 s 4 c 4 c5 s 6
s5 s6
c5 s 4 s 6 c 4 c6
0

c 2 s3 c3 s 2
0
s 2 s3 c 2 c3
0

c 4 s5
c5
s 4 s5
0

a3
d 4
0

(2.2)

0 a 2 c 2 a1

1
0

0 a2 s2

0
1

(2.3)

Using sum of angle formulas:

C23 = c2c3 s2s3
S23

= c2s3 + s2c3

Equation (2.3) becomes;

T3

s 23
0
c 23
0

c 23
0

s 23

0 a 2 c 2 a1

1
0

0 a2 s2

0
1

(2.4)

Then we have

1 r11

T6 =

r11 =

T3 . 3T6

r21
1 r31

c 23 s 4 s 6 c 4 c5 c 6 s 23c 6 s5

r12

r22
1
r32
0

r13

r23
1
r33
0

px

py
1
px

(2.5)

r21 =

s 23 s 4 s 6 c 4 c5 c 6 c 23c 6 s5

r31 =

r12 =

r22 =

r13 =

r23 =

c 4 s 6 c5 c6 s 4

r33 =

s 23 s5 s 6 c 23 c6 s 4 c 4 c5 s 6
c5 s 4 s 6 c 4 c6
s 23 c5 c 23c 4 s5
s 4 s5
s 23c 4 s5 c 23 c5

a1 a3 c 23 d 4 s 23 a 2 c 2

px =

py = 0

pz =

d 4 c 23 a3 s 23 a 2 s 2

(2.6)

The product of all the six link transforms is:

r11
r
21
r31

r12
r22
r32

r13
r23
r33

px
p y
px

T6 =

r11 =
r21 =
r31 =

T1 . 1T6

s1 s 4 s 6 c 4 c5 c 6 c1 c 23 s 4 s6 c 4 c5 c6 s 23 c 6 s5
c1 c 4 s 6 c5 c 6 s 4 s1 c 23 s 4 s 6 c 4 c5 c6 s 23 c6 s5
s 23 s 4 s 6 c 4 c5 c 6 c 23c6 s5

(2.7)

s1 c 4 c6 c5 s5 s6 c1 c 23 c6 s 4 c 4 c5 s 6 s 23 s5 s 6

r12 =

s1 c 23 c6 s 4 c 4 c5 s 6 s 23 s5 s 6 c1 c 4 c6 c5 s 4 s 6

r22 =

s 23 c 6 s 4 c 4 c5 s 6 c 23 s5 s 6

r32 =

c1 s 23c5 c 23c 4 s5 s1 s 4 s5

r13 =

c1 s 4 s1 s1 s 23c5 c 23c 4 s5

r23 =

s 23c 4 s 5 c 23c5

r33 =
px =

c1 a1 a 3 c 23 d 4 s 23 a 2 c 2
s1 a1 a3 c 23 d 4 s 23 a 2 c 2

py =

d 4 c 23 a3 s 23 a 2 s 2

pz =

(2.8)

3.0

INVERSE KINEMATICS

Inverse kinematic solution is concerned with determining values for the joint
variables that achieve a desired position and orientation for the end-effector
of the robot. That is giving the position and orientation of the end effector
relative to the base frame, we are required to compute all the possible sets
of joint angles and link geometries which could be used to attain the given
position and orientation of the end effector.

T
0

T 12T 23T 34T 45T 56T

0
6

(3.1)

 c1
s
1

s1
c1
0
0

0
0
1
0

0
0
0

r11
r
21
r31

r12
r22
r32
0

r13
r23
r33
0

px
p y
px

1
6

(3.2)

From (2, 4) element;

s 1 p x c1 p y 0
p x p cos
p y p sin
p

px 2 py 2

A tan 2( p y , p x )
s 1 p cos c 1 p sin 0
s 1 pc c1 s 0

sin 1 0

cos 1 1 0 1

1 A tan 2(0,1)
1 A tan 2 py , px A tan 2 0,1

T
1

1 0

c2
s
2

1 0

(3.3)

T6 2 T6

(3.4)

0 s2
0 c2
1
0
0
0

a1 c2
a 1 s 2
0

c1
s
1

s1
c1
0
0

0
0
1
0

0
0
0

r11
r
21
r31

r12
r22
r32
0

r13
r23
r33
0

px
p y
px

= 2T6

(3.5)

We obtained 2T6 below:

2

T6 =

c 6 s 3 s5 c 3 c 4 c3 s 4 s 6
c c s c c s s s s
3 4 6
6 3 5 4 5 3

c 4 s 6 c5 c 6 s 4

c1 c2
c s
1 2

s1

c 2 s1
s1 s 2
c1
0

s2
c2
0
0

s 6 c3 s 4 s6 c3 c 4 c5 c3 c 6 s 4
s 6 c3 s 5 c 4 c5 s3 c6 s 3 s 4
c5 s 4 s 6 c 4 c 6
0

a1c 2
a1 s 2
0

r11
r
21
r31

r12
r22
r32
0

r13
r23
r33
0

c5 s3 c3 c 4 s 5
c3 c5 c 4 s 3 s5
s 4 s5
0

px
p y
px

a 2 a 3 c3 d 4 s3
d 4 c3 a3 s3

T6

(3.6)

From (3, 4 element) of equation (3.6), we obtain

s1 p x c1 p y 0

(3.7)

From (1, 4 element)

c1c 2 p x c 2 s1 p y s 2 p z a1c 2 a 2 a 3 c3 d 4 s3

(3.8)

From (2, 4 element)

c1 s 2 p x s1 s 2 p y c 2 p z a 1 s 2 d 4 c3 a3 s3

(3.9)

If we square equations (3.7) and (3.8) and add the resulting equation, we
obtain

c1 p x 2 s 1 p y 2 p 2z a 21 2c1 s1 p x p y 2c1a 1 p x 2s1 a1 p y

a 22 a 32 d 42 2a 2 a 3 c3 2a 2 d 4 s3

0)
If we square equation (3.7) and add the resulting equation to equation
(3.10), we obtain

p x2 p y2 p z2 a12 2a1 p x c1 2a1 p y s1 a 22 a32 d 42 2a 2 a3 c3 2a 2 d 4 s3

(3.1

p x2 p y2 p z2 a12 2a1 p x c1 2a1 p y s1 a 22 a32 d 42

2a 2

Let K =

(3.11)

Then,
K a 3 c3 d 4 s 3

(3.12)

3 A tan 2 a3 , d 4 A tan 2 K , a32 d 42 K 2

(3.13)

[0T3]-1[0T6] = [3T6]

s23

a1c23 a2 c3

s23c1 s23c1 c23

a1s23 a2 s3

c1c23

-1

[ T3] =

c1c 23
s c
23 1

s1

(3.14)

c23s1

s1

c1

c 23 s1
s 23c1
c1
0

s 23
c 23
0
0

a1c 23 a 2 c3
a1 s 23 a 2 s3

r11
r
21
r31

r12
r22
r32
0

r13
r23
r33
0

px
p y 3
T6
pz

(3.15)

Where 3T6 is given by equation (2.2). Equating both the (1, 4) element and
the (2, 4) elements from equation 5, we obtain the following,

c1c23 Px s1c23 Py s23 Pz a1c23 a2 c3 a3

c1s23 Px s1s23 Py c23 Pz a1s23 a2 s3 d4

(3.16)
(3.17)

Solving equation (3.16) and (3.17) simultaneously for s23 and c23 results to
the following:

s23=

c23=

Pz (a3 a2 c3 ) (d4 a2 s3 )(c1Px s1Py a1 )

Pz2 (c1Px s1Py a1 )2

(3.18)

Pz (d4 a2 s3 ) (a3 a2 c3 )(c1Px s1Py a1 )

Pz2 (c1Px s1Py a1 )2

(3.19)

From equation (3.17) and (3.18) above,

23 A tan 2[ Pz (a3 a2c3 ) (d4 a2 s3 )(c1Px s1Py a1 ), Pz (d 4 a2 s3 ) (a3 a2c3 )(c1Px s1Py a1 )]

These gives us four values for

23

, according to four possible values for

. Then using the following equation for possible solutions,

computed.

2 23 3
=

T
0

From

(3.20)

and

will be

(3.21)

1 0

. T6 3T6

(3.22)
Equating both the (1, 3) elements and the (3, 3) elements from the two sides
of equation (3.22), we get:
r13c1c 23 r23c 23 s1 r33 s 23 c 4 s5
r13 s1 r23 c1 s 4 s5

4 A tan 2 r13 s1 r23c1 , r13c1c 23 r 23 c 23 s1 r33 s 23 ; s5 0

(3.24)

To find 5 ;

0
4

0
4

T 4

T 4

T
1 0

4
5

T 5 6 T 6
5

s1 s 4 c1c 4 c 23
c s c s c
4 1 1 4 23

s 23 c1

T6 =
(3.27)

c5 c 6
s
6
s5 c6

(3.25)

c1 s 4 c 23c 4 s1
c1c 4 s1 s 4 c 23
s 23 s1

s 23 c 4
s 23 s 4
c 23

a1c 23c 4 a3 c 4 a 2 c 4 c 3
a1c 23 s 4 a 3 s 4 a 2 c3 s 4
d 4 a1 s 23 a 2 s3

c5 c6
c6
s5 s6
0

s5
0
c5
0

(3.26)

0
0
0

Equating both (1, 3) elements and the (3, 3) elements from the two sides of
equation (3.27), we get
r13 c1c 4 c 23 s1 s 4 r 23 (c 1 s 4 c 23 c 4 s1 ) r33 s 23 c 4 s 5
r13 c1 s 23 r 23 ( s 1 s 23 ) r33 c 23 c 5

(3.29)
(3.30)

From equation (3.25) and (3.26);

5 A tan 2 s 5 , c5

T
0
5

1 0
6

T 56 T 6

(3.31)

(3.32)

Equating (3, 1) elements and (1, 1) elements of equation (3.32), we have

s 6 r13 c1c 23 s 4 s1c 4 r 21( s 1 c 23 s 4 c 1 c 4 ) r31 s 23 s 4
c 6 r11 c5 s1 s 4 s 23c 1 s5 c 23c1c 4 c5 r21 c 23c 4 c5 s1 c1c5 s 4 s 23 s 1 s5 r31 s 23c 4 c5 c 23 s5

 6 A tan 2 s6 , c 6

(3.33)

FORWARD AND INVERSE KINEMATIC SOLUTIONS WITH MAT LAB

CODE

The forward kinetics of our robot was obtained using theta = [10 20 30 40 50 60].
The output of our robot is shown below:

T_0_6 =

- 0.3344 0.0315 - 0.9419

- 0.9424 - 0.0200 0.3339

- 0.0084 0.9993
0.0364

0
0
0

13.6517
2.4072
- 30.3073

The next step was to verify that our inverse kinematics would produce the same
angle that was used to obtain the forward kinematics. Putting T_0_6 into our inverse
kinematics matlab code yielded the following:

Theta =

20.0000
30.0000
40.00000 50.0000 60.0000
10.0000
170.0000 169.9055 160.1453 149.9414 100.5592 94.3944

We noticed that due to an offset a1 in our robot kinematics, we could only obtain
2 sets of theta. This sets of theta was obtained using for loop to remove the
unwanted results thus, our results are consistent.