robotic lab 2

8/10/2019 Robotic Lab 2

1/15

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


2/15

1.0 Dimension of the Fanuc Robot ARC Mate 100iC


3/15

2.0 Frame Assignment

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

*unit in millimeter (mm)

2) The D-H algorithm table is shown as below:

Link i di ai i

1 1 450 0 -90

2 2 0 600 0

3 3 200 0 90

4 4 860 0 -905 5 100 0 90

6 0 0 50 0

*In this report, assume 6 = 0

z1

x1

z2

x2

x3

z3

x4

z4

x5

z5

x6

z6

1

2

3

4

5

6

d5 =100

d1 =450

d2=600

d3=200

d4= 860

d6 =50


4/15

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 thenevaluation is done in the Matlab command window as shown below:

The script file for all , , is shown in the following page.


5/15


6/15


7/15


8/15

Let , , for the corresponding D-Hmatrices

The position and orientation of the robot tool frame with respect to the base frame are

shown in the transformation matrix .


9/15

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


10/15

4.0 Inverse Kinematics Solution

Since there is only 3 equation which is Px, Pyand 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) 1

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

Pz = 450 - 600*sin(x2) 3


11/15

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)

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

2(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

(

)

1

sin (x1 - ) =


12/15

Solving the remaining unknown,

=

Hence,

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

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

Pz = 100*cos(x4) + 200 6


13/15


14/15

Solving the remaining unknown,

=

Hence,

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

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

Pz = 860 - 50*sin(x5) 9


15/15

Multiplying 7 and8 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)

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

2(x4)]

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

Let Px = d*cos()

Py = d*sin()

d =

From9 , rewrite equation:

50*sin(x5) = 860 - Pz

=

(

)

1

sin (x4 - ) =

robotic lab 2

Documents