1

Robot Kinematics II

Introduction to ROBOTICS

2

Outline

• Review– Manipulator Specifications

• Precision, Repeatability– Homogeneous Matrix

• Denavit-Hartenberg (D-H) Representation

• Kinematics Equations• Inverse Kinematics

3

Review

• Manipulator, Robot arms, Industrial robot – A chain of rigid bodies (links) connected by

joints (revolute or prismatic)• Manipulator Specification

– DOF, Redundant Robot– Workspace, Payload – Precision– Repeatability

How accurately a specified point can be reached

How accurately the same position can be reached if the motion is repeated many times

4

Review• Manipulators:

Cartesian: PPP Cylindrical: RPP Spherical: RRP

SCARA: RRP(Selective Compliance Assembly Robot Arm)

Articulated: RRR

Hand coordinate:n: normal vector; s: sliding vector;

a: approach vector, normal to the

tool mounting plate

5

Review• Basic Rotation Matrix

uvwxyz RPP =

xyzuvw QPP =

TRRQ == −1

uvw

w

v

u

z

y

x

xyz RPppp

ppp

P =⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

⋅⋅⋅⋅⋅⋅⋅⋅⋅

=⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

=

wzvzuz

wyvyuy

wxvxux

kkjkikkjjjijkijiii

x

z

y

v

wP

u

6

Basic Rotation Matrices– Rotation about x-axis with

– Rotation about y-axis with

– Rotation about z-axis with

uvwxyz RPP =

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−=θθθθθ

CSSCxRot

00

