[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.

Submitted by

Mali Vilas Nagnath (13MT07CDM005)

Rahul M. R. (13MT07CDM008)

Rashmi Ranjan Nayak (13MT07CDM009)

Sunny Surraf (13MT07CDM011)

2013

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 J2 J3 J4 J5 J6

ai L1 L2 L3 0 0 0αi 90o 0 90o -90o 90o 0di d1 0 0 d4 0 d6

Θi Θ1 Θ2 Θ3 Θ4 Θ5 Θ6

C1 0 s1 l1*c1

S1 0 -c1 l1*s1

0T1 = 0 1 0 d1

0 0 0 1

c2 -s2 0 l2*c2

s2 c2 0 l2*s2

1T2 = 0 0 1 0 0 0 0 1

C3 0 s3 l3*c3

S3 0 -c3 l3*s3

2T3 = 0 1 0 0 0 0 0 1

C4 0 -s4 0 S4 0 C4 0 3T4 = 0 -1 0 d4

0 0 0 1

C5 0 -s5 0 S5 0 c5 0 4T5 = 0 -1 0 0 0 0 0 1

C6 -s6 0 0 S6 c6 0 0 5T6 = 0 0 1 d6

0 0 0 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

ɵ1, θ2,θ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.

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 + S1Py–L1 = d6 (C1ax + S1ay ) + d4 ( S12 ) + L2C2

L2S2 = -d1 + Pz – d6az

L2C2 = C1Px + S1Py – L1 –d6 ( C1ax + S1ay )

Therefore,

θ2=tan−1[ −d1+P z−d6 az

C1 Px+S1 P y−L1−d6 (C1ax+S1 ay ) ]Now , for the solution of θ3 again premultiplying on both sides by

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

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

[nzs2+c 2(c 1nx+nys 1) , oz s2+c2(c1ox+oys1) azs2+c 2(axc 1+ays1) s2(d 1+ pz)−l 2+c 2(c1 px−l1+ pys1)c2nz−s2(c1nx+nys1) c2 oz−s2(c1 ox+oys1) azc 2−s2(axc1+ays1) c 2(d 1+ pz )−s2(c 1 px−l 1+ pys1)

nx s1−c1 ny0

oxs1−c 1oy0

axs1−ayc 10

pxs1−c1 py1 ]

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,

tanɵ3 = - [C1C2Px – S1C2Py + L1C2 + S2Pz – d1S2 – L2 - d6 (-C1C2ax – S1C2ay + S2az)] ÷ [-S2C1Px – S1S2Py + L1S2 + C2Pz – d1C2 - d6 (-S2C1ax – S1S2ay + C2az ) ]

Therefore,

θ3=tan−1[C1 C2 Px−S1C 2 P y+L1C2+S2 P z−d1 S2−L2−d6 (−C1 C2 ax−S1C2 ay+S2 az )−S2 C1 Px−S1C2 Py+L1 S2+C2 P z−d1 C2−d6 (−S2C1 ax−S1 S2 ay+C2az ) ]

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.

0T1 1T2 2T3 3T4 4T5

5T6

0 1 2 3 4 5 6

Path 2 Path 1

Path 1 :

Frame {3} Frame {4} Frame {5} Frame {6}

Along this path the transformation matrix can be obtained.3T6 = 3T4 * 4T5 * 5T6

c4 c5 c8+ s4 s6 s4c6-c4c5s6 - c4 s5 d6 s5 c4

S4 c5 c6-c4 s6 -s4c5s6-c4c6 - s4s5 -d6s4s5 A 3T6 = -s5c6 s5s6 -c5 d4-d6c5

0 0 0 1

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 3T6 = 3T2* 2T1 *1T0 *0T6 (0T6 = T)

And i-1Ti = [ iTi-1 ]-1

3T6 = [ 2T3 ]-1 [ 1T2 ]-1 [ 0T1 ]-1 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

3T6 = r21 r22 r23 r24 Br31 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 ,

S 4c 4 =

r 24r 24

θ4 = tan−1 r 24r 24

Joint θ6 :

Comparing element (3,1) (3,2) 0f [A] with (3,1) (3,2) of [B] ,

S 6c 6 =

r3 2r31

Θ6 = tan−1 r 32r 31

Joint θ5 :

Comparing ,

- c4 s5 = r13 and - s4s5 = r23

Squaring and adding , and simplifying

c42 s5

2 + s42 s5

2 = r132 + r23

2

s52 = r13

2 + r23

2

Now comparing ,

C52 = r23

2

Divide to s52 by C5

2 , we gets5

2

c52=

r132 +r23

2

r332

( tan θ5 )2 = r13

2 +r232

r332

Taking square root we get ,

θ5 = tan−1 √ r132 +r23

2

r332

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 arereasonably smooth and clean.

• Magnetic: One or more electromagnets or permanent magnets for handlingferrous 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 suitablefor 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 0T3 = 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 d1+l2*s2 +l3*c2*s3+l3*c3*s2 0 0 0 1

r11 r12 r13 r14

0T3 = r21 r22 r23 r24

r31 r32 r33 r34

r41 r42 r43 r44

By comparing above two equations ,

For θ1

θ1 =tan−1 −r12

r22

For θ2

Θ2 =tan−1 r34−l3 r31−d1

r24+l3 r33+l1 s1∗s1

For θ3

Θ3 =tan−1 r31

−r33

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 end-effector as specified by the Tg.

θ1g = 40˚

θ2g = 1˚

θ3g = 80˚

The joint velocity’s 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 û

PO = ORO.û =[001]i-1Pn= OTn On – OTi-1 On

OP3 = OT3 On – OTO On = [C1 ( L2C2+ L3C23)+L1 C1

S1 ( L2C2+ L3C23)+L1 S1

d1+ L2 S2+L3 S23

0]

J1 = [Po× ° P3

Po ] = [−S1 ( L2 C2+L3 C23+L1 )

C1 ( L2 C2+L3 C23 )0001

]P1 = OR1û =[ S1

−C1

0 ]1P3 = OT3 On – OT1 On = [C1 ( L2C2+ L3C23)

S1 ( L2C2+L3 C23 )L2 S2+L3 S23+S2

0]

J2 = [P1× P3❑1

P1 ] = [−C1 ( L2 S2+L3 S23 )−S1 ( L2 S2+L3 S23)

L2 C2+L3 C23

S1

−C1

1]

P2 = OR2û =[ S1

−C1

0 ]2P3 = OT3 On – OT2 On = [L3C23

L3C23

L3 S23

0 ] J3 = [P1× P3❑

1

P1 ] = [−C1 L3 S23

−S1 L3 S23

L3C23 ( S1−C1 )S1

−C1

1]

J= [J 1 J 2 J3 ]

¿ [−S1 ( L2C2+L3C23+ L1 )

C1 ( L2C2+L3C23 )0001

−C1 ( L2 S2+L3 S23 )−S1 ( L2 S2+L3 S23 )

L2C2+L3 C23

S1

−C1

1

−C1 L3 S23

−S1 L3 S23

L3 C23 (S1−C1)S1

−C1

1]

To convert the joint space velocity in to Cartesian space following equation is helpful, in which Jacobian matrix used,

Ve= [5146.79847.44500.520.100−9.00011.000

] Ve= J.q̇