MANF 482 Introduction to Robotics

Mechanics

Project Report

Spring 2013

Presented By:

Ahmed Hassan Ibrahim

MoustafaSherif Ibrahim

Ahmed MagedAshraf

PUMA 560

Introduction

Puma 560 Manipulator

Inventor

Drawing

Robot Workspace Analysis

Kinematics

Frame Assignments

Contents

Simplified Diagram

DH Parameters Table

PUMA 560 Workspace

Homogeneous Transforms

Matlabfunction

Parametric Curve

Jacobian

Dynamic of Robot Manipulator

Introduction

ThePUMA(ProgrammableUniversalMachinefor Assembly, or Programmable UniversalManipulationArm) is an industrial robot armdevelopedby Victor Scheinmanat pioneeringrobot companyUnimation. Initially developedfor GeneralMotors, the PUMAwasbasedonearlier designsScheinmaninvented while atStanfordUniversity.

Puma 560 Manipulator

The PUMA 560 hasSIXrevolute joints.A revolute joint has ONE degree of freedom ( 1 DOF) that is defined by its angle.

Inventor/Puma.ipjInventor/Puma.ipj

Inventor

Drawing

Kinematics

Forward Kinematics (angles to position)What you are given:

The length of each linkThe angle of each joint

What you can find:

coordinates)

Frame Assignments

Spherical joint

a3x4

y4

x5

z5

x3

y3

x3

z3

d4

Simplified Diagram

DH Parameters Table

0 0 -90

0

0 90

4 0 -90

5 0 0 90

6 0 0

Link Parameters

Robot Workspace Analysis

Usually this workspace is defined in the

following manner: a geometricalobject that

will includeall possiblelocationof a specific

point of the platform and ranges for the

parametersthat describetheorientationof the

platform.

PUMA 560 Workspace

Side view Top view

1000

00

00

0001

1000

0100

0010

001

1000

100

0010

0001

1000

0100

00

00

ii

iiii

ii

cs

sc

a

d

cs

sc

T

i

i

These four DH parameters,

iiii ad

represent the following homogeneous matrix:

1000

0 i

i

i

dcs

sascccs

casscsc

ii

iiiiii

iiiiii

Homogeneous Transforms

1000

0010

00

00

11

11

1

0cs

sc

T

1000

100

00

00

6

6

5 6

6

d

cs

sc

T i

i

1000

0010

00

00

55

55

5

4cs

sc

T

1000

010

00

00

4

4

3 44

44

d

cs

sc

T

1000

0010

0

0

333

333

3

3

3

2sacs

casc

T

1000

100

0

0

2

2

2

2

1 222

222

d

sacs

casc

T

MATLAB Code: Transformation

syms q1 q2 q3 q4 q5 q6 a2 a3 d2 d4 d6

T0_1=[cos(q1),0,-1*sin(q1),0;sin(q1),0,cos(q1),0;0,-1,0,0;0,0,0,1];

T1_2= [cos(q2),-1*sin(q2),0,a2*cos(q2);sin(q2),cos(q2),0,a2*sin(q2);

0,0,1,d2;0,0,0,1];

T2_3= [cos(q3),0,sin(q3),a3*cos(q3);sin(q3),0,-1*cos(q3),a3*sin(q3);

0,1,0,0;0,0,0,1];

T3_4=[cos(q4),0,-1*sin(q4),0;sin(q4),0,cos(q4),0;0,-1,0,d4;0,0,0,1];

T4_5=[cos(q5),0,sin(q5),0;sin(q5),0,-1*cos(q5),0;0,1,0,0;0,0,0,1];

T5_6=[cos(q6),-1*sin(q6),0,0;sin(q6),cos(q6),0,0;0,0,1,d6;0,0,0,1];

DH.m

Matlab/DH.m

T0_2=T0_1*T1_2

T0_3=T0_2*T2_3

T0_4=T0_3*T3_4

T0_5=T0_4*T4_5

T0_6=T0_5*T5_6

Remember those double-angle

formulas

sincoscossinsin

sinsincoscoscos