001),(

0

0100

),(⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−=

θθ

θθθ

CS

SCyRot

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

10000

),( θθθθ

θ CSSC

zRot

θ

θ

θ

7

Review• Coordinate transformation from {B} to {A}

• Homogeneous transformation matrix

'oAPBB

APA rrRr +=

⎥⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡

× 1101 31

' PBoAB

APA rrRr

⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡= ××

× 10101333

31

' PRrRT

oAB

A

BA

Position vector

Rotation matrix

Scaling

8

Review• Homogeneous Transformation

– Special cases1. Translation

2. Rotation

⎥⎦

⎤⎢⎣

⎡=

×

×

100

31

13BA

BA RT

⎥⎦

⎤⎢⎣

⎡=

×

×

10 31

'33

oA

BA rIT

9

Review

• Composite Homogeneous Transformation Matrix

• Rules:– Transformation (rotation/translation) w.r.t. (X,Y,Z)

(OLD FRAME), using pre-multiplication– Transformation (rotation/translation) w.r.t.

(U,V,W) (NEW FRAME), using post-multiplication

10

Review• Homogeneous Representation

– A point in space

– A frame in space

3R

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=⎥⎦

⎤⎢⎣

⎡=

10001000 zzzz

yyyy

xxxx

pasnpasnpasn

PasnF

x

y

z ),,( zyx pppP

ns

a⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

1z

y

x

ppp

P Homogeneous coordinate of P w.r.t. OXYZ

3R

11

Review• Orientation Representation

(Euler Angles) – Description of Yaw, Pitch, Roll

• A rotation of about the OX axis ( ) -- yaw

• A rotation of about the OY axis ( ) -- pitch

• A rotation of about the OZ axis ( ) -- roll

X

Y

Z

ψθ

φψ

θ

φ

ψ,xR

θ,yR

φ,zR

yaw

pitch

roll

12

Quiz 1• How to get the resultant rotation matrix for YPR?

X

Y

Z

ψθ

φ

ψθφ ,,, xyz RRRT =

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

100001000000

φφφφ

CSSC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−100000001000

θθ

θθ

CS

SC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

100000000001

ψψψψ

CSSC

13

Quiz 2• Geometric Interpretation?

•

⎥⎦

⎤⎢⎣

⎡= ××

101333 PR

T

⎥⎦

⎤⎢⎣

⎡ −=−

101 PRR

TTT

441

100

1010 ×− =⎥

⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡⎥⎦

⎤⎢⎣

⎡ −= I

RRPRPRRTT

TTT

Position of the origin of OUVW coordinate frame w.r.t. OXYZ frame

Inverse of the rotation submatrixis equivalent to its transposePosition of the origin of OXYZ reference frame w.r.t. OUVW frame

Orientation of OUVW coordinate frame w.r.t. OXYZ frame

Inverse Homogeneous Matrix?

14

Kinematics Model• Forward (direct) Kinematics

• Inverse Kinematics

),,( 21 nqqqq L=

),,,,,( ψθφzyxY =

x

y

z

Direct Kinematics

Inverse Kinematics

Position and Orientationof the end-effector

Jointvariables

15

Robot Links and Joints

16

Denavit-Hartenberg Convention• Number the joints from 1 to n starting with the base and ending with

the end-effector. • Establish the base coordinate system. Establish a right-handed

orthonormal coordinate system at the supporting base with axis lying along the axis of motion of joint 1.

• Establish joint axis. Align the Zi with the axis of motion (rotary or sliding) of joint i+1.

• Establish the origin of the ith coordinate system. Locate the origin of the ith coordinate at the intersection of the Zi & Zi-1 or at the intersection of common normal between the Zi & Zi-1 axes and the Ziaxis.

• Establish Xi axis. Establish or along the common normal between the Zi-1 & Zi axes when they are parallel.

• Establish Yi axis. Assign to complete the right-handed coordinate system.

• Find the link and joint parameters

),,( 000 ZYX

iiiii ZZZZX ××±= −− 11 /)(

iiiii XZXZY ××+= /)(

0Z

17

Example I• 3 Revolute Joints

a0 a1

Z0

X0

Y0

Z3

X2

Y1

X1

Y2

d2

Z1

X33O

2O1O0O

Z2

Joint 1

Joint 2

Joint 3

Link 1 Link 2

18

Link Coordinate Frames• Assign Link Coordinate Frames:

– To describe the geometry of robot motion, we assign a Cartesian coordinate frame (Oi, Xi,Yi,Zi) to each link, as follows:

• establish a right-handed orthonormal coordinate frame O0 at the supporting base with Z0 lying along joint 1 motion axis.

• the Zi axis is directed along the axis of motion of joint (i + 1), that is, link (i + 1) rotates about or translates along Zi;

Link 1 Link 2

a0 a1

Z0

X0

Y0

Z3

X2

Y1

X1

Y2

d2

Z1

X33O

2O1O0O

Z2

Joint 1

Joint 2

Joint 3

19

Link Coordinate Frames– Locate the origin of the ith coordinate at the intersection

of the Zi & Zi-1 or at the intersection of common normal between the Zi & Zi-1 axes and the Zi axis.

– the Xi axis lies along the common normal from the Zi-1axis to the Zi axis , (if Zi-1 is parallel to Zi, then Xi is specified arbitrarily, subject only to Xi being perpendicular to Zi);

iiiii ZZZZX ××±= −− 11 /)(

a0 a1

Z0

X0

Y0

Z3

X2

Y1

X1

Y2

d2

Z1

X33O

2O1O0O

Z2

Joint 1

Joint 2

Joint 3

20

Link Coordinate Frames– Assign to complete the right-

handed coordinate system.• The hand coordinate frame is specified by the geometry

of the end-effector. Normally, establish Zn along the direction of Zn-1 axis and pointing away from the robot; establish Xn such that it is normal to both Zn-1 and Zn axes. Assign Yn to complete the right-handed coordinate system.

iiiii XZXZY ××+= /)(

nO

a0 a1

Z0

X0

Y0

Z3

X2

Y1

X1

Y2

d2

Z1

X33O

2O1O0O

Z2

Joint 1

Joint 2

Joint 3

21

Link and Joint Parameters• Joint angle : the angle of rotation from the Xi-1 axis to

the Xi axis about the Zi-1 axis. It is the joint variable if joint i is rotary.

• Joint distance : the distance from the origin of the (i-1) coordinate system to the intersection of the Zi-1 axis and the Xi axis along the Zi-1 axis. It is the joint variable if joint i is prismatic.

• Link length : the distance from the intersection of the Zi-1axis and the Xi axis to the origin of the ith coordinate system along the Xi axis.

• Link twist angle : the angle of rotation from the Zi-1 axis to the Zi axis about the Xi axis.

iθ

id

ia

iα

22

Example I

Joint i αi ai di θi

1 0 a0 0 θ0

2 -90 a1 0 θ1

3 0 0 d2 θ2

D-H Link Parameter Table

: rotation angle from Xi-1 to Xi about Zi-1iθ: distance from origin of (i-1) coordinate to intersection of Zi-1 & Xi along Zi-1

: distance from intersection of Zi-1 & Xito origin of i coordinate along Xi

id

: rotation angle from Zi-1 to Zi about Xi

iaiα

a0 a1

Z0

X0

Y0

Z3

X2

Y1

X1

Y2

d2

Z1

X33O

2O1O0O

Z2

Joint 1

Joint 2

Joint 3

23

Example II: PUMA 260

iiiii ZZZZX ××±= −− 11 /)(

iiiii XZXZY ××+= /)(

1θ2θ

3θ

4θ

5θ6θ

0Z

1Z

2Z

3Z

4Z5Z

1O

2O3O

5O4O

6O

1X1Y

2X2Y

3X

3Y

4X

4Y5X

5Y6X

6Y

6Z

1. Number the joints

2. Establish base frame

3. Establish joint axis Zi

4. Locate origin, (intersect. of Zi & Zi-1) OR (intersect of common normal & Zi )

5. Establish Xi,Yi

PUMA 260

t

24

Link Parameters

1θ2θ

3θ

4θ

5θ6θ

0Z

1Z

2Z

3Z

4Z5Z

1O

2O3O

5O4O

6O

1X1Y

2X2Y

3X

3Y

4X

4Y5X

5Y6X

6Y

6Z

: angle from Zi-1 to Ziabout Xi

: distance from intersectionof Zi-1 & Xi to Oi along Xi

Joint distance : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1

: angle from Xi-1 to Xiabout Zi-1

iθ

iα

ia

id

t0060090580-904

00903802

130-901J iθ

1θ

4θ

2θ3θ

6θ5θ

iα ia id

-l

25

Transformation between i-1 and i• Four successive elementary transformations

are required to relate the i-th coordinate frame to the (i-1)-th coordinate frame:– Rotate about the Z i-1 axis an angle of θi to align the

X i-1 axis with the X i axis.– Translate along the Z i-1 axis a distance of di, to bring

Xi-1 and Xi axes into coincidence.– Translate along the Xi axis a distance of ai to bring

the two origins Oi-1 and Oi as well as the X axis into coincidence.

– Rotate about the Xi axis an angle of αi ( in the right-handed sense), to bring the two coordinates into coincidence.

26

Transformation between i-1 and i• D-H transformation matrix for adjacent coordinate

frames, i and i-1.– The position and orientation of the i-th frame coordinate

can be expressed in the (i-1)th frame by the following homogeneous transformation matrix:

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

−

= −−−

10000

),(),(),(),( 111

iii

iiiiiii

iiiiiii

iiiiiiiii

i

dCSSaCSCCSCaSSSCC

xRaxTzRdzTT

ααθθαθαθθθαθαθ

αθ

Source coordinate

ReferenceCoordinate

27

Kinematic Equations • Forward Kinematics

– Given joint variables– End-effector position & orientation

• Homogeneous matrix – specifies the location of the ith coordinate frame w.r.t.

the base coordinate system– chain product of successive coordinate transformation

matrices of

⎥⎦

⎤⎢⎣

⎡=⎥

⎦

⎤⎢⎣

⎡=

= −

100010000

12

11

00

nnn

nn

n

PasnPR

TTTT K

),,( 21 nqqqq L=

),,,,,( ψθφzyxY =

iiT 1−

nT0

Orientation matrix

Position vector

28

Kinematics Equations• Other representations

– reference from, tool frame

– Yaw-Pitch-Roll representation for orientation

tooln

nref

toolref HTBT 0

0=

ψθφ ,,, xyz RRRT =

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

100001000000

φφφφ

CSSC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−100000001000

θθ

θθ

CS

SC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

100000000001

ψψψψ

CSSC

29

Solving forward kinematics

• Forward kinematics

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

⇒

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

ϕθφ

θθθθθθ

z

y

x

p

pp

6

5

4

3

2

1

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

1000zzzz

yyyy

xxxx

pasnpasnpasn

T

• Transformation Matrix

30

Solving forward kinematics• Yaw-Pitch-Roll representation for orientation

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−−++−

=

1000

0z

y

x

n

pCCSCSpSCCSSCCSSSCSpSSCSCCSSSCCC

Tψθψθθ

ψφψθφψφψθφθφψφψθφψφψθφϑφ

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

1000

0zzzz

yyyy

xxxx

n

pasnpasnpasn

T

)(sin 1zn−= −θ

)cos

