The City College of New York

1
Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
Robot Kinematics II
Introduction to ROBOTICS
The City College of New York
2
Outline

Review
Manipulator Specifications
Precision, Repeatability
Homogeneous Matrix
Denavit-Hartenberg (D-H)
Representation
Kinematics Equations
Inverse Kinematics


The City College of New York
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
The City College of New York
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
The City College of New York
5
Review
Basic Rotation Matrix



uvw xyz
RP P =
xyz uvw
QP P =
T
R R Q = =
1
uvw
w
v
u
z
y
x
xyz
RP
p
p
p
p
p
p
P =
(
(
(

(
(
(

=
(
(
(

=
w z v z u z
w y v y u y
w x v x u x
k k j k i k
k j j j i j
k i j i i i
x
z
y
v
w
P
u
The City College of New York
6
Basic Rotation Matrices
Rotation about x-axis with


Rotation about y-axis with



Rotation about z-axis with



uvw xyz
RP P =
(
(
(

=
u u
u u u
C S
S C x Rot
0
0
0 0 1
) , (

0
0 1 0
0
) , (
(
(
(

=
u u
u u
u
C S
S C
y Rot
(
(
(

=
1 0 0
0
0
) , ( u u
u u
u C S
S C
z Rot
u
u
u
The City College of New York
7
Review
Coordinate transformation from {B} to {A}



Homogeneous transformation matrix
' o A P B
B
A P A
r r R r + =
(

=
(

1 1 0 1
3 1
' P B o A
B
A P A
r r R r
(

=
(

1 0
1 0
1 3 3 3
3 1
'
P R
r R
T
o A
B
A
B
A
Position
vector
Rotation
matrix
Scaling
The City College of New York
8
Review
Homogeneous Transformation
Special cases
1. Translation



2. Rotation
(

1 0
0
3 1
1 3 B
A
B
A
R
T
(

1 0
3 1
'
3 3
o A
B
A
r I
T
The City College of New York
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

The City College of New York
10
Review
Homogeneous Representation
A point in space




A frame in space

3
R
(
(
(
(

=
(

=
1 0 0 0
1 0 0 0
z z z z
y y y y
x x x x
p a s n
p a s n
p a s n
P a s n
F
x
y
z
) , , (
z y x
p p p P
n
s
a
(
(
(
(

=
1
z
y
x
p
p
p
P
Homogeneous coordinate of P w.r.t. OXYZ
3
R
The City College of New York
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

u
|

u
|
, x
R
u , y
R
| , z
R
yaw
pitch
roll
The City College of New York
12
Quiz 1
How to get the resultant rotation matrix for YPR?
X
Y
Z

u
|
u | , , , x y z
R R R T =
(
(
(
(

=
1 0 0 0
0 1 0 0
0 0
0 0
| |
| |
C S
S C
(
(
(
(

1 0 0 0
0 0
0 0 1 0
0 0
u u
u u
C S
S C
(
(
(
(

1 0 0 0
0 0
0 0
0 0 0 1


C S
S C
The City College of New York
13
Quiz 2
Geometric Interpretation?





(

=

1 0
1 3 3 3
P R
T
(

1 0
1
P R R
T
T T
4 4
1
1 0
0
1 0
1 0

=
(

=
(

= I
R R
P R
P R R
T T
T T T
Position of the origin of OUVW
coordinate frame w.r.t. OXYZ frame
Inverse of the rotation submatrix
is equivalent to its transpose
Position 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?
The City College of New York
14
Kinematics Model
Forward (direct) Kinematics







Inverse Kinematics
) , , (
2 1 n
q q q q =
) , , , , , ( u | z y x Y =
x
y
z
Direct Kinematics
Inverse Kinematics
Position and Orientation
of the end-effector
Joint
variables
The City College of New York
15
Robot Links and Joints
The City College of New York
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 Z
i
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 Z
i
& Z
i-1
or at the
intersection of common normal between the Z
i
& Z
i-1
axes and the Z
i

axis.
Establish X
i
axis. Establish or along the
common normal between the Z
i-1
& Z
i
axes when they are parallel.
Establish Y
i
axis. Assign to complete the
right-handed coordinate system.
Find the link and joint parameters
) , , (
0 0 0
Z Y X
i i i i i
Z Z Z Z X =
1 1
/ ) (
i i i i i
X Z X Z Y + = / ) (
0
Z
The City College of New York
17
Example I
3 Revolute Joints
a
0
a
1

Z
0

X
0

Y
0

Z
3

X
2

Y
1

X
1

Y
2

d
2

Z
1

X
3

3
O
2
O
1
O
0
O
Joint 1
Joint 2
Joint 3
Link 1 Link 2
The City College of New York
18
Link Coordinate Frames
Assign Link Coordinate Frames:
To describe the geometry of robot motion, we assign a Cartesian
coordinate frame (O
i
, X
i
,Y
i
,Z
i
) to each link, as follows:
establish a right-handed orthonormal coordinate frame O
0
at
the supporting base with Z
0
lying along joint 1 motion axis.
the Z
i
axis is directed along the axis of motion of joint (i + 1),
that is, link (i + 1) rotates about or translates along Z
i
;
Link 1 Link 2
a
0
a
1

Z
0

X
0

Y
0

Z
3

X
2

Y
1

X
1

Y
2

d
2

Z
1

X
3

3
O
2
O
1
O
0
O
Joint 1
Joint 2
Joint 3
The City College of New York
19
Link Coordinate Frames
Locate the origin of the ith coordinate at the intersection
of the Z
i
& Z
i-1
or at the intersection of common normal
between the Z
i
& Z
i-1
axes and the Z
i
axis.
the X
i
axis lies along the common normal from the Z
i-1

axis to the Z
i
axis , (if Z
i-1
is
parallel to Z
i
, then X
i
is specified arbitrarily, subject only
to X
i
being perpendicular to Z
i
);

i i i i i
Z Z Z Z X =
1 1
/ ) (
a
0
a
1

Z
0

X
0

Y
0

Z
3

X
2

Y
1

X
1

Y
2

d
2

Z
1

X
3

3
O
2
O
1
O
0
O
Joint 1
Joint 2
Joint 3
The City College of New York
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 Z
n
along the
direction of Z
n-1
axis and pointing away from the robot;
establish X
n
such that it is normal to both Z
n-1
and Z
n

axes. Assign Y
n
to complete the right-handed coordinate
system.

i i i i i
X Z X Z Y + = / ) (
n
O
a
0
a
1

Z
0

X
0

Y
0

Z
3

X
2

Y
1

X
1

Y
2

d
2

Z
1

X
3

3
O
2
O
1
O
0
O
Joint 1
Joint 2
Joint 3
The City College of New York
21
Link and Joint Parameters
Joint angle : the angle of rotation from the X
i-1
axis to
the X
i
axis about the Z
i-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 Z
i-1
axis and
the X
i
axis along the Z
i-1
axis. It is the joint variable if joint i
is prismatic.

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

axis and the X
i
axis to the origin of the ith coordinate
system along the X
i
axis.

Link twist angle : the angle of rotation from the Z
i-1
axis
to the Z
i
axis about the X
i
axis.

i
u
i
d
i
a
i
o
The City College of New York
22
Example I
Joint i
o
i
a
i
d
i

u
i
1 0 a
0
0
u
0

2 -90 a
1
0
u
1

3 0 0 d
2

u
2


D-H Link Parameter Table
: rotation angle from X
i-1

to X
i
about Z
i-1

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

: distance from intersection of Z
i-1
& X
i

to origin of i coordinate along X
i

i
d
: rotation angle from Z
i-1
to Z
i
about X
i

i
a
i
o
a
0
a
1

Z
0

X
0

Y
0

Z
3

X
2

Y
1

X
1

Y
2

d
2

Z
1

X
3

3
O
2
O
1
O
0
O
Joint 1
Joint 2
Joint 3
The City College of New York
23
Example II: PUMA 260
i i i i i
Z Z Z Z X =
1 1
/ ) (
i i i i i
X Z X Z Y + = / ) (
1
u
2
u
3
u
4
u
5
u
6
u
0
Z
1
Z
2
Z
3
Z
4
Z
5
Z
1
O
2
O
3
O
5
O
4
O
6
O
1
X
1
Y
2
X
2
Y
3
X
3
Y
4
X
4
Y
5
X
5
Y
6
X
6
Y
6
Z
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
The City College of New York
24
Link Parameters
1
u
2
u
3
u
4
u
5
u
6
u
0
Z
1
Z
2
Z
3
Z
4
Z
5
Z
1
O
2
O
3
O
5
O
4
O
6
O
1
X
1
Y
2
X
2
Y
3
X
3
Y
4
X
4
Y
5
X
5
Y
6
X
6
Y
6
Z
: angle from Z
i-1
to Z
i
about X
i

: distance from intersection
of Z
i-1
& X
i
to Oi along X
i

Joint distance : distance from Oi-1 to intersection of Z
i-1
& X
i
along Z
i-1

: angle from X
i-1

to X
i
about Z
i-1

i
u
i
o
i
a
i
d
t 0 0 6
0 0 90 5
8 0 -90 4
0
0 90 3
8 0 2
13 0 -90 1
J
i
u
1
u
4
u
2
u
3
u
6
u
5
u
i
o
i
a
i
d
-l
The City College of New York
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 u
i
to align the
X
i-1
axis with the X
i
axis.
Translate along the Z
i-1
axis a distance of d
i
, to bring
X
i-1
and X
i
axes into coincidence.
Translate along the X
i
axis a distance of a
i
to bring
the two origins O
i-1
and O
i
as well as the X axis into
coincidence.
Rotate about the X
i
axis an angle of
i
( in the right-
handed sense), to bring the two coordinates into
coincidence.
The City College of New York
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:



(
(
(
(

=

1 0 0 0
0
) , ( ) , ( ) , ( ) , (
1 1 1
i i i
i i i i i i i
i i i i i i i
i i i i i i i i
i
i
d C S
S a C S C C S
C a S S S C C
x R a x T z R d z T T
o o
u u o u o u
u u o u o u
o u
Source coordinate
Reference
Coordinate
The City College of New York
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
(

=
(

=
=

1 0 0 0 1 0
0 0 0
1
2
1
1
0 0
n n n
n
n
n
P a s n P R
T T T T
) , , (
2 1 n
q q q q =
) , , , , , ( u | z y x Y =
i
i
T
1
n
T
0
Orientation
matrix
Position
vector
The City College of New York
28
Kinematics Equations
Other representations
reference from, tool frame


Yaw-Pitch-Roll representation for orientation
tool
n
n
ref
tool
ref
H T B T
0
0
=
u | , , , x y z
R R R T =
(
(
(
(

=
1 0 0 0
0 1 0 0
0 0
0 0
| |
| |
C S
S C
(
(
(
(

1 0 0 0
0 0
0 0 1 0
0 0
u u
u u
C S
S C
(
(
(
(

1 0 0 0
0 0
0 0
0 0 0 1


C S
S C
The City College of New York
29
Representing forward kinematics
Forward kinematics

(
(
(
(
(
(
(
(

(
(
(
(
(
(
(
(

u
|
u
u
u
u
u
u
z
y
x
p
p
p
6
5
4
3
2
1
(
(
(
(

=
1 0 0 0
z z z z
y y y y
x x x x
p a s n
p a s n
p a s n
T
Transformation Matrix
The City College of New York
30
Representing forward kinematics
Yaw-Pitch-Roll representation for orientation
(
(
(
(

+
+
=
1 0 0 0
0
z
y
x
n
p C C S C S
p S C C S S C C S S S C S
p S S C S C C S S S C C C
T
u u u
| u | | u | u |
| u | | u | 0 |
(
(
(
(

=
1 0 0 0
0
z z z z
y y y y
x x x x
n
p a s n
p a s n
p a s n
T
) ( sin
1
z
n =

u
)
cos
( cos
1
u

z
a

=
)
cos
( cos
1
u
|
x
n

=
Problem? Solution is inconsistent and ill-conditioned!!
The City College of New York
31
atan2(y,x)
x
y

+ s s
s s
+ s s
+ + s s
= =
y and x f or
y and x f or
y and x f or
y and x f or
x y a




0 90
90 180
180 90
90 0
) , ( 2 tan
u
u
u
u
u
The City College of New York
32
Yaw-Pitch-Roll Representation
u | , , , x y z
R R R T =
(
(
(
(

=
1 0 0 0
0 1 0 0
0 0
0 0
| |
| |
C S
S C
(
(
(
(

1 0 0 0
0 0
0 0 1 0
0 0
u u
u u
C S
S C
(
(
(
(

1 0 0 0
0 0
0 0
0 0 0 1


C S
S C
(
(
(
(

=
1 0 0 0
0
0
0
z z z
y y y
x x x
a s n
a s n
a s n
The City College of New York
33
Yaw-Pitch-Roll Representation
u | , ,
1
, x y z
R R T R =

(
(
(
(

1 0 0 0
0 1 0 0
0 0
0 0
| |
| |
C S
S C
(
(
(
(

=
1 0 0 0
0 0
0 0 1 0
0 0
u u
u u
C S
S C
(
(
(
(

1 0 0 0
0 0
0 0
0 0 0 1


C S
S C
(
(
(
(

1 0 0 0
0
0
0
z z z
y y y
x x x
a s n
a s n
a s n
(Equation A)
The City College of New York
34
Yaw-Pitch-Roll Representation
0 cos sin = +
y x
n n | |

=
= +
u
u | |
sin
cos sin cos
z
y x
n
n n

= +
= +
| |
| |
sin cos sin
cos cos sin
y x
y x
a a
s s
Compare LHS and RHS of Equation A, we have:
) , ( 2 tan
x y
n n a = |
) sin cos , ( 2 tan
y z z
n n n a + = | | u
) cos sin , cos (sin 2 tan
y x y x
s s a a a + = | | | |
The City College of New York
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

The City College of New York
36
Example
Joint i
o
i
a
i
d
i

u
i
1 0 a
0
0
u
0

2 -90 a
1
0
u
1

3 0 0 d
2

u
2


a
0
a
1

Z
0

X
0

Y
0

Z
3

X
2

Y
1

X
1

Y
2

d
2

Z
1

X
3

3
O
2
O
1
O
0
O
Joint 1
Joint 2
Joint 3
The City College of New York
37
Example
Joint i
o
i
a
i
d
i

u
i
1 0 a
0
0
u
0

2 -90 a
1
0
u
1

3 0 0 d
2

u
2


(
(
(
(

=
1 0 0 0
0 1 0 0
0 cos sin
0 sin cos
0 0
0 0
0
0 0
0 0
1
sin
cos
u
u
a
a
T
(
(
(
(

=
1 0 0 0
0 0 0
sin
cos
1
1
1
1
sin cos 0
cos sin 0
1 1 1
1 1 1
2
u u
u u
a
a
T
) )( )( (
2 1 0
3 2 1 3
0
T T T T =
(
(
(
(

=
1 0 0 0
0 sin cos
2 2
2
2
2 2 3
1 0 0
0 0 cos sin
0
d
T
u u
(
(
(
(

1 0 0 0
0
1
i i i
i i i i i i i
i i i i i i i
i
i
d C S
S a C S C C S
C a S S S C C
T
o o
u u o u o u
u u o u o u
The City College of New York
38
Example: Puma 560
The City College of New York
39
Example: Puma 560
The City College of New York
40
Link Coordinate Parameters
Joint i
u
i


o
i

a
i
(mm) d
i
(mm)

1
u
1

-90

0 0
2
u
2

0 431.8 149.09
3
u
3

90 -20.32 0
4
u
4

-90 0 433.07
5
u
5

90 0 0
6
u
6

0 0 56.25

PUMA 560 robot arm link coordinate parameters
The City College of New York
41
Example: Puma 560
The City College of New York
42
Example: Puma 560
The City College of New York
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
) , , (
2 1 n
q q q q =
x
y
z
The City College of New York
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)
The City College of New York
45
Inverse Kinematics
(
(
(
(
(
(
(
(

6
5
4
3
2
1
u
u
u
u
u
u
6
5
5
4
4
3
3
2
2
1
1
0
1 0 0 0
T T T T T T
p a s n
p a s n
p a s n
T
z z z z
y y y y
x x x x
=
(
(
(
(

=
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)
The City College of New York
46
Thank you!
x
y
z
x
y
z
x
y
z
x
z
y
Homework 2 posted on the web.
Due: Sept. 23, 2008
Next class: Inverse Kinematics, Jocobian
Matrix, Trajectory planning