FACULTY OF ENGINEERING AND BUILD ENVIRONMENT

DEPARTMENT OF MECHANICAL AND MATERIAL ENGINEERING

KKKP 4214 Automation and Robotics

Lab Report 2

Title:
Formulize The Forward And Inverse Kinematics Of The FANUC Robot

Lecturer:
Dr. Rizauddin Ramli

Group 2

Name

: CHAN KIEN HO

Matric No.

: A125070

Department

: JKMB/4

Due Date

: 11 June 2012

1.0

Dimension of the Fanuc Robot ARC Mate 100iC

2.0

Frame Assignment

1)

The joint and link configuration is simplified as shown in figure below:

d5 =100
d4= 860

x5

x4
5

x3

x6
6

z6

z4
z5

d6 =50

z3
d3=200
3

z2
d2=600

x2

z1
1

d1 =450

x1

*unit in millimeter (mm)

2)

The D-H algorithm table is shown as below:

Link

di

ai

450

-90

600

200

90

860

-90

100

90

50

*In this report, assume 6 = 0

3.0

Forward Kinematics Solution

Using the D-H transformation matrix to obtain forward kinematic

Matlab software is used to obtain

, script is written and then

evaluation is done in the Matlab command window as shown below:

The script file for all

is shown in the following page.

Let

for the corresponding D-H

matrices

The position and orientation of the robot tool frame with respect to the base frame are
shown in the transformation matrix

The computation is done using Matlab software, the results are:

