Introduction to ROBOTICS

Robot Kinematics II
Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu

The City College of New

Outline
Review
Manipulator Specifications
Precision, Repeatability

Homogeneous Matrix

Denavit-Hartenberg (D-H)
Representation
Kinematics Equations
Inverse Kinematics
The City College of New

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
How accurately a specified point can be reached
Precision
accurately the same position can be reached
Repeatability How
if the motion is repeated many times
The City College of New

Review
Manipulators:

Cartesian: PPP

Cylindrical: RPP

Spherical: RRP

Hand coordinate:
Articulated: RRR

SCARA: RRP

n: normal vector; s: sliding vector;

(Selective Compliance
Assembly Robot Arm)

a: approach vector, normal to the

The City College of New

tool mounting plate

Review
Basic Rotation Matrix
px i x i u
Pxyz p y jy i u
p z k z i u

Pxyz RPuvw

i x jv
j y jv
k z jv

ix k w
jy k w
k z k w

pu
p RP
uvw
v
pw
z

P v

Puvw QPxyz

Q R 1 R T

x
The City College of New

Basic Rotation Matrices

Rotation about x-axis with

1 0
Rot ( x, ) 0 C
0 S

0
S
C

Rotation about y-axis with

C
Rot ( y, ) 0
S

Rotation about z-axis with

Pxyz RPuvw

C
Rot ( z , ) S
0

The City College of New

0
1

S
0

0 C
S
C
0

0
0

Review
Coordinate transformation from {B} to {A}
r AR B B r P A r o '

A P

Ar P

A RB

013

B P

r
r

1 1

A o'

Homogeneous transformation matrix

RB

TB

013

R33

1 0

A o'

P31

Scaling
The City College of New

Rotation
matrix
Position
vector

Review
Homogeneous Transformation
Special cases
1. Translation

I 33

A o'

013

A RB

031

013

TB

2. Rotation

TB

The City College of New

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 postmultiplication

The City College of New

Review
Homogeneous Representation
A point in R 3 space
P

px
p
y

pz

Homogeneous coordinate of P w.r.t. OXYZ

1
3

A frame in R space
nx s x

n s a P ny s y
F

0 0 0 1 nz s z

0 0

ax
ay
az
0

px
p y
pz

The City College of New

P( p x , p y , pz )
a

s
n
y

x
10

Review
Orientation Representation
(Euler Angles)
Description of Yaw, Pitch, Roll
A rotation of about the OX
axis ( Rx , ) -- yaw
A rotation of about the OY
axis ( R y , ) -- pitch
yaw
A rotation of about the OZ
axis ( Rz , ) -- roll
X

The City College of New

roll

pitch

11

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

T Rz , R y , Rx ,
C
S

S
C
0
0

0
0
1
0

0
0
0

C
0

0 S
1 0
0 C
0 0

0
0
0

1 0
0 C

0 S

0 0

0
S
C
0

0
0
0

The City College of New

Y
12

Quiz 2
Geometric Interpretation?

R33
T
0

P31

Orientation of OUVW coordinate

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

Inverse Homogeneous Matrix?

T
T
Inverse of the rotation submatrix

R
P
1
is equivalent to its transpose
T

Position of the origin of OXYZ

0
1

reference frame w.r.t. OUVW frame

T

R
1
T T
0

RT P

R P R T R 0
I 44
0 1

0 1

The City College of New

13

Kinematics Model
Forward (direct) Kinematics

q (q1 , q2 , qn )
J o in t
v a r ia b le s
D ir e c t K in e m a t ic s

P o s it io n a n d O r ie n t a t io n
o f th e e n d -e ffe c to r

y
x

I n v e r s e K in e m a t ic s

Y ( x, y, z , , , )

Inverse Kinematics

The City College of New

14

Robot Links and Joints

The City College of New

15

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 ( X 0 , Y0 , Z 0 ) at the supporting base with
Z 0 lying along the axis of motion of joint 1.
axis
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 Zi
axis.
Establish Xi axis. Establish X i ( Z i 1 Z i ) / Z i 1 Z i or along the
common normal between the Zi-1 & Zi axes when they are parallel.
Establish Yi axis. Assign Yi ( Z i X i ) / Z i X i to complete the right-

handed coordinate system.

Find the link and joint parameters

The City College of New

16

Example I
3 Revolute Joints
Z1

Z0
Y0

Joint 3
Y1

