Submitted by

Mali Vilas Nagnath

(13MT07CDM005)

Rahul M. R.

(13MT07CDM008)

Rashmi Ranjan Nayak (13MT07CDM009)

Sunny Surraf

(13MT07CDM011)

[CASE STUDY ON 6-DOF ROBOT]

Finding forward kinematic equation, inverse kinematics, C-space of robot, gripper selection
on the basis of application, joint space trajectory and Cartesian space trajectory, time
required for completion of task.

Definitions :
(a) Link Length(ai) Distance measured along the xi axis from the point of
intersection of xi axis with zi-1 axis (point C)to the origin of the frame {i},
that is distance CD.
(b)Link twist (i) - Angle between zi-1 and zi axes measured about xi axis in the
right hand sense.
(c) Joint Distance (di) Distance measured along z i-1 axis from the origin of
the frame {i-1} (point B) to the intersection of xi axis with zi-1 axis (point
C),that is distance BC.
(d)Joint Angle (i) - Angle between xi-1 and xi axes measured about zi-1 axis in
the right hand sense.

Fig.: Fanuc r2000i robot

Frame assignment :

1. D-H parameter of the fanuc R2000i robot:

J1
L1
90o
d1
1

ai
i
di
i

T1

T2

J2
L2
0
0
2

J3
L3
90o
0
3

J4
0
-90o
d4
4

C1
S1
0
0

0
0
1
0

s1
-c1
0
0

l1*c1
l1*s1
d1
1

c2
s2
0
0

-s2
c2
0
0

0
0
1
0

l2*c2
l2*s2
0
1

C3
S3

0
0

s3
-c3

l3*c3
l3*s3

J5
0
90o
0
5

J6
0
0
d6
6

T3

T4

T5

T6

0
0

1
0

C4
S4
0
0

0
0
-1
0

C5
S5
0
0

0
0
-1
0

C6
S6
0
0

-s6
c6
0
0

0
0
-s4
C4
0
0
-s5
c5
0
0
0
0
1
0

0
1
0
0
d4
1
0
0
0
1
0
0
d6
1

2. Forward kinematic equation using syms toolbox in matlab can

obtained as follows:
The Matlab program for forward kinematic equation is :

3. Inverse kinematics using the methods given in Mittals book:

From the above forward kinematic equation we can able to calculate the value of

, 3 , 4, 5, 6 . The value of 1, 2 , 3 from the above FWK (0T6 ) can

not be obtained directly. To get the solution for these angles inverse transpose
approach is used.
1

Step-1: premultiplying the above 0T6 by (0T1)-| , this will give the solution for 1 as
follow, equating

[(0T1)-|*T] = 1T6

{(3,2), (3,3) compared with (3,2),(3,3) of [1T6]}

-S4S5 = S1ax C1ay
-d6S4S5 = S1Px C1Py
Therefore,
d6 = ( S1Px C1Py ) / (S1ax C1ay )
( S1axd6 C1axd6 ) = S1Px C1Py
S1(axd6 Px ) = C1 ( ayd6 Py )
S1 / C1 = ( ayd6 - Py ) / ( axd6 - Px )
1 = tan-1[(axd6 Py ) / ( axd6 Px )]
Therefore,
1 = Atan2 [(ayd6 Py ) , ( axd6 - Px )]

{(2,3), (2,4) compared with (2,3),(2,4) of [1T6]}

( -d1 + Pz ) = d6 ( S2C3C4S5 S2S3C5 C2S3S5C4 + C2C3C5) + S2S4d5 d4C2C3 + L2S2
( -d1 + Pz ) = d6az + d4 ( S2S3 C2C3 ) + L2S2
= d6az + d4 ( C12 ) + L2S2
C1Px + S1PyL1 = d6 (C1ax + S1ay ) + d4 ( S12 ) + L2C2
L2S2 = -d1 + Pz d6az
L2C2 = C1Px + S1Py L1 d6 ( C1ax + S1ay )
Therefore,
1

2=tan