*( cos(x5) means cos 5 and so forth )
nx = - cos(x5)*{sin(x1)*sin(x4) + cos(x4)*[cos(x1)*sin(x2)*sin(x3) cos(x1)*cos(x2)*cos(x3)]}- sin(x5)*[cos(x1)*cos(x2)*sin(x3) +
cos(x1)*cos(x3)*sin(x2)]
ny = cos(x5)*{cos(x1)*sin(x4) + cos(x4)*[cos(x2)*cos(x3)*sin(x1) sin(x1)*sin(x2)*sin(x3)]} - sin(x5)*(cos(x2)*sin(x1)*sin(x3) +
cos(x3)*sin(x1)*sin(x2)
nz = - sin(x5)*[cos(x2)*cos(x3) - sin(x2)*sin(x3)] - cos(x4)*cos(x5)*(cos(x2)*sin(x3) +
cos(x3)*sin(x2))
sx = sin(x4)*[cos(x1)*sin(x2)*sin(x3) - cos(x1)*cos(x2)*cos(x3)] - cos(x4)*sin(x1)
sy = cos(x1)*cos(x4) - sin(x4)*[cos(x2)*cos(x3)*sin(x1) - sin(x1)*sin(x2)*sin(x3)]
sz = sin(x4)*[cos(x2)*sin(x3) + cos(x3)*sin(x2)]
ax = cos(x5)*(cos(x1)*cos(x2)*sin(x3) + cos(x1)*cos(x3)*sin(x2)) sin(x5)*(sin(x1)*sin(x4) + cos(x4)*(cos(x1)*sin(x2)*sin(x3) cos(x1)*cos(x2)*cos(x3)))
ay = sin(x5)*(cos(x1)*sin(x4) + cos(x4)*(cos(x2)*cos(x3)*sin(x1) sin(x1)*sin(x2)*sin(x3))) + cos(x5)*(cos(x2)*sin(x1)*sin(x3) +
cos(x3)*sin(x1)*sin(x2))
az = cos(x5)*[cos(x2)*cos(x3) - sin(x2)*sin(x3)]- cos(x4)*sin(x5)*[cos(x2)*sin(x3) +
cos(x3)*sin(x2)]
px = 600*cos(x1)*cos(x2) - 200*sin(x1) - 100*cos(x4)*sin(x1) 50*cos(x5)*{sin(x1)*sin(x4) + cos(x4)*[cos(x1)*sin(x2)*sin(x3) cos(x1)*cos(x2)*cos(x3)]} + 100*sin(x4)*[cos(x1)*sin(x2)*sin(x3) cos(x1)*cos(x2)*cos(x3)] - 50*sin(x5)*[cos(x1)*cos(x2)*sin(x3) +
cos(x1)*cos(x3)*sin(x2)] + 860*cos(x1)*cos(x2)*sin(x3) +
860*cos(x1)*cos(x3)*sin(x2)
py = 200*cos(x1) + 100*cos(x1)*cos(x4) + 600*cos(x2)*sin(x1) +
50*cos(x5)*{cos(x1)*sin(x4) + cos(x4)*[cos(x2)*cos(x3)*sin(x1) sin(x1)*sin(x2)*sin(x3)]} - 100*sin(x4)*[cos(x2)*cos(x3)*sin(x1) sin(x1)*sin(x2)*sin(x3)] - 50*sin(x5)*[cos(x2)*sin(x1)*sin(x3) +
cos(x3)*sin(x1)*sin(x2)] + 860*cos(x2)*sin(x1)*sin(x3) +
860*cos(x3)*sin(x1)*sin(x2)
pz = 860*cos(x2)*cos(x3) - 600*sin(x2) - 860*sin(x2)*sin(x3) +
100*sin(x4)*[cos(x2)*sin(x3) + cos(x3)*sin(x2)] - 50*sin(x5)*[cos(x2)*cos(x3) sin(x2)*sin(x3)] - 50*cos(x4)*cos(x5)*[cos(x2)*sin(x3) + cos(x3)*sin(x2)]+ 450

4.0

Inverse Kinematics Solution

Since there is only 3 equation which is Px, Py and Pz , the five unknown 1 , 2 , 3 , 4 and
5 are unable to be solved. Therefore, the unknown are solved separately by reducing the
equation into 3 unknown.

Hence,
Px = 600*cos(x1)*cos(x2) - 200*sin(x1)

Py = 200*cos(x1) + 600*cos(x2)*sin(x1)

Pz = 450 - 600*sin(x2)

Multiplying

1 and
2 with

sin(x1) and cos(x1) respectively:

Px*sin(x1) = 600*cos(x1)*cos(x2)*sin(x1) 200*sin2(x1)

Py*cos(x1) =200*cos2(x1) + 600*cos(x1)*cos(x2)*sin(x1)
1
2

Px*sin(x1) - Py*cos(x1) = -200 [sin (x1) + cos (x1)]

Px*sin(x1) - Py*cos(x1) = -200

Let

Px = d*cos()
Py = d*sin()
d=

From
3 , rewrite equation:
600*sin(x2) = 450 - Pz

sin (x1 - ) =

( )

Solving the remaining unknown,

Hence,
Px = 860*sin(x3) - 100*cos(x3)*sin(x4)

Py = -860*cos(x3) - 100*sin(x3)*sin(x4)

Pz = 100*cos(x4) + 200

Multiplying

4 and
5 with

sin(x3) and cos(x3) respectively:

Px*sin(x3) = 860*sin2(x3) - 100*cos(x3)* sin(x3)*sin(x4)

Py*cos(x3) = -860*cos2(x3) - 100* cos(x3)*sin(x3)*sin(x4)
1
2

Px*sin(x3) - Py*cos(x3) = 860 [sin (x3) + cos (x3)]

Px*sin(x1) - Py*cos(x1) = 860

Let

Px = d*cos()
Py = d*sin()
d=

sin (x3 - ) =

( )

Solving the remaining unknown,

Hence,
Px = 50*cos(x4)*cos(x5) - 100*sin(x4)

Py = 100*cos(x4) + 50*cos(x5)*sin(x4)

Pz = 860 - 50*sin(x5)

Multiplying
7 and
8 with sin(x4) and cos(x4) respectively:
Px*sin(x4) = 50*cos(x4)*cos(x5)*sin(x4) - 100*sin2(x4)
Py*cos(x4) = 100*cos2(x4) + 50* cos(x4)*cos(x5)*sin(x4)
1
2

Px*sin(x4) - Py*cos(x4) = -100 [sin (x4) + cos (x4)]

Px*sin(x4) - Py*cos(x4) = -100

Let

Px = d*cos()
Py = d*sin()
d=

From
9 , rewrite equation:
50*sin(x5) = 860 - Pz
=

sin (x4 - ) =

( )