The following abbreviations have been used here

)sin(),cos(),sin(),cos( jiijjiijii scsc

1000

0 2222

1221212121

1221212121

2

1

1

0

2

0

sacs

cdcsacsscs

sdccassccc

TTT

1000

0010

00

00

11

11

1

0cs

sc

T

1000

0

)(

)(

23322232332

122332212311231

122332212311231

3

2

2

1

1

0

3

0

sasacscsc

cdcacasssccs

sdcacacscscc

TTTT

1000

34333231

24232221

14131211

4

0

aaaa

aaaa

aaaa

T

4

3

3

2

2

1

1

0

4

0 TTTTT

2332

23122

23112

23431

41423121

41423111

ca

ssa

sca

sca

scccsa

ssccca

2332223434

1223322234124

1223322234114

23433

42311423

42311413

)(

)(

sasacda

cdcacasdsa

sdcacasdca

ssa

scscca

sccsca

1000

34333231

24232221

14131211

5

0

aaaa

aaaa

aaaa

T

5

4

4

3

3

2

2

1

1

0

5

0 TTTTTT

42332

42311422

42311412

235452331

5231234141521

5231234141511

)(

)(

ssa

scscca

sccsca

sccsca

sssccsscca

sscsccssca

2332223434

1223322234124

1223322234114

235423533

2341415235123

4231415235113

)(

)(

)(

)(

sasacda

cdcacasdsa

sdcacasdca

ssccca

ccsscsscsa

cccsssscca

222334235423523634

254612223342352354236124

254612223342352354236114

523542333

5415235423123

5415235423113

6523646542332

64654165236465423122

64654165236465423112

6523646542331

64654165236465423121

64654165236465423111

)(

)())((

)())((

)(

)(

)(

)())((

)())((

)(

)())((

)())((

sasadcccsccda

dssdccacadscssccdsa

dssdscacadscssccdca

ccscsa

ssccssccsa

ssscssccca

cscsscccsa

scccsccsscsscccsa

scccsscsscsscccca

cscsscccsa

scccsccssssccccsa

scccsscssssccccca

1000

34333231

24232221

14131211

6

0

aaaa

aaaa

aaaa

T

6

5

5

4

4

3

3

2

2

1

1

0

6

0 TTTTTTT

Matlabfunction

click here

Matlab/DHmain.m

Robotics Toolbox For Matlab

Purpose construct a robot object

P560

Forward Kinematic

Purpose compute forward kinematics

q = [0 -pi/4 -pi/4 0 pi/8 0]

T = fkine(p560, q)

Purpose Drive a graphical robot

drivebot(p560)

Kinematics

Inverse Kinematics (position to angles)What you are given:

The length of each linkThe position of some point on the robot

What you can find:The angles of each joint needed to obtain that position

Figure: the schematic representation of the forward and inverse kinematics

Number of Solutions

It depends on:

1. Number of Joints

2. Link Parameters

Multiple Solutions

Figure: Four solutions of the inverse position kinematics for the PUMA manipulator.

Geometric Solution Approach

Geometric solution approach is based on

decomposing the spatial geometry of the

manipulator into several plane geometry

problems. It is applied to the simple robot

structures,suchas,2-DOF planermanipulator

ARM SOLUTION FOR THE FIRST THREE

JOINTS OF A PUMA ROBOT ARM

we define a position vector p which points

from the origin of the shoulder coordinate

system (xo , yo , zo) to the point where the last

three joint axes intersect which corresponds to

the position vector of T04:

2332223434

1223322234124

1223322234114

)(

)(

sasacdp

cdcacasdsp

sdcacasdcp

Joint One Solution

If we project the position vector p onto the

x0-y0 plane as in Figure, we obtain the

following equations for solving :

where the superscript L/R on joint angles indicates

the LEFT/RIGHT arm configurations.

Combining Eqs. and using the ARM indicator

to indicate the LEFT/RIGHT arm

configuration, we obtain the sine and cosine

functions of 0, respectively:

Joint Two Solution

To find joint 2, we project the position

vector p onto the x1-y1 plane as shown in

Figure. From Figure, we have four different

arm configurations. Each arm configuration

corresponds to different values of joint two

as:

Table 1

ARM ELBOW

KELBOWARMArm Configurations

-1+1-1LEFT and ABOVE arm

+1-1-1LEFT and BELOW arm

+1+1+1RIGHT and ABOVE arm

-1-1+1RIGHT and BELOW arm

From the above table, 2 can be expressed in one

equation for different arm and elbow configurations

using the ARM and ELBOW indicators as:

Joint Three Solution

For joint 3, we project the position vector p

onto the x2-y2 plane as shown in Figure.

From Figure, we again have four different

arm configurations. Each arm configuration

corresponds to different values of joint three

as:

Table 2

Arm Configurations ARM ELBOW ARM ELBOW

LEFT and ABOVE arm o -1 +1 -1

LEFT and BELOW arm o -1 -1 +1

RIGHT and ABOVE arm 0 +1 +1 +1

RIGHT and BELOW arm o +1 -1 -1

where is the y-component of the position

vector from the origin of ( ) to the point

where the last three joint axes intersect.

From the arm geometry in Figure 8, we obtain the

following equations for finding the solution for 3:

From Table 2, we obtain the equation for 3:

ARM SOLUTION FOR THE LAST THREE

JOINTS OF A PUMA ROBOT ARM

Knowing the first three joint angles, we can

evaluate the T03 matrix which is used

extensively to find the solution of the last

three joints. The solution of the last three

joints of a PUMA robot arm can be found

by setting these joints to meet the following

criteria:

1. Set joint 4 such that a rotation about joint 5

will align the axis of motion of joint 6 with

the given approach vector (a of T)

2. Set joint 5 to align the axis of motion of

joint 6 with the approach vector.

3. Set joint 6 to align the given orientation

vector ( or sliding vector or y6 ) and

normal vector.

Mathematically the above criteria respectively

mean:

In Eq. 43, the vector cross product may be

taken to be positive or negative.

Joint Four Solution

Both orientations of the wrist (UP and

DOWN) are defined by looking at the

orientation of the hand coordinate frame

(n,s,a) with respect to the

coordinate frame. The sign of the vector cross

product in Eq. 43 cannot be determined

without referring to the orientation of either

the n or s unit vector with respect to the x5 or

y5 unit vector, respectively,

We shall start with an assumption that the vector

cross product in Eq. 43 has a positive sign.

From Figure 2, and using Eq. 43, the

orientation indicator n can be rewritten as:

Table 3

Wrist Orientation WRISTM

= WRIST- sign{ )

DOWN > o +1 + 1

DOWN < 0 +1 -1

UP > 0 -1 -1

UP < o -1 +1

Again looking at the projection of the coordinate

frame on the x3-y3 plane and from the

Table 3 and Figure 9, it can be shown that the

followings are true (see Figure 9):

where and are the x and y column vector of T03

respectively, M= WRIST-sign( ), and the sign

function is defined as:

In addition to this, the user can turn on the FLIP toggle

to obtain the other solution of , that is

Joint Five Solution

To find , we use the criterion that aligns

the axis of rotation of joint six with the

approach vector ( or a = ). Looking at the

projection of the coordinate frame

( ) on the plane, it can be

shown that the followings are true (see

Figure 10):

where and are the x and y column vector of

respectively and & is the approach vector. Thus the

solution for is:

If , then the degenerate case occurs.

Joint Six Solution

Up to now, we have aligned the axis of joint

6 with the approach vector. Next we need to

align the orientation of the gripper to ease

picking up the object. The criterion for

doing this is to set . Looking at the

projection of the hand coordinate frame

(n,S,a) on the plane, it can be shown

that the followings are true (see Figure 11):

where y5 is the y column vector of and n and B

are the normal and sliding vectors of

respectively. Thus the solution for is:

In summary, there are eight solutions to the

inverse kinematics problem of a six-joint

PUMA robot arm. There are four solutions for

the first three joint solutions - two for the right

shoulder arm configuration and two for the

left shoulder arm configuration. For each arm

configuration, Eqs. 26, 35, 42, 49, 52, and 54

give one set of solution ( )

and ( ) (with the

FLIP toggle on) give another set of solution.

More Simplification

Elbow manipulator with shoulder offset.

in general, be only two solutions for

Thesecorrespondto the so-called left arm

and right arm configurationsas shown in

Figures4.6 and 4.7. Figure 4.6 showsthe

left arm configuration. Fromthis figure, we

seegeometricallythat

Figure 4.6: Left arm configuration.

Figure 4.7: Right arm configuration.

Algebraic Solution Approach

For the manipulatorswith more links and

whose arm extends into 3 dimensionsthe

geometry gets much more tedious. Hence,

algebraicapproachis chosenfor the inverse

kinematicssolution.

Mathematically,we rearrangeequationso that

we isolate the homogeneoustransformations

thatarea functionof theunknownjoint values

and somehowsolve for the joint values by

applyingthefollowing equation:

(joint values unknown)

(right side known)

What are the joint angles as a

function of the wrist position and orientation (

or when is given as numeric values)

1000

333231

232221

131211

6

5

5

4

4

3

3

2

2

1

1

0

6

0

z

y

x

paaa

paaa

paaa

TTTTTTT

Solution (General Technique): Multiplying each

side of the direct kinematics equation by a an

inverse transformation matrix for separating out

variables in search of solvable equation

Put the dependence on on the left hand side of the

equation by multiplying the direct kinematics eq.

with gives

Algebraic Solution for UNIMATION PUMA 560

)()()()()()(

1000

6

5

65

4

54

3

43

2

32

1

21

0

1

333231

232221

131211

0

6 TTTTTTprrr

prrr

prrr

Tz

y

x

We wish to Solve

For i when 06T is given as numeric values

(4.54)

Paul (1981) suggest pre multiplying the

above matrix by its unworn inverse

transform successively and determine the

unknown angle from the elements of the

resultant matrix equation.

After all eight solutions have been computed,

some of them may have to be discarded

because of joint limitations. Usually the one

closest to present manipulator configuration is

chosen.

Robotics Toolbox For Matlab

q = [0 -pi/4 -pi/4 0 pi/8 0];

T = fkine(p560, q)

ikine(p560, T)

Parametric Curve

Suppose that x and y are both given asfunctions of a third variable t (called aparameter) by theequations

x = f(t); y = g(t)

(called parametric equations). Each value of t determines a point (x; y); which we can plot in a coordinate plane. As t varies, the point (x; y) = (f(t); g(t)) varies and traces out a curve C; which we call a parametric curve.

circle

Ellipse

Circle Involute

Spiral

Butterfly Curve

Jacobian

The function of the Jacobian

Matrix is to describe the

relationshipbetweenthe velocity

vector of the robot hand and the

velocity vectorof thejoints

v

ii

bzJi ,1

654321

654321

JJJJJJ

JJJJJJ vvvvvv

5

0

4

0

3

0

2

0

1

0

0

0 zzzzzzJ

1

0

0

0zb

0

1

1

1 c

s

zb

23

231

231

2

c

ss

sc

zb

234

423114

423114

3

ss

scscc

sccsc

zb

2354235

23414152351

42314152351

4 )(

)(

ssccc

ccsscsscs

cccsssscc

zb

5235423

54152354231

54152354231

5 )(

)(

ccscs

ssccssccs

ssscssccc

zb

1st method

11 i

b

eff

b

i

b

v ppzJ i

Cross Product of Two Vectors

Example

2nd method

q

xJ

iv

2223342354235236

2546122233423523542361

2546122233423523542361

)(

)())((

)())((

sasadcccsccdp

dssdccacadscssccdsp

dssdscacadscssccdcp

z

y

x

0

2546122233423523542361

2546122233423523542361

1)dss(ds)cacads)cssc(c(dc

)dss(dc)cacads)cssc(c(ds

Jv

654321 vvvvvvvJJJJJJJ

2223342354235236

22233423523542361

22233423523542361

)(

))((

))((

2

cacadsccccsd

sasadcccscsds

sasadcccscsdc

Jv

23342354235236

233423523542361

233423523542361

)(

))((

))((

3

cadsccccsd

sadcccscsds

sadcccscsdc

Jv

)(

)()(

)()(

54236

5461542316

5461542316

4

cssd

scdcsscsd

scdsssccd

Jv

)(

)()(

)()(

54235236

546152316

546152316

5

scsscd

csdcsssd

csdssscd

Jv

0

0

0

6vJ

Matlab

jacob0(p560, q)

compute Jacobean in base coordinate frame

jacobn(p560, q)

compute Jacobean in end-effector coordinate frame

tr2diff(T)

homogeneous transform to differential motion vector

tr2jac(T)

homogeneous transform to Jacobean

Dynamic of Robot Manipulator

Dynamic equation is the study of motion with regard

to forces. Dynamic modeling is vital for

control, mechanical design, and simulation. It is used

to describe dynamic parameters and also to

describe the relationship between displacement,

velocity and acceleration to force acting on robot

manipulator.

PUMA 560 robot Dynamics

)().,().( qGqqqVqqM

q: position vector

M(q): inertia matrix of the manipulator

V(q, ): vector of Centrifugal and Coriolis

terms

G(q): vector of gravity terms

T: vector of torques

where,

B(q) nx(n-1)/2 matrix of Coriolis torques

C(q): nxnmatrix of Centrifugal torques

: n(n-1)/2x1 vector of joint velocity products given by:

: nx1 vector given by:

)(]).[(].).[().( 2 qGqqCqqqBqqM

Recall that only three links of PUMA robot

are used in this thesis,

With, Matrix A is a symmetric 6x6 matrix:

66

55

44

35333231

232221

131211

00000

00000

00000

00

000

000

)(

a

a

a

aaaa

aaa

aaa

qA

Where,

23.23.2.23.23.2.23.2..2

23.2.23.23.2.

221615125

211110731111

SCISCISSICCISCI

SSISCISCISSICCIIIa m

23.23.2.23.2. 181398412 CISICICISIa

23.23.23. 1813813 CISICIa

3.2.3..2 161512562222 SIICISIIIIa m

1516126523 .23.3.3. ISICIISIa

156333 .2 IIIa m

171535 IIa

14444 IIa m

17555 IIa m

23666 IIa m

1221 aa

1331 aa

2332 aa

While matrix B is:

000000000000000

00000000000000

000000000000

00000000000000

00000000000

00000000000

)(

514

415413412

314

235225223214

123115113112

b

bbb

b

bbbb

bbbb

qB

)23.21.()]23.21.(

23.23.2.23.2.23.2.23.23.2..[2

1022

2116151275113

SSISSI

SCICCISCISCISCICCIb

]23.23.2.23.23.[2 221615115 CCICCISCISCb

]23.23.23..[2 18138123 SICISIb

)5.01.(23..223.23. 201914214 SISISIb

]3.3.3..[2 16512223 CICISIb

]3..[2 2216225 ICIb

]3..[2 2216235 ICIb

)2.21.()23.21.()]23.21.(

23.223.23.2.223.23.223.2..[2

111022

21161512753112

SSISSISSI

SCICISCISISCICISCIb

23.23.)]5.01.(23..[2 191420314413 SISISIbb

23.23. 1720415 SISIb

23.23. 1720415514 SISIbb

23.23.)]5.01.(23..[2 191420314 SISISIb

)]5.01.(23..223.23.[ 201914214412 SISISIbb

Matrix C is:

000000

0000

000000

0000

0000

0000

)(

5251

3231

2321

1312

cc

cc

cc

cc

qC

23.23.2.23.2. 181398412 SICISISICIc

23.23.23..5.0 1813812313 SICISIbc

)2.21.(.5.0)23.21.(.5.0)23.21.(

23.223.23.2.223.23.223.2..5.0

111022

2116151275311221

SSISSISSI

SCICISCISISCICISCIbc

3.3.3..5.0 1651222323 CICISIbc

)23.21.(.5.0)23.21.(

23.23.2.23.2.23.2.23.23.2..5.0

1022

211615127511331

SSISSI

SCICCISCISCISCICCIbc

3.3.3. 165122332 CICISIcc

23.23.2.23.23.5.0 22161511551 CCICCISCISCbc

221622552 3..5.0 ICIbc

And matrix G is:

0

0

0

)(

5

3

2

g

g

g

qg

23.23.2.23.2. 543212 SgCgSgSgCgg

23.23.23. 5423 SgCgSgg

23.55 Sgg