d 1+ P z d 6 az
C 1 P x + S 1 P y L1d 6 ( C 1 ax + S1 a y )

Now , for the solution of

again premultiplying on both sides by

(1T2 )-|*(0T1)-|*T= 2T6

(1T2 )-|*(0T1)-|*T=

nzs 2+c 2(c 1 nx +nys 1), oz s 2+ c 2( c 1 ox+ oys 1) azs 2+c 2( axc 1+ays 1) s 2(d 1+ pz )l2+ c 2(c 1 pxl 1
c 2 nzs 2(c 1nx +nys 1) c 2 ozs 2(c 1 ox+ oys 1) azc 2s 2(axc 1+ ays 1)
c 2( d 1+ pz)s 2(c 1 pxl 1+
nx s 1c 1 ny oxs 1c 1 oy
axs 1ayc 1
pxs 1c 1 py
0
0
0
1

Comparing {(2,3), (2,4) with (2,3),(2,4) of [2T6]}

-S2C1Px S1S2Py + L1S2 + C2Pz d1C2 = d6 (-S2C1ax S1S2ay + C2az ) d4C3

Comparing {(1,3), (1,4) with (1,3),(1,4) of [2T6]}

C1C2Px S1C2Py + L1C2 + S2Pz d1S2 L2 = d6 (-C1C2ax S1C2ay + S2az ) + d4S3

So by simplifying :-S2C1Px S1S2Py + L1S2 + C2Pz d1C2 - d6 (-S2C1ax S1S2ay + C2az ) = d4C3
C1C2Px S1C2Py + L1C2 + S2Pz d1S2 L2 - d6 (-C1C2ax S1C2ay + S2az ) = d4S3
then,
tan3 = - [C1C2Px S1C2Py + L1C2 + S2Pz d1S2 L2 - d6 (-C1C2ax S1C2ay + S2az)]
[-S2C1Px S1S2Py + L1S2 + C2Pz d1C2 - d6 (-S2C1ax S1S2ay + C2az ) ]
Therefore,
3=tan1

C 1 C 2 P x S1 C 2 P y + L1 C2 + S2 Pz d 1 S 2L2d 6 (C 1 C 2 a x S 1 C 2 a y + S 2 az )
S 2 C 1 P x S 1 C 2 P y + L1 S 2+C 2 P zd 1 C 2d 6 (S 2 C 1 a x S1 S 2 a y +C2 a z )

Step 2:
To obtain the solution for the remaining three joints displacement 4, 5, 6
another approach will be used . It is possible to view the description of tol frame ,
frame {6} , with frame {3} , the arm end point frame , along two different paths.
0
0

T1

T2

T3

T4

Path 2

T5

T6

Path 1

Path 1 :
Frame {3}

Frame {4}

Frame {5}

Frame {6}

Along this path the transformation matrix can be obtained.

3

T6 = 3T4 * 4T5 * 5T6

c4 c5 c8+ s4 s6
3

T6

s4c6-c4c5s6

S4 c5 c6-c4 s6
-s5c6

-s4c5s6-c4c6
s5s6

- c 4 s5

d6 s5 c4

- s4s5
-c5

-d6s4s5
d4-d6c5

Path 2 :
Frame {3}

Frame{2}

Frame{1}

Frame{0}

Frame{6}

This path is via base , hence it can be reached from Frame {3} transversing the
links of the arm , thus
3

T6 = 3T2* 2T1 *1T0 *0T6

i-1

Ti = [ iTi-1 ]-1

And
3

T6 = [ 2T3 ]-1 [ 1T2 ]-1 [ 0T1 ]-1 T

(0T6 = T)

Since 1, 2, 3 are known , the matrix 2T3 1T2 & 0T1 and their inverse can be
computed . Substituting these values and multiplying the matrix , 3T6 can be
computed . Let it be denoted as ,

r11 r12 r13 r14

T6

r21 r22 r23 r24

r31 r32 r33 r34

r41 r42 r43 r44

Joint 4 :
From (A) and (B), comparing element (2,4) 0f [A] with (2,4) of [B] and
(1,4) of [A] with (1,4) of [B] gives ,
-d6 s4 s6 = r24 and -d6 s5 c4 = r14
On dividing we get ,
S4
c4

r 24
r 24

1 r 24
4 = tan r 24

Joint 6 :
Comparing element (3,1) (3,2) 0f [A] with (3,1) (3,2) of [B] ,
S6
c6

r32
r 31

1 r 3 2
6 = tan r 31

Joint 5 :
Comparing ,
- c4 s5 = r13

and

- s4s5 =

r23

Squaring and adding , and simplifying

c42 s52 + s42 s52 = r132 + r232
s52 = r132 + r232
Now comparing ,
C52 = r232
Divide to s52 by C52 , we get
s 25 r 213 +r 223
= 2
c25
r 33
r 213 +r 223
r 233

( tan 5 )2 =
Taking square root we get ,
5

= tan

r 213 +r 223
r 233

4. Contructing C-space for robot using kinematic constraint are as

follows:
The algorithm used for constructing C-space of robot in matlab is,

The output of the above mat-lab program gives the following C-space:

Fig. C-space for Fanuc r2000i robot

Fig. Configuration space for fanuc r2000i robot

5. The given application is grinding of truck wheel:

The task of the robot is to grasp the wheel of the truck in the gripper,
move it to the grinding wheel and align over the grinding wheel until the
desired finishing of the truck wheel is not obtained.
After finishing the grinding operation place it to the safe place. The
same operation is repeated for the entire wheel. Also we can mount two
adjustable gripper with rotary motion for increasing the speed of
application.
6. Considering the given application (truck wheel manufacturing) the
selection of gripper can be based on the following points.
Robot grippers or end effectors can roughly be classified into five categories:
Mechanical: Two or more fingers or jaws for external/internal grasping.
Vacuum: One or more vacuum cups for handling nearly flat work pieces that are
reasonably smooth and clean.
Magnetic: One or more electromagnets or permanent magnets for handling
ferrous materials.
Expandable: Inflatable bag or cuff to handle odd shapes and fragile materials,
also inflatable prehensile fingers.
Adhesive: For soft materials (like cloth) that are of lightweight and not suitable
for vacuum techniques.
The industrial robots use grippers as an end-effector for picking up the raw and
finished work parts. A robot can perform good grasping of objects only when it
obtains a proper gripper selection and design. Therefore, several factors that are
required to be considered in gripper selection and design are,
The gripper must have the ability to reach the surface of a work part.
The change in work part size must be accounted for providing accurate
positioning.

 During machining operations, there will be a change in the work part size.
As a result, the gripper must be designed to hold a work part even when the
size is varied.
The gripper must not create any sort of distort and scratch in the fragile
work parts.
The gripper must hold the larger area of a work part if it has various
dimensions, which will certainly increase stability and control in
positioning.
The gripper can be designed with resilient pads to provide more grasping
contacts in the work part. The replaceable fingers can also be employed for
holding different work part sizes by its interchangeability facility.
Moreover, it is difficult to find out the magnitude of gripping force that a
gripper must apply to pick up a work part. The following significant factors must
be considered to determine the necessary gripping force.
Consideration must be taken to the weight of a work part.
It must be capable of grasping the work parts constantly at its centre of mass.
The speed of robot arm movement and the connection between the direction
of movement and gripper position on the work part should be considered.
It must determine either friction or physical constriction helps to grip the
work part.
It must consider the co-efficient of friction between the gripper and work
part.

Now , the application is grinding of truck wheel for finishing purpose.

For this application the gripper with adjustable mechanism is more suitable.

Fig. Adjustable gripper mechanism.

7. Joint angle trajectory for the robot using inverse kinematics
equation:

c1 *(c2*c3-s2*s3)
s1
c1*(c2*s3+c3*s2)
c1*(l2*c2+l3*c2*c3-l3*s2*s3)+ l1*c1
0
T3
=
s1*(c2*c3-s2*s3)
-c1 s1*(c2*s3+c3*s2)
s1*(l2*c2+l3*c2*c3-l3*s2*s3)+l1*s1
c2*s3+c3*s2
0
s2*s3-c2*c3
+l3*c2*s3+l3*c3*s2
0
0
0
1

r11 r12 r13 r14

0

T3

= r21 r22 r23 r24

r31 r32 r33 r34
r41 r42 r43 r44

By comparing above two equations ,

For 1

d1+l2*s2

1 =

tan1

r 12
r 22

For 2
2 =

tan 1

r 34 l3 r 31d 1
s
r 24+ l3 r 33 +l 1 s 1 1

For 3
3 =

tan 1

r 31
r 33

For initial position of the end effector specified by Ts as follows give the values of
1s, 2s, 3s

1s = -18.43
2s = -26.04
3s = -7.60
Similarly, values of 1g ,2g ,3g corresponding to the final position of the endeffector as specified by the Tg.

1g = 40
2g = 1
3g = 80
The joint velocitys of joint 1 , 2 ,3 are 110/sec given in the manual. So we can
get time required for each joint for positioning End Effector as follows ,
t1 = 0.5 sec
t2 = 0.25 sec
t3 = 2 sec
Suppose total time required for moving the End Effector is 2sec .
Now to plot the trajectory planning in joint space we use cubic polynomial
equation as follows,
(t) = c0 + c1*t + c2*t2 + c3*t3
d
dt

= c1+ 2*c2*t2 + 3*c3*t2

To obtain the continuity gives the following four constraints:

(0) = 1s
(tg) = 1g

(0) = 0

(tg) = 0

The program used for cubic polynomial trajectory in matlab is:

By using above algorithms we got the cubic polynomial trajectory for

different joints as follow:

8. Jacobian of position matrix:

jacobian of the manipulator defines a linear mapping between the vector q of
the joint velocities and end-effector velocity Ve , from joint space to Cartesian
space.
Pi-1=0Ri-1
O

PO = RO. =

[]
0
0
1

i-1

Pn= OTn On OTi-1 On

J1 =

P o P3
Po

[ ]

P3 = OT3 On OT1 On =

J2 =

P1 1 P3
P1

P2 = OR2 =

S1 ( L2 C 2+ L3 C 23+ L1 )
C 1 ( L2 C 2 + L3 C 23 )
0
0
0
1

S1
C 1
0

P1 = OR1 =

P3 = OT3 On OTO On =

C 1 ( L2 C 2+ L3 C23 ) + L1 C1
S1 ( L2 C 2+ L3 C23 ) + L1 S 1
d 1+ L2 S 2 + L3 S23
0

C 1 ( L2 C 2+ L3 C23 )
S 1 ( L2 C2 + L3 C23 )
L2 S2 + L3 S 23+ S2
0

[ ]
C 1 ( L2 S 2+ L3 S23 )
S1 ( L2 S 2+ L3 S 23 )
L2 C 2 +L3 C 23
S1
C 1
1

[ ]
S1
C 1
0

P3 = OT3 On OT2 On =

J3 =

P1 1 P3
P1

[ ]
L3 C23
L3 C23
L3 S23
0

[ ]
C 1 L3 S 23
S 1 L3 S 23
L3 C23 ( S1C 1 )
S1
C1
1

J =[ J 1 J 2 J 3 ]

C 1 L3 S 23
S 1 ( L2 C2 + L3 C23 + L1 ) C1 ( L2 S2 + L3 S 23 )
S1 L3 S23
S1 ( L2 S2 + L3 S 23 )
C 1 ( L2 C 2+ L3 C23 )
L3 C 23 ( S 1C1 )
L2 C 2+ L3 C23

0
S1
S1
0
0
C 1
C 1
1
1
1

To convert the joint space velocity in to Cartesian space following equation is

helpful, in which Jacobian matrix used,

[]
5146.7
9847.4
4500.5
20.100
9.000
11.000

Ve= J.
q

V e=