1

1 The City College of New York

Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
Manipulator Dynamics
Introduction to ROBOTICS
2 The City College of New York
Outline
Homework Highlight
Review
Kinematics Model
Jacobian Matrix
Trajectory Planning
Dynamic Model
Langrange-Euler Equation
Examples
3 The City College of New York
Homework highlight
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
4 The City College of New York
Homework Highlight
Homogeneous Representation
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
(X)
(y)
(z)
5 The City College of New York
Homework Highlight
Assign to complete the right-
handed coordinate system.
The hand coordinate frame is specified by the
geometry of tool. Normally, establish Z
n
along the
direction of Z
n-1
axis and pointing away from the
robot; establish X
n
so that it is normal to both Z
n-1
and Z
n
. Assign Y
n
to complete the right-handed
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
6 The City College of New York
Review
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
2
7 The City College of New York
Review
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 4
successive elementary transformations:
(
(
(
(

=
=

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




8 The City College of New York
Review
Kinematics Equations
chain product of successive coordinate transformation
matrices of
specifies the location of the n-th coordinate frame
w.r.t. the base coordinate system
(

=
(

=
=

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
i
i
T
1
n
T
0
Orientation
matrix
Position
vector
9 The City College of New York
Jacobian Matrix
(
(
(
(
(
(
(
(

6
5
4
3
2
1

(
(
(
(
(
(
(
(

6
5
4
3
2
1

&
&
&
&
&
&
(
(
(
(
(
(
(
(

z
y
x
Joint Space Task Space
Forward
Inverse
Kinematics
Jaconian Matrix: Relationship between joint
space velocity with task space velocity
(
(
(
(
(
(
(
(

z
y
x
z
y
x

&
&
&
Jacobian
Matrix
10 The City College of New York
Jacobian Matrix
(
(
(
(
(
(
(
(

z
y
x
z
y
x

&
&
&
1
2
1
6
) (

(
(
(
(

=
n
n
n
q
q
q
dq
q dh
&
M
&
&
n
n
n
n
n
q
h
q
h
q
h
q
h
q
h
q
h
q
h
q
h
q
h
dq
q dh
J

(
(
(
(
(
(
(
(

=
|
|
.
|

\
|
=
6
6
2
6
1
6
2
2
2
1
2
1
2
1
1
1
6
) (
L
M M M M
L
L
Jacobian is a function of
q, it is not a constant!
11 The City College of New York
Jacobian Matrix
Inverse Jacobian
Singularity
rank(J)<min{6,n},
Jacobian Matrix is less than full rank
Jacobian is non-invertable
Occurs when two or more of the axes of the robot
form a straight line, i.e., collinear
Avoid it
(
(
(
(

= =
66 62 61
26 22 21
16 12 11
J J J
J J J
J J J
q J Y
L
M M M M
L
L
&
&
(
(
(
(
(
(
(
(

6
5
4
3
2
1
q
q
q
q
q
q
&
&
&
&
&
&
Y J q
&
&
1
=
5
q
1
q
12 The City College of New York
Trajectory Planning
Trajectory planning,
interpolate or approximate the desired path by a class of
polynomial functions and generates a sequence of time-based
control set points for the control of manipulator from the
initial configuration to its destination.
Requirements: Smoothness, continuity
Piece-wise polynomial interpolate
4-3-4 trajectory
30 31
2
32
3
33
4
34 3
20 21
2
22
3
23 2
10 11
2
12
3
13
4
14 1
) (
) (
) (
a t a t a t a t a t h
a t a t a t a t h
a t a t a t a t a t h
+ + + + =
+ + + =
+ + + + =
3
13 The City College of New York
Manipulator Dynamics
Mathematical equations describing the
dynamic behavior of the manipulator
For computer simulation
Design of suitable controller
Evaluation of robot structure
Joint torques Robot motion, i.e.
acceleration, velocity, position
14 The City College of New York
Manipulator Dynamics
Lagrange-Euler Formulation
Lagrange function is defined
K: Total kinetic energy of robot
P: Total potential energy of robot
: Joint variable of i-th joint
: first time derivative of
: Generalized force (torque) at i-th joint
i
q
P K L =
i
i i
q
L
q
L
dt
d
=

) (
&
i
q&
i
q
i

15 The City College of New York

Manipulator Dynamics
Kinetic energy
Single particle:
Rigid body in 3-D space with linear velocity (V) , and
angular velocity ( ) about the center of mass
I : Inertia Tensor:
Diagonal terms: moments of inertia
Off-diagonal terms: products of inertia
2
2
1
mv k =

T T
I V mV k
2
1
2
1
+ =

(
(
(
(

+
+
+

dm y x yzdm xzdm
yzdm dm z x xydm
xzdm xydm dm z y
I
) (
) (
) (
2 2
2 2
2 2

+ = dm z y I
xx
) (
2 2

= dm xy I
xy
) (
16 The City College of New York
Velocity of a link
i
i
r
i
x
i
y
i
z
0
x
0
z
0
y
i
r
0
(
(
(
(

=
1
i
i
i
i
i
z
y
x
r
i
i
i
i
i
i
i i
r T T T r T r ) (
1
2
1
1
0 0 0
= = L
A point fixed in link i and expressed w.r.t. the i-th frame
Same point w.r.t the base frame
17 The City College of New York
Velocity of a link
i
i
i
j
j
j
i
i
i
i i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i
i i
i
r q
q
T
r T r T T T
r T T T r T T T
r T T T
dt
d
r
dt
d
V V
) (
) (
1
0
0 1
2
1
1
0
1
2
1
1
0 1
2
1
1
0
1
2
1
1
0 0 0

= + +
+ + =
= =
& &
&
L L
L
&
L
&
L
0 =
i
i
r&
Velocity of point expressed w.r.t. i-th frame is zero
i
i
r
Velocity of point expressed w.r.t. base frame is:
i
i
r
18 The City College of New York
Velocity of a link
(
(
(
(

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



(
(
(
(

1 0 0 0
0 0 0 0
1 i i i i i i i
i i i i i i i
i
i
i
C a S S S C C
S a C S C C S
q
T

(
(
(
(

(
(
(
(

= =

1 0 0 0
0
0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 0
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
d C S
S a C S C C S
C a S S S C C
T Q
q
T



Rotary joints,
i i
q =
(
(
(
(

=
0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 0
i
Q
4
19 The City College of New York
Velocity of a link
(
(
(
(

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



(
(
(
(

0 0 0 0
1 0 0 0
0 0 0 0
0 0 0 0
1
i
i
i
q
T
i
i i
i
i
i
T Q
q
T
1
1

Prismatic joint,
i i
d q =
(
(
(
(

=
0 0 0 0
1 0 0 0
0 0 0 0
0 0 0 0
i
Q
20 The City College of New York
Velocity of a link

>

i j for
i j for T T Q T T T
q
T
i
i
j
j j
j
j
j
i
0
1 1
1
2
2
1
1
0 0
L L

>

i j for
i j for T Q T
q
T
U
i
j j
j
j
i
ij
0
1
1
0 0
The effect of the motion of joint j on all the points on link i

= =

= = =
i
j
i
i j ij
i
i
i
j
j
j
i
i
i
i
i
i i
i
r q U r q
q
T
r T T T
dt
d
r
dt
d
V V
1 1
0
1
2
1
1
0 0 0
) ( ) ( ) ( & & L
21 The City College of New York
Kinetic energy of link i
(

=
(

=
(

=
= + + =

= =
= =
= =
i
p
r p
T
ir
T
i
i
i
i ip
i
r
i
p
r p
T
ir
T
i
i
i
i ip
i
r
i
p
i
r
T i
i r ir
i
i p ip
T
i i i i i i
q q U dmr r U Tr
dm q q U r r U Tr
dm r q U r q U Tr
dm V V trace dm z y x dK
1 1
1 1
1 1
2 2 2
) (
2
1
2
1
) (
2
1
) (
2
1
) (
2
1
& &
& &
& &
& & &

n
i
ii
a A Tr
1
) (
Kinetic energy of a particle with differential mass dm in link i
22 The City College of New York
Kinetic energy of link i
(

= =

= =
i
p
r p
T
ir
T
i
i
i
i ip
i
r
i i
q q U dm r r U Tr dK K
1 1
) (
2
1
& &
(
(
(
(
(
(
(

+
+
+ +
=
(
(
(
(
(

= =

i i i i i i i
i i
zz yy xx
yz xz
i i yz
zz yy xx
xy
i i xz xy
zz yy xx
i i i
i i i i i i
i i i i i i
i i i i i i
T
i
i
i
i i
m z m y m x m
z m
I I I
I I
y m I
I I I
I
x m I I
I I I
dm dm z dm y dm x
dm z dm z dm z y dm z x
dm y dm z y dm y dm y x
dm x dm z x dm y x dm x
dm r r J
2
2
2
2
2
2
(
(
(
(

=
1
i
i
i
i
i
z
y
x
r
Center of mass

= dm x
m
x
i
i
i
1
Pseudo-inertia
matrix of link i
23 The City College of New York
Manipulator Dynamics
| |

= = =
= = = =
=
(

= =
n
i
i
p
i
r
r p
T
ir i ip
i
p
r p
T
ir
T
i
i
i
i ip
i
r
n
i
n
i
i
q q U J U Tr
q q U dm r r U Tr K K
1 1 1
1 1 1 1
) (
2
1
) (
2
1
& &
& &
i
J
i
q
Total kinetic energy of a robot arm
: Pseudo-inertia matrix of link i, dependent on the mass
distribution of link i and are expressed w.r.t. the i-th frame,
Need to be computed once for evaluating the kinetic energy
Scalar quantity, function of and , i
q&
n i L , 2 , 1 =
24 The City College of New York
Manipulator Dynamics
) (
0 0
i
i
i
i
i
i i
r T g m r g m P = =

= =
= =
n
i
n
i
i
i
i
i i
r T g m P P
1 1
0
] ) ( [
) 0 , , , (
z y x
g g g g =
Potential energy of link i
: gravity row vector
expressed in base frame
2
sec / 8 . 9 m g =
i
r
0
i
i
r : Center of mass
w.r.t. i-th frame
: Center of mass
w.r.t. base frame
Potential energy of a robot arm
g
Function of i
q
5
25 The City College of New York
Manipulator Dynamics
Lagrangian function
| |

= = = =
+ = =
n
i
i
j
i
k
n
i
i
i
i
i k j
T
ik i ij
r T g m q q U J U Tr P K L
1 1 1 1
0
) ( ) (
2
1
& &
j
j ji
n
i j
j
n
i j
j
k
j
m
m k
T
ji j
m
jk
n
i j
j
k
k
T
ji j jk
i i
i
r U g m
q q U J
q
U
Tr q U J U Tr
q
L
q
L
dt
d

=
= = = = =

+ =

=
1 1 1
) ( ) (
) (
& & & &
&

26 The City College of New York

Manipulator Dynamics

< <


=

k i or j i
k j i T Q T Q T
j k i T Q T Q T
U
q
U
i
j j
j
k k
k
i
k k
k
j j
j
ijk
k
ij
0
1
1
1
1
0
1
1
1
1
0
The interaction effects of the motion of joint j and joint k
on all the points on link i

>

i j for
i j for T Q T
q
T
U
i
j j
j
j
i
ij
0
1
1
0 0
The effect of the motion of joint j on all the points on link i
27 The City College of New York
Manipulator Dynamics
i m
n
k
n
m
k ikm
n
k
k ik i
C q q h q D + + =

= = =
& & & &
1 1 1

=
=
n
k i j
T
ji j jk ik
U J U Tr D
) , max(
) (

=
=
n
m k i j
T
ji j jkm ikm
U J U Tr h
) , , max(
) (
Dynamics Model
j
j ji
n
i j
j i
r U g m C

=
=
28 The City College of New York
Manipulator Dynamics
) ( ) , ( ) ( q C q q h q q D + + = & & &
(
(
(

=
nn n
n
D D
D D
D
L
M
L
1
1 11
(
(
(

=
n
h
h
q q h M &
1
) , (
Dynamics Model of n-link Arm
(
(
(

=
n
C
C
q C M
1
) (
The Acceleration-related Inertia
matrix term, Symmetric
The Coriolis and Centrifugal terms
The Gravity terms
(
(
(

=
n

M
1
Driving torque
applied on each link
29 The City College of New York
Example
X0
Y0
X1
Y1
1

L
m
(
(
(
(

=
1
0
0
1
1
l
r
Example: One joint arm with point mass (m)
concentrated at the end of the arm, link
length is l , find the dynamic model of the
robot using L-E method.
Set up coordinate frame as in the figure
] 0 , 0 , 8 . 9 , 0 [ = g
1
1
1 1
1 1
1
1
1
0
1
0
1 0 0 0
0 1 0 0
0 0
0 0
r
C S
S C
r T r
(
(
(
(

= =


30 The City College of New York
Example
X0
Y0
X1
Y1
1

L
m

&
&
(
(
(
(

= = = =
0
0
1
1
1
1
1
0 1
1
1
1
0
1
0 1
C l
S l
r T Q r T
dt
d
r V
1
1
1 1
1 1
1
1
1
0
1
0
1 0 0 0
0 1 0 0
0 0
0 0
r
C S
S C
r T r
(
(
(
(

= =


6
31 The City College of New York
Example
| | dm C l S l
C l
S l
Tr K
2
1 1 1
1
1
) 0 0
0
0
(
2
1

&

(
(
(
(

=
dm V V Tr dK
T
) (
2
1
1 1
=
m
C l C S l
C S l S l
Tr
2
1
2
1
2
1 1
2
1 1
2 2
1
2
)
0 0 0 0
0 0 0 0
0 0 ) (
0 0 ) (
(
2
1

&
(
(
(
(
(

=
2
1
2 2 2
1
2 2
1
2
2
1
] ) ( ) ( [
2
1

& &
m l m C l S l = + =
Kinetic energy
32 The City College of New York
Example
) (
1
1
0
r T mg P = | |
(
(
(
(

(
(
(
(

=
1
0
0
1 0 0 0
0 1 0 0
0 0
0 0
0 0 8 . 9 0
1 1
1 1
l
C S
S C
m


Potential energy
1
8 . 9 S l m =
1
2
1
2
8 . 9
2
1
S l m m l P K L = =
&
1 1
2
1 1
2
1 1
1
8 . 9 8 . 9 ) (
) (

C l m m l C l m m l
dt
d
L L
dt
d
= =

=
& & &
&
Lagrange function
Equation of Motion
33 The City College of New York
Example: Puma 560
Derive dynamic equations for the first 4 links of
PUMA 560 robot
34 The City College of New York
Example: Puma 560
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

Get robot link parameters
Set up D-H Coordinate frame
Get transformation matrices
i
i
T
1
Get D, H, C terms
35 The City College of New York
Example: Puma 560
) ( ) ( ) (
31 3 31 21 2 21 11 1 11 11
T T T
U J U Tr U J U Tr U J U Tr D + + =
) ( ) (
31 3 32 21 2 22 21 12
T T
U J U Tr U J U Tr D D + = =
) (
31 3 33 31 13
T
U J U Tr D D = =
Get D, H, C terms

=
=
n
k i j
T
ji j jk ik
U J U Tr D
) , max(
) ( 3 , 2 , 1 ; 3 = = i n
) ( ) (
32 3 32 22 2 22 22
T T
U J U Tr U J U Tr D + =
) (
32 3 33 32 23
T
U J U Tr D D = =
) (
33 3 33 33
T
U J U Tr D =
36 The City College of New York
Example: Puma 560
) ( ) ( ) (
31 3 311 21 2 211 11 1 111 111
T T T
U J U Tr U J U Tr U J U Tr h + + =

=
=
n
m k i j
T
ji j jkm ikm
U J U Tr h
) , , max(
) (
(
(
(

=
n
h
h
q q h M &
1
) , (
Get D, H, C terms

= =
=
n
k
n
m
m k ikm i
q q h h
1 1
& &
) ( ) (
31 3 312 21 2 212 112
T T
U J U Tr U J U Tr h + =
) (
31 3 323 123
T
U J U Tr h =
) ( ) (
31 3 321 21 2 221 121
T T
U J U Tr U J U Tr h + =
) ( ) (
31 3 322 21 2 222 122
T T
U J U Tr U J U Tr h + =

2
3 133 2 3 132 1 3 131 3 2 123
2
2 122 2 1 121 3 1 113 2 1 112
2
1 111 1
q h q q h q q h q q h
q h q q h q q h q q h q h h
& & & & & & &
& & & & & & & &
+ + + +
+ + + + =
) (
31 3 313 113
T
U J U Tr h =
7
37 The City College of New York
Example: Puma 560
2
0
2
1 211
) ( T Q U =

< <


=

k i or j i
k j i T Q T Q T
j k i T Q T Q T
U
q
U
i
j j
k
j k
k
i
k k
k
j j
j
ijk
k
ij
0
1
1
1
1
0
1
1
1
1
0
3
0
2
1 311
) ( T Q U =
Get D, H, C terms
1
0
2
1 111
) ( T Q U =
2
1
2
2
1
0 222
) ( T Q T U =
3
1 2
1
0 1 321 312
T Q T Q U U = =
3
2 3
2
0 1 313
T Q T Q U =
2
1 2
1
0 1 221 212
T Q T Q U U = =
3
1
2
2
1
0 322
) ( T Q T U =
3
2 3
2
1 2
1
0 332 323
T Q T Q T U U = =
3
2 3
2
0 1 331
T Q T Q U =
3
2 2 3
2
0 333
T Q Q T U =
38 The City College of New York
Example: Puma 560
j
j ji
n
i j
j i
r U g m C

=
=
3
3 31 3
2
2 21 2
1
1 11 1 1
r gU m r gU m r gU m C =
3
3 32 3
2
2 22 2 2
r gU m r gU m C =
Get D, H, C terms
3
3 33 3 3
r gU m C =
39 The City College of New York
Thank you!
x
y
z
x
y
z
x
y
z
x
z
y
Homework 4 posted on the web.
Due: Oct. 7, 2005 (Friday)
Next class: Manipulator Control