(cos 1

θψ za−=

)cos

(cos 1

θφ xn−=

Problem? Solution is inconsistent and ill-conditioned!!

31

atan2(y,x)

x

y

⎪⎪⎩

⎪⎪⎨

⎧

−+≤≤−−−−≤≤−

+−≤≤++≤≤

==

yandxforyandxfor

yandxforyandxfor

xya

oo

oo

oo

oo

09090180

18090900

),(2tan

θθθθ

θ

32

Yaw-Pitch-Roll Representationψθφ ,,, xyz RRRT =

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

100001000000

φφφφ

CSSC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−100000001000

θθ

θθ

CS

SC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

100000000001

ψψψψ

CSSC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

1000000

zzz

yyy

xxx

asnasnasn

33

Yaw-Pitch-Roll Representation

ψθφ ,,1, xyz RRTR =−

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

100001000000

φφφφ

CSSC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−=

100000001000

θθ

θθ

CS

SC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

100000000001

ψψψψ

CSSC

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

1000000

zzz

yyy

xxx

asnasnasn

(Equation A)

34

Yaw-Pitch-Roll Representation

0cossin =⋅+⋅− yx nn φφ

⎩⎨⎧

−==⋅+⋅

θθφφ

sincossincos

z

yx

nnn

⎩⎨⎧

−=⋅+⋅−=⋅+⋅−

ψφφψφφ

sincossincoscossin

yx

yx

aass

• Compare LHS and RHS of Equation A, we have:

),(2tan xy nna=φ

)sincos,(2tan yzz nnna ⋅+⋅−= φφθ

)cossin,cos(sin2tan yxyx ssaaa ⋅+⋅−⋅−⋅= φφφφψ

35

Kinematic Model

• Steps to derive kinematics model:– Assign D-H coordinates frames– Find link parameters– Transformation matrices of adjacent joints– Calculate Kinematics Matrix– When necessary, Euler angle representation

36

Example

Joint i αi ai di θi

1 0 a0 0 θ0

2 -90 a1 0 θ1

3 0 0 d2 θ2

a0 a1

Z0

X0

Y0

Z3

X2

Y1

X1

Y2

d2

Z1

X33O

2O1O0O

Z2

Joint 1

Joint 2

Joint 3

37

ExampleJoint i αi ai di θi

1 0 a0 0 θ0

2 -90 a1 0 θ1

3 0 0 d2 θ2

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

10000100

0cosθsinθ0sinθcosθ

00

00

000

00

1 sincos

θθ

aa

T

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−

−

=

1000000

sinθcosθ

1

1

1 1sincos0cossin0

111

111

2 θθθθ

aa

T

))()(( 2103213

0 TTTT =⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

1000

0sinθcosθ 22

22

223

10000cossin0

dT

θθ

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

−

=−

100001

iii

iiiiiii

iiiiiii

ii dCS

SaCSCCSCaSSSCC

Tαα

θθαθαθθθαθαθ

38

Example: Puma 560

39

Example: Puma 560

40

Link Coordinate Parameters

Joint i θi αi ai(mm) di(mm)

1 θ1 -90 0 0 2 θ2 0 431.8 149.09

3 θ3 90 -20.32 0

4 θ4 -90 0 433.075 θ5 90 0 0 6 θ6 0 0 56.25

PUMA 560 robot arm link coordinate parameters

41

Example: Puma 560

42

Example: Puma 560

43

Inverse Kinematics• Given a desired position (P)

& orientation (R) of the end-effector

• Find the joint variables which can bring the robot the desired configuration

),,( 21 nqqqq L=

x

y

z

44

Inverse Kinematics• More difficult

– Systematic closed-form solution in general is not available

– Solution not unique• Redundant robot• Elbow-up/elbow-down

configuration – Robot dependent

(x , y)l2

l1l2

l1

45

Inverse Kinematics

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

6

5

4

3

2

1

θθθθθθ

65

54

43

32

21

10

1000

TTTTTTpasnpasnpasn

Tzzzz

yyyy

xxxx

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

• Transformation Matrix

Special cases make the closed-form arm solution possible:

1. Three adjacent joint axes intersecting (PUMA, Stanford)

2. Three adjacent joint axes parallel to one another (MINIMOVER)