1

1
The City College of New York
Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
Robot Kinematics II
Introduction to ROBOTICS
2
The City College of New York
Outline
Review
Manipulator Specifications
Precision, Repeatability
Homogeneous Matrix
Denavit-Hartenberg (D-H)
Representation
Kinematics Equations
Inverse Kinematics
3
The City College of New York
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
The City College of New York
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
The City College of New York
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
6
The City College of New York
Basic Rotation Matrices
Rotation about x-axis with
Rotation about y-axis with
Rotation about z-axis with
uvw xyz
RP P =
(
(
(

=


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

0
0 1 0
0
) , (
(
(
(

C S
S C
y Rot
(
(
(

=
1 0 0
0
0
) , (

C S
S C
z Rot

2
7
The City College of New York
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
8
The City College of New York
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
9
The City College of New York
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
The City College of New York
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
11
The City College of New York
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

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

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


C S
S C
(
(
(
(

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


C S
S C
3
13
The City College of New York
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?
14
The City College of New York
Kinematics Model
Forward (direct) Kinematics
Inverse Kinematics
) , , (
2 1 n
q q q q L =
) , , , , , ( z y x Y =
x
y
z
Direct Kinematics
Inverse Kinematics
Position and Orientation
of the end-effector
Joint
variables
15
The City College of New York
Robot Links and Joints
16
The City College of New York
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
17
The City College of New York
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
Z
2
Joint 1
Joint 2
Joint 3
Link 1 Link 2
18
The City College of New York
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
Z
2
Joint 1
Joint 2
Joint 3
4
19
The City College of New York
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
Z
2
Joint 1
Joint 2
Joint 3
20
The City College of New York
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
Z
2
Joint 1
Joint 2
Joint 3
21
The City College of New York
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

i
d
i
a
i

22
The City College of New York
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 X
i-1
to X
i
about Z
i-1 i

: 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

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
Z
2
Joint 1
Joint 2
Joint 3
23
The City College of New York
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

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
24
The City College of New York
Link Parameters
1

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

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

i
a
i
d
-l
5
25
The City College of New York
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 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.
26
The City College of New York
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




Source coordinate
Reference
Coordinate
27
The City College of New York
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 K
) , , (
2 1 n
q q q q L =
) , , , , , ( z y x Y =
i
i
T
1
n
T
0
Orientation
matrix
Position
vector
28
The City College of New York
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
=
, , , 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


C S
S C
(
(
(
(

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


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

(
(
(
(
(
(
(
(

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
30
The City College of New York
Solving 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



(
(
(
(

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

)
cos
( cos
1

z
a

=
)
cos
( cos
1

x
n

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

+

+
+ +
= =
y and x for
y and x for
y and x for
y and x for
x y a
o o
o o
o o
o o
0 90
90 180
180 90
90 0
) , ( 2 tan

32
The City College of New York
Yaw-Pitch-Roll Representation
, , , 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


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
33
The City College of New York
Yaw-Pitch-Roll Representation
, ,
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


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)
34
The City College of New York
Yaw-Pitch-Roll Representation
0 cos sin = +
y x
n n

=
= +

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 + =
) cos sin , cos (sin 2 tan
y x y x
s s a a a + =
35
The City College of New York
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
The City College of New York
Example
Joint i
i
ai di
i
1 0 a
0
0
0

2 -90 a
1
0
1

3 0 0 d2 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
Z
2
Joint 1
Joint 2
Joint 3
7
37
The City College of New York
Example
Joint i i
ai di i
1 0 a0 0 0
2 -90 a1 0 1
3 0 0 d2 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

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


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

(
(
(
(

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



38
The City College of New York
Example: Puma 560
39
The City College of New York
Example: Puma 560
40
The City College of New York
Link Coordinate Parameters
Joint i
i

i
a
i
(mm) d
i
(mm)
1
1
-90

0 0
2
2
0 431.8 149.09
3
3
90 -20.32 0
4
4
-90 0 433.07
5
5
90 0 0
6
6
0 0 56.25

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

6
5
4
3
2
1

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