Link 1
Joint 1

Z3

O0 X0

Joint 2

O3

X3

d2

Link 2

O1 X1 O2 X2
Y2

a0

a1

The City College of New

17

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 O 0 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;
Z1

Z0
Y0

Joint 3
Y1

Link 1
Joint 1

Z3

O0 X0

Joint 2

O3

X3

d2

Link 2

O1 X1 O2 X2
Y2

a0

a1

The City College of New

18

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-1
axis to the Zi axis X i ( Z i 1 Z i ) / Z i 1 Z i , (if Z i-1 is
parallel to Zi, then Xi is specified arbitrarily, subject only
Z
to Xi being perpendicularZ to Zi);
3

Z0

Y0

Joint 1

Joint 3

Y1

O3

X3

d2
O0 X0

Joint 2

O1 X1 O2 X2
Y2

a0

a1

The City College of New

19

Link Coordinate Frames

Assign Yi ( Z i X i ) / Z i X i to complete the righthanded coordinate system.
The hand coordinate frame is specified by the geometry
On
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
Z3
system.
Z1

Z0
Y0

Joint 1

Joint 3
Y1

O3

X3

d2
O0 X0

Joint 2

O1 X1 O2 X2
Y2

a0

a1

The City College of New

20

Link and Joint Parameters

Joint angle i : 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.

di

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.

ai

Link length : the distance from the intersection of the Z i-1

axis 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.
The City College of New

21

Example I
Z1

Z0
Y0

Joint 1

Z3

Joint 3
Y1

O3

d2
O0 X0

Joint 2

O1 X1 O2 X2
Y2

a0

a1

i : rotation angle from Zi-1 to Zi about Xi

ai : distance from intersection of Zi-1 & Xi
di

X3

D-H Link Parameter Table

Joint i

ai

di

a0

-90

a1

d2

to origin of i coordinate along Xi

: distance from origin of (i-1) coordinate to intersection of Z i-1 & Xi along Zi-1

i : rotation angle from X to X about Z

i-1
i
i-1
The City College of New

22

Example II: PUMA 260

1
O1

Z1

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 )

X1
Z2 Z6
5.
Y1
O2
Y3
Z
Z4
O3
X 2 5 6 Y6
Y2
5
O
Z0
6
Y
Y5
X3 4
t
O5
X5 X6
O4 Z 3
X4

Establish Xi,Yi

X i ( Z i 1 Z i ) / Z i 1 Z i
Yi ( Z i X i ) / Z i X i

PUMA 260
The City College of New

23

Link Parameters
1
O1

Z1

1
2

X1
Z2 Z6
Y1
O
Y3 2
Z
Z4
O3
X 2 5 6 Y6
Y2
5
O
Z0
6
Y
Y5
X3 4
O5
X5 X6
O4 Z 3

3
4
5
6

i
1
2
3
4
5
6

ai d i

-90 0

13

90

-90 0

-l
8

90

i : angle from X to X
i-1
i
Zi-1
about
i : angle from Z

i-1

to Zi

about Xi
X4
4 ai : distance from intersection
of Zi-1 & Xi to Oi along Xi
Joint distance d i : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1
The City College of New

24

Transformation between i-1 and i

Four successive elementary transformations are
required to relate the i-th coordinate frame to the (i1)-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 righthanded sense), to bring the two coordinates into
coincidence.
The City College of New

25

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:
Source coordinate

Ti i 1 T ( zi 1 , d i ) R ( zi 1 , i )T ( xi , ai ) R ( xi , i )
Reference
Coordinate

C i
S
i

C i S i
C i C i
S i

S i S i
S i C i
C i

ai C i
ai S i
di

The City College of New

26

Kinematic Equations
q (q1 , q2 , qn )

Forward Kinematics

Given joint variables

End-effector position & orientation Y ( x, y , z , , , )
n
Homogeneous matrix T0

specifies the location of the ith coordinate frame w.r.t.

the base coordinate system
chain product of successive coordinate transformation
i
T
matrices of i 1

T T T T
n
0

Orientation
matrix

R0n

1 2
0 1

n
n 1

Position
vector

P0n n s a P0n

1 0 0 0 1

The City College of New

27

Kinematics Equations
Other representations
reference from, tool frame
0
Treftool Bref
T0n H ntool

Yaw-Pitch-Roll representation for orientation

T Rz , R y , Rx ,
C
S

S
C
0
0

0
0
1
0

0
0
0

C
0

0 S
1 0
0 C
0 0

The City College of New

0
0
0

1 0
0 C

0 S

0 0

0
S
C
0

0
0
0

28

Representing forward kinematics

Forward kinematics

px
p
y

pz

Transformation Matrix
nx
n
T y
nz

sx
sy
sz
0

ax
ay
az
0

px
p y
pz

The City College of New

29

Representing forward kinematics

Yaw-Pitch-Roll representation for orientation
CC CSS SC
SC SSS CC
T0n
S
CS

0
0

T
n
0

Problem?

nx
n
y
nz

sx

ax

sy

ay

sz
0

az
0

px

p y
pz

CSC SS
SSC CS
CC
0

sin 1 (nz )

px
p y
pz

az
cos (
)
cos
nx
1
cos (
)
cos
1

Solution is inconsistent and illconditioned!!

The City College of New

30

atan2(y,x)
y

a tan 2( y, x)

0 90
90 180

180

90

90 0

for x and y
for x and y
for x and y
for x and y

The City College of New

31

Yaw-Pitch-Roll Representation
T Rz , R y , Rx ,
C
S

S
C
0
0

0
0
0

sx

ax

sy

ay

nz

sz

az

C
0

0
0
1
0

nx
n
y

0 S
1 0
0 C
0 0

0
0
0

1 0
0 C

0 S

0 0

0
S
C
0

0
0
0

0
0

The City College of New

32

Yaw-Pitch-Roll Representation
1
z ,

R T R y , Rx ,

C
S

S
C
0
0

C
0

0
1

0
0
1
0
S
0

0 C
0 0

0
0
0

nx
n
y
nz

sx
sy
sz

ax
ay
az

0
0
0

0 1 0

0 0 C
0 0 S
0 0
1

S
C

0
0

(Equation A)

The City College of New

33

Yaw-Pitch-Roll Representation
Compare LHS and RHS of Equation A, we have:

sin nx cos n y 0

a tan 2(n y , nx )

cos nx sin n y cos

nz sin

a tan 2(nz , cos nz sin n y )

sin s x cos s y cos

sin a x cos a y sin

a tan 2(sin a x cos a y , sin s x cos s y )
The City College of New

34

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

The City College of New

35

Example
Z1

Z0
Y0

Joint 1

Z3

Joint 3
Y1

O3

X3

d2
O0 X0

Joint 2

O1 X1 O2 X2
Y2

a0

a1

Joint i

ai

di

a0

-90

a1

d2

The City College of New

36

Example
Joint i

ai

di

a0

-90

a1

d2

Ti i1

C i
S
i

C i S i
C i C i
S i
0

S i S i
S i C i
C i
0

T (T )(T )(T )
3
0

1
0

2
1

3
2

ai C i
ai S i
di

cos 0

1 sin 0
T 0
0

0
cos1

2 sin1
T 1
0

cos 2
sin
2

3
2

0 a0 cos 0
0 a0 sin 0
1
0

0
1

sin 0
cos 0
0
0
0

sin 1

0
1

cos 1
0

a1 sin 1
0

sin 2

cos 2

a1 cos 1

1 d2

The City College of New

37

Example: Puma 560

The City College of New

38

Example: Puma 560

The City College of New

39

Link Coordinate Parameters

PUMA 560 robot arm link coordinate parameters

The City College of New

40

Example: Puma 560

The City College of New

41

Example: Puma 560

The City College of New

42

Inverse Kinematics
Given a desired position (P)
& orientation (R) of the endz
effector
q (q1 , q2 , qn )

Find the joint variables

which can bring the robot
the desired configuration

The City College of New

y
x

43

Inverse Kinematics
More difficult
Systematic closed-form
solution in general is not
available
Solution not unique

l2

l1
l1

Redundant robot
Elbow-up/elbow-down
configuration

(x , y)

l2

Robot dependent

The City College of New

44

Inverse Kinematics
Transformation Matrix
nx
n
y

ax

sy

ay

nz

sz

az

sx

px

p y
pz

T01T12T23T34T45T56

1

2
3

4
5

6

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)

The City College of New

45

Thank you!
Homework 2 posted on the web.
Due: Sept. 23, 2008
Next class: Inverse Kinematics, Jocobian
Matrix, Trajectory planningz

z
z

x
z

y
x

y
x
The City College of New

46