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 York 1

 Outline

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

The City College of New York 2

 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 How accurately a specified point can be reached

Repeatability How accurately the same position can be reached

if the motion is repeated many times

The City College of New York 3

 Review
Manipulators:

Cartesian: PPP Cylindrical: RPP Spherical: RRP

Hand coordinate:
SCARA: RRP n: normal vector; s: sliding vector;
Articulated: RRR (Selective Compliance a: approach vector, normal to the
Assembly Robot Arm)
tool mounting plate
The City College of New York 4
 Review
Basic Rotation Matrix
px i x i u i x jv i x k w pu

Pxyz p y jy i u jy jv
jy k w pv RPuvw
pz k z i u k z jv k z k w pw
z
Pxyz RPuvw w
P v
Puvw QPxyz

Q R 1 RT y
u
x
The City College of New York 5
 Basic Rotation Matrices
Rotation about x-axis with
1 0 0
Rot ( x, ) 0 C S
0 S C
Rotation about y-axis with
C 0 S
Rot ( y, ) 0 1 0
S 0 C

Rotation about z-axis with

C S 0
Pxyz RPuvw Rot ( z, ) S C 0
0 0 1
The City College of New York 6
 Review
Coordinate transformation from {B} to {A}
r ARB B r P Ar o '
A P

A r P A RB A r o ' B r P

1 013 1 1
Homogeneous transformation matrix
RB r R33 P31
A A o' Rotation

TB
A matrix

013 1 0 1 Position
vector
Scaling
The City College of New York 7
 Review
Homogeneous Transformation
Special cases
1. Translation
I 33 r
A o'
A
TB
013 1

2. Rotation
A
RB 031
A
TB
013 1

The City College of New York 8

 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 9

 Review
Homogeneous Representation
A point in R3 space
px
p
P y Homogeneous coordinate of P w.r.t. OXYZ
pz
z P( p , p y , pz )

1
a
x

s
A frame in R space
3

n
nx sx ax px
y
n s a P n y sy ay p y
F
0 0 0 1 nz sz az pz

0 0 0 1 x
The City College of New York 10
 Review
Orientation Representation
(Euler Angles)
Description of Yaw, Pitch, Roll
A rotation of about the OX
Z
axis ( Rx , ) -- yaw roll
A rotation of about the OY
axis ( R y , ) -- pitch yaw
A rotation of about the OZ Y
axis ( Rz , ) -- roll
pitch
X

The City College of New York 11

 Quiz 1
How to get the resultant rotation matrix for YPR?
T Rz , R y , Rx ,
C S 0 0 C 0 S 0 1 0 0 0
S C 0 0 0 1 0 0 0 C S 0

0 0 1 0 S 0 C 0 0 S C 0

0 0 0 1 0 0 0 1 0 0 0 1
Z

Y

X
The City College of New York 12
 Quiz 2
Geometric Interpretation?
R33 P31 Orientation of OUVW coordinate

T
frame w.r.t. OXYZ frame
Position of the origin of OUVW
0 1 coordinate frame w.r.t. OXYZ frame
Inverse Homogeneous Matrix?
1 R T
R T
P Inverse of the rotation submatrix
T Position of the origin of OXYZ
is equivalent to its transpose

0 1 reference frame w.r.t. OUVW frame

1 R T
R T P R P R T R 0
T T I 44
0 1 0 1 0 1

The City College of New York 13

 Kinematics Model
Forward (direct) Kinematics
q (q1 , q2 , qn ) z
Joint Position and Orientation
variables of the end-effector
Direct Kinematics

Inverse Kinematics x
Y ( x, y, z , , , )

Inverse Kinematics
The City College of New York 14
Robot Links and Joints

The City College of New York 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 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 Zi
axis.
Establish Xi axis. Establish X i (Zi 1 Zi ) / Zi 1 Zi or along the
common normal between the Zi-1 & Zi axes when they are parallel.
Establish Yi axis. Assign Yi (Zi X i ) / Zi X i to complete the
right-handed coordinate system.
Find the link and joint parameters
The City College of New York 16
 Example I
3 Revolute Joints

Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

Link 1 Link 2 d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1

The City College of New York 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 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;
Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

Link 1 Link 2 d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1
The City College of New York 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 (Zi 1 Zi ) / Zi 1 Zi , (if Zi-1 is
parallel to Zi, then Xi is specified arbitrarily, subject only
to Xi being perpendicular to Zi); Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1
The City College of New York 19
 Link Coordinate Frames
Assign Yi (Zi X i ) / Zi X i to complete the right-
handed 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
system. Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1
The City College of New York 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.

Joint distance d i : 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 ai : the distance from the intersection of the Zi-1

axis and the Xi axis to the origin of the ith coordinate
system along the Xi axis.

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

The City College of New York 21

 Example I
Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

d2
Joint 1
O0 X0 O1 X1 O2 X2 D-H Link Parameter Table
Joint 2
Y2 Joint i i ai di i

a0 a1 1 0 a0 0 0

i : rotation angle from Zi-1 to Zi about Xi 2 -90 a1 0 1

ai : distance from intersection of Zi-1 & Xi 3 0 0 d2 2

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

The City College of New York 22

Example II: PUMA 260
1. Number the joints
2. Establish base frame
1 2 3. Establish joint axis Zi
Z1 4. Locate origin, (intersect.
O1 3 of Zi & Zi-1) OR (intersect
of common normal & Zi )
X1 Z2 Z6
Y1 5. Establish Xi,Yi
O
Y3 2 Z5 Z4
O3 X Y6 X i (Zi 1 Zi ) / Zi 1 Zi
Y2 2
O
6
5 Yi (Zi X i ) / Zi X i
Z0 6
X3 4Y Y5
t
O5 X5 X6
O4 Z 3
X4 4
PUMA 260
The City College of New York 23
 Link Parameters
J i i ai d i
1 2 1 1 -90 0 13
Z1 2 2 0 8 0
O1 3 3 3 90 0 -l
X1 Z2 Z6 4 -90 0 4 8
Y1 5
Y3O2
5 90 0 0
Z5 Z4 6
O3 X Y6 0 0 t 6
Y2 2
O
6
5 : angle from X to X
Z0 6 i i-1 i
X 3 Y4 Y5 about Zi-1
O5 X5 X6
O4 Z 3 i : angle from Zi-1 to Zi
about X i
X4 4 ai : distance from intersection
of Zi-1 & Xi to Oi along Xi
Joint distance di : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1
The City College of New York 24
 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.
The City College of New York 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 )
C i C i S i S i S i ai C i
Reference S C i C i S i C i ai S i
Coordinate i
0 S i C i di

0 0 0 1
The City College of New York 26
 Kinematic Equations
Forward Kinematics q (q1 , q2 , qn )
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
matrices of Ti i 1
T T T T
n 1 2 n Position
0 0 1 n 1 vector

Orientation R0n P0n n s a P0n

matrix
0 1 0 0 0 1
The City College of New York 27
 Kinematics Equations
Other representations
reference from, tool frame
Treftool Bref
0
T0n H ntool

Yaw-Pitch-Roll representation for orientation

T Rz , R y , Rx ,
C S 0 0 C 0 S 0 1 0 0 0
S C 0 0 0 1 0 0 0 C S 0

0 0 1 0 S 0 C 0 0 S C 0

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

The City College of New York 28

 Representing forward kinematics

Forward kinematics Transformation Matrix

1 px
p nx sx ax px
2 y n
3 pz sy ay p y
T y
nz sz az pz
4
5 0 0 0 1

6

The City College of New York 29

 Representing forward kinematics
Yaw-Pitch-Roll representation for orientation
CC CSS SC CSC SS px
SC SSS CC SSC CS p y
T0n
S CS CC pz

0 0 0 1
nx sx ax px sin 1 (nz )
n p y az
T0n y
sy ay cos ( 1
)
nz sz az pz cos
nx
0 0 0 1 cos (
1
)
cos
Problem? Solution is inconsistent and ill-conditioned!!

The City College of New York 30

 atan2(y,x)
y

0 90 for x and y

90 180 for x and y

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

The City College of New York 31

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

0 0 1 0 S 0 C 0 0 S C 0

0 0 0 1 0 0 0 1 0 0 0 1
nx sx ax 0
n sy ay 0
y
nz sz az 0

0 0 0 1

The City College of New York 32

Yaw-Pitch-Roll Representation
1
R T Ry , Rx,
z ,

C S 0 0 nx sx ax 0
S 0 n
C 0
y sy ay 0
0 0 1 0 nz sz az 0

0 0 0 1 0 0 0 1
(Equation A)

C 0 S 0 1 0 0 0
0 0 C 0
1 0 0 S

S 0 C 0 0 S C 0

0 0 0 1 0 0 0 1

The City College of New York 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 sx cos s y cos

sin ax cos a y sin

a tan 2(sin ax cos a y , sin s x cos s y )

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

 Example
Z3
Z0 Z1
Joint 3
Y0 O3 X3
Y1

d2
Joint 1
O0 X0 O1 X1 O2 X2
Joint 2
Y2

a0 a1 Joint i i ai di i
1 0 a0 0 0

2 -90 a1 0 1

3 0 0 d2 2

The City College of New York 36

 Example
Joint i i ai di i cos 0 sin 0 0 a0 cos 0
0 a0 sin 0
1 0 a0 0 0 1 sin 0 cos 0
T 0
0 0 1 0
2 -90 a1 0 1
0 0 0 1

3 0 0 d2 2 cos1 0 sin 1 a1 cos 1

0 cos 1 a1 sin 1
2 sin1
T 1
0 1 0 0
C i C i S i S i S i ai C i
S C i C i S i C i ai S i 0 0 0 1
Ti i 1 i
0 S i C i di
sin 2
0 0 0 1 cos 2 0 0
0 0
3 sin 2 cos 2
T 2
T03 (T 01)(T 21)(T 23) 0 0 1 d2

0 0 0 1

The City College of New York 37

Example: Puma 560

The City College of New York 38

Example: Puma 560

The City College of New York 39

Link Coordinate Parameters
PUMA 560 robot arm 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.07
5 5 90 0 0
6 6 0 0 56.25

The City College of New York 40

Example: Puma 560

The City College of New York 41

Example: Puma 560

The City College of New York 42

 Inverse Kinematics
Given a desired position (P)
& orientation (R) of the end-
z
effector

q (q1 , q2 , qn )
y
Find the joint variables
which can bring the robot x
the desired configuration

The City College of New York 43

 Inverse Kinematics
More difficult
Systematic closed-form
solution in general is not (x , y)
available
Solution not unique
Redundant robot
Elbow-up/elbow-down
configuration
Robot dependent

The City College of New York 44

 Inverse Kinematics
Transformation Matrix 1

nx sx ax px 2
n sy ay p y 3
T y T01T12T23T34T45T56
nz

sz az pz

4
0 0 0 1 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 York 45

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

x
The City College of New York 46