Introduction to ROBOTICS
Manipulator Dynamics
Dr. Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
The City College of New York
1
Outline
• Homework Highlight
• Review
– Kinematics Model
– Jacobian Matrix
– Trajectory Planning
• Dynamic Model
– Langrange-Euler Equation
– Examples
The City College of New York
2
Homework highlight
• Composite Homogeneous Transformation
Matrix Rules:
– Transformation (rotation/translation) w.r.t.
(X,Y,Z) (OLD FRAME), using premultiplication
– Transformation (rotation/translation) w.r.t.
(U,V,W) (NEW FRAME), using postmultiplication
The City College of New York
3
Homework Highlight
• Homogeneous Representation
– A frame in
R
3
space
P( px , py , pz )
z
n
F  
0
s
a
0
0
nx

P
ny


1  nz

0
sx
ax
sy
ay
sz
az
0
0
px 

py

pz 

1 
a (z’)
s (y’)
n (X’)
y
x
The City College of New York
4
Homework Highlight
– Assign Y   ( Z  X ) / Z  X to complete the righthanded coordinate system.
– The hand coordinate frame is specified by the O n
geometry of tool. Normally, establish Zn along the
direction of Zn-1 axis and pointing away from the
robot; establish Xn so that it is normal to both Zn-1
and Zn. Assign Yn to complete the right-handed
Z3
Z1
system. Z0
Joint 3
i
i
i
i
i
Y0
O3
Y1
X3
d2
Joint 1
O0
X0
O 1 X1
O 2 X2
Joint 2
Y2
a
a
0
1
The City
College of New
York
5
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
The City College of New York
6
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:
T i 1  T ( z i 1 , d i ) R ( z i 1 ,  i )T ( x i , a i ) R ( x i ,  i )
i
C  i

S i


 0

 0
 C  i S i
S i S i
C  iC  i
 S iC  i
S i
C i
0
0
The City College of New York
aiC  i 

ai S i

di 

1 
7
Review
• Kinematics Equations
– chain product of successive coordinate transformation
i
matrices of T i  1
– T 0n specifies the location of the n-th coordinate frame
w.r.t. the base coordinate system
T 0  T 0 T1  T n 1
n
Orientation
matrix
1
 R 0n

 0
2
n
n
P0   n

1  0
s
a
0
0
n
P0 

1 
Position
vector
The City College of New York
8
Jacobian Matrix
  1  Forward
x 
 
 
2
y
 
 
 3 
z 
  Kinematics  
 4 
 
 
 
5
 
 
Inverse

 6 
 
Joint Space
Task Space
 1 
 
 2 
 
3
 

 4 
 
 5
6 
Jacobian
Matrix
 x 
 
y
 
 z 
 
 x 
 
y
 
 z 
Jaconian Matrix: Relationship between joint
space velocity with task space velocity
The City College of New York
9
Jacobian Matrix
 x 
 
y
 q 1 
 
 z   dh ( q )   q 2 
 
  
  
 x   dq  6  n 
 
 
y
 q n  n 1
 
 z 
 dh ( q ) 

J  
 dq  6  n
Jacobian is a function of
q, it is not a constant!
  h1
 q
 1
  h2
  q
1
 
 h
 6
  q 1
The City College of New York
 h1
q 2
 h2
q 2

 h6
q 2




 h1 
q n 

 h2 
q n 
 
 h6 

 q n  6  n
10
Jacobian Matrix
• Inverse Jacobian
 J 11

J 21


Y  J q 
 

 J 61
J 12

J 22



J 62

J 16 

J 26

 

J 66 
 q 1 
 
q
 2
 q 3 
 
 q 4 
 q 
5
 
 q 6 
1
q  J Y
q5
q1
• 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
The City College of New York
11
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
h1 ( t )  a 14 t  a 13 t  a12 t  a 11 t  a10
4
3
2
h 2 ( t )  a 23 t  a 22 t  a 21 t  a 20
3
2
h 3 ( t )  a 34 t  a 33 t  a 32 t  a 31 t  a 30
4
3
2
The City College of New York
12
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
The City College of New York
13
Manipulator Dynamics
• Lagrange-Euler Formulation
d
(
L
)
L
 i
dt  q i
qi
– Lagrange function is defined
L K P
• K: Total kinetic energy of robot
• P: Total potential energy of robot
• q i : Joint variable of i-th joint
• q i: first time derivative of q i
•  i : Generalized force (torque) at i-th joint
The City College of New York
14
Manipulator Dynamics
• Kinetic energy
k 
1
2
– Single particle:
2
– Rigid body in 3-D space with linear velocity (V) , and
angular velocity (  ) about the center of mass
1
1
T
T
k  mV V  I  
2
2
 ( y 2  z 2 ) dm


I     xydm

   xzdm

mv
 xydm
 ( x  z ) dm
2

2
 yzdm




2
2
 ( x  y ) dm 
 xzdm
  yzdm

– I : Inertia Tensor:
I xx 
• Diagonal terms: moments of inertia
• Off-diagonal terms: products of inertia I xy 
The City College of New York
 ( y  z ) dm
2
2
 ( xy ) dm
15
Velocity of a link
 xi 
 
yi
i

 A point fixed in link i and expressed w.r.t. the i-th frame
ri 
 zi 
yi
 
zi
1
ri
z0
Same point w.r.t the base frame
i
xi
i
r0  T 0 ri  (T 0 T1  Ti 1 ) ri
i
i
i
1
2
i
i
r0
y0
x0
The City College of New York
16
Velocity of a link
Velocity of point
ri
i
expressed w.r.t. i-th frame is zero
ri  0
i
i
Velocity of point ri expressed w.r.t. base frame is:
Vi  V 
i
0
d
dt
r 
i
0
d
dt
1
2
i
(T 0 T1  T i 1 ) ri
i
1 2
i
i
1 2
i
i
 T0 T1  T i 1 ri  T 0 T1  T i 1 ri 
i
1 2
i
i
i i
  T 0 T1  Ti 1 ri  T 0 ri  ( 
j 1
 T0
i
q j
The City College of New York
i
q j ) ri
17
Velocity of a link
Rotary joints, q i   i
•
T
i
i 1
 T i 1
i
qi
C  i

S i


 0

 0
 C  i S i
S i S i
C  iC  i
 S iC  i
S i
C i
0
0
 S i

Ci


 0

 0
 T i 1
 C  iC  i
S iC  i
 C  i S i
S i S i
0
0
0
0
i
qi
 Q iT
i
i 1
0

1


0

0
1
0
0
0
0
0
0
0
aiC  i 

ai S i

di 

1 
 ai S i 

aiC  i


0

0

0  C  i

0 S i

0  0

0  0
0

1

Qi 
0

0
1
0
0
0
0
0
0
0
 C  i S i
S i S i
C  iC  i
 S iC  i
S i
C i
0
0
The City College of New York
0

0

0

0
aiC  i 

ai S i

di 

1 
18
Velocity of a link
• Prismatic joint, q i  d i
T
i
i 1
C  i

S i


 0

 0
 T i 1
i
qi
0

0


0

0
 C  i S i
S i S i
C  iC  i
 S iC  i
S i
C i
0
0
0
0
0
0
0
0
0
0
0

0

1

0
aiC  i 

ai S i

di 

1 
T
i
i 1
qi
0

0

Qi 
0

0
 Q iT
The City College of New York
0
0
0
0
0
0
0
0
0

0

1

0
i
i 1
19
Velocity of a link
The effect of the motion of joint j on all the points on link i
 T0
i
q j
T 01T1 2  T j j21Q j T j j1  T i i 1

 0
 T0
i
U ij 
Vi  V0 
i
d
dt
q j
r0 
i
T 0 j 1Q j T ji1

 0
d
dt
i
i
j 1
ji
ji
ji
ji
for
(T 0 T1  Ti 1 ) ri  ( 
2
for
for
i
1
for
 T0
i
q j
The City College of New York
i
i
i
q j ) ri  (  U ij q j ) ri
j 1
20
Kinetic energy of link i
• Kinetic energy of a particle with differential mass dm in link i
dK i 
1
2
2
2
2
( x i  y i  z i ) dm 
1
2
i
 i
i
i T
 Tr   U ip q p ri (  U ir q r ri )
2
r 1
 p 1
1
T
trace (V iV i ) dm

 dm

 i i

i iT
T
 Tr    U ip ri ri U ir q p q r  dm
2
 p 1 r 1

1
 i i

i
iT
T
 Tr    U ip ( ri dmr i )U ir q p q r 
2
 p 1 r 1

1
The City College of New York
n
Tr ( A ) 
a
ii
i 1
21
Kinetic energy of link i
 i i

i iT
T
K i   dK i  Tr    U ip (  ri ri dm )U ir q p q r 
2
 p 1 r 1

1
 x i 2 dm


  x i y i dm
i iT
I i   ri ri dm  
  x i z i dm
 x dm
  i
  I xx



 




 I yy  I zz
2
I xy
 x y dm  x z dm  x dm 
 y dm  y z dm  y dm 

y
z
dm
z
dm
z
dm





y
dm
z
dm
dm


 
i
i
i
i
i
i
i
i
2
i
2
i
i
i
I xy
I xx  I yy  I zz
2
I xz
I yz
m i xi
mi yi
i
i
i
I xz
I yz
I xx  I yy  I zz
2
mi zi

m i xi 

mi yi 


mi zi 

mi 
The City College of New York
 xi 
 
yi
i


ri 
 zi 
 
1
xi 
1
mi
 x dm
i
Center of mass
Pseudo-inertia
matrix of link i
22
Manipulator Dynamics
• Total kinetic energy of a robot arm
n
K 
K
i

i 1

1
2
1
 i i

i iT
T
Tr    U ip (  ri ri dm )U ir q p q r 
 p 1 r 1

n

2
i 1
   Tr (U
n
i
i
T
ip
I iU ir ) q p q r

i 1 p 1 r 1
Scalar quantity, function of
qi
and
q i
, i  1, 2 ,  n
I i : 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
The City College of New York
23
Manipulator Dynamics
• Potential energy of link i
i
r0 : Center of mass
w.r.t. base frame
Pi   m i g r0   m i g (T 0 ri )
i
i
i
i
ri : Center of mass
w.r.t. i-th frame
g  ( g x , g y , g z ,0 )
g  9 . 8 m / sec
2
g : gravity row vector
expressed in base frame
• Potential energy of a robot arm
n
P 

i 1
n
Pi 
 [  m i g (T0 ri ) ]
i
i
Function of
i 1
The City College of New York
qi
24
Manipulator Dynamics
• Lagrangian function
L  K P 
1
2
i 
n

d
(
L
dt  q i
)
   Tr (U
n
i
i 1
i
j  i k 1
i
i i
0 i
r )
i 1
L
qi
n
jk
T
I U ik ) q j q k 
ij i
j 1 k 1
j
  Tr (U
  m g (T
n
I jU
T
ji
) qk 
j
j
U
   Tr (  q
j  i k 1 m 1
jk
I jU
T
ji
) q k q m
m
n
  m j gU ji r j
j
ji
The City College of New York
25
Manipulator Dynamics
The effect of the motion of joint j on all the points on link i
U ij 
T
i
0
q j
T 0 j 1Q j T ji1

 0
ji
for
ji
for
The interaction effects of the motion of joint j and joint k
on all the points on link i
 U ij
q k
 U ijk
 T 0 j 1Q j T jk11Q k T ki1
 k 1
j 1
i
  T 0 Q k T k 1 Q j T j 1
0
i j

The City College of New York
ik  j
i jk
or
ik
26
Manipulator Dynamics
• Dynamics Model
n
i 
D
n
ik
qk 
k 1
n
h
ikm
q k q m  C i
k 1 m 1
n
 Tr (U
D ik 
jk
I jU
T
ji
)
j  max( i , k )
n
 Tr (U
h ikm 
jkm
I jU
T
ji
)
j  max( i , k , m )
n
C i    m j gU ji r j
j
ji
The City College of New York
27
Manipulator Dynamics
• Dynamics Model of n-link Arm
  D ( q ) q  h ( q , q )  C ( q )
 D 11

D 

 D n 1



D1 n 


D nn 
 h1 
 
h ( q , q )  
 
 h n 
 C1 
 
C (q )  
 
 C n 
The Acceleration-related Inertia
matrix term, Symmetric
The Coriolis and Centrifugal terms
The Gravity terms
 1 
 
   Driving torque
  applied on each link
 n 
The City College of New York
28
Example
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.
L
m
Y0
Set up coordinate frame as in the figure
l 
 
0
1


r1 
0 
 
1 
Y1
g  [ 0 ,  9 .8,0 ,0 ]
X1
1
X0
C  1

S1
1
1 1

r0  T 0 r1 
 0

 0
 S1
0
C 1
0
0
1
0
0
0

0 1
r
1

0

1
The City College of New York
29
Example
C  1

S1
1
1 1
r0  T 0 r1  
 0

 0
 S1
0
C 1
0
0
1
0
0
0

0 1
r
1

0

1
L
m
Y0
Y1
 l  S1 


l

C

d 1 1
1  
1
1 1

V1  r0 
T 0 r1  Q 1T 0 r1  



0
dt


0


The City College of New York
X1
1
X0
30
Example
Kinetic energy
dK 
 l  S1 


l

C

1
1 
 l  S  1 l  C  1 0
K   Tr ( 


0
2


0


2
 l 2  (S1 ) 2
 l  S1  C 1
 2
2
2

l

S


C

l

(
C

)
1
1
1
1
 Tr ( 

2
0
0

0
0

1
1
2
T
Tr (V1V1 ) dm
2
0 )1 dm
0
0
0
0
0

0  2
) 1 m
0

0 
1 2 2
2

 [ l ( S  1 )  l ( C  1 ) ]m   l m  1
2
2
2
2
2
2
The City College of New York
31
Example
• Potential energy
P   mg (T r ) 
1
0 1
 m 0
 9 .8 m  l  S  1
 9 .8
0
C  1

S1

0
 0

 0
 S1
0
C 1
0
0
1
0
0
0  l 
 
0 0
 
0  0 
 
1  1 
• Lagrange function
L K P 
1
2
2

l m  1  9 .8 m  l  S  1
2
• Equation of Motion
L
L
1 
(
)
dt  1
1
d

d
dt
2
2
( l m 1 )  9 . 8 m  l  C  1  l m 1  9 . 8 m  l  C  1
The City College of New York
32
Example: Puma 560
• Derive dynamic equations for the first 4 links of
PUMA 560 robot
The City College of New York
33
Example: Puma 560
•
•
Set up D-H Coordinate frame
Get robot link parameters
Joint i
1
2
3
4
5
6
i
1
2
3
4
5
6
i
-90
0
90
-90
90
0
ai(mm)
0
431.8
-20.32
0
0
0
di(mm)
0
-149.09
0
433.07
0
56.25
•
Get transformation matrices T i i 1
•
Get D, H, C terms
The City College of New York
34
Example: Puma 560
•
Get D, H, C terms
n
D ik 
 Tr (U
jk
I jU
T
ji
n  3; i  1, 2 ,3
)
j  max( i , k )
T
T
T
D 11  Tr (U 11 I 1U 11 )  Tr (U 21 I 2U 21 )  Tr (U 31 I 3U 31 )
T
T
D 12  D 21  Tr (U 22 I 2U 21 )  Tr (U 32 I 3U 31 )
T
D 13  D 31  Tr (U 33 I 3U 31 )
T
T
D 22  Tr (U 22 I 2U 22 )  Tr (U 32 I 3U 32 )
T
D 23  D 32  Tr (U 33 I 3U 32 )
T
D 33  Tr (U 33 I 3U 33 )
The City College of New York
35
Example: Puma 560
•
Get D, H, C terms
n
hi 
 h1 
 
h ( q , q )  
 
 h n 
n
h
ikm
q k q m
k 1 m 1
n
 Tr (U
h ikm 
I U
jkm j
T
ji
)
j  max( i , k , m )
2
h1  h111 q1  h112 q1 q 2  h113 q1 q 3  h121 q1 q 2  h122 q 2
 h123 q 2 q 3  h131 q 3 q1  h132 q 3 q 2  h133 q 3
T
2
2
T
T
h111  Tr (U 111 I 1U 11 )  Tr (U 211 I 2U 21 )  Tr (U 311 I 3U 31 )
T
T
T
T
h113  Tr (U 313 I 3U 31 )
T
T
h123  Tr (U 323 I 3U 31 )
h112  Tr (U 212 I 2U 21 )  Tr (U 312 I 3U 31 )
h121  Tr (U 221 I 2U 21 )  Tr (U 321 I 3U 31 )
h122  Tr (U 222 I 2U 21 )  Tr (U 322 I 3U 31 )
T
……
The City College of New York
T
36
Example: Puma 560
•
Get D, H, C terms
 U ij
q k
 U ijk
U 211  ( Q 1 ) T 0
U 111  ( Q 1 ) T 0
2
2
1
U 212  U 221  Q1T Q 2 T1
1
0
U 313  Q1T Q 3T
2
0
 T 0 j 1Q j T jk11Q k T ki1
 k 1
k 1
i
  T 0 Q k T j 1 Q j T j 1
0
i j

2
1
1
2
ik
or
U 312  U 321  Q1T 0 Q 2 T1
2
U 323  U 332  T 0 Q 2 T1 Q 3T 2
1
i jk
U 311  ( Q1 ) T 0
2
U 222  T 0 ( Q 2 ) T1
3
2
ik  j
3
2
3
3
U 322  T 0 ( Q 2 ) T1
2
1
U 331  Q 1T 0 Q 3T 2
2
The City College of New York
3
2
3
U 333  T 0 Q 3 Q 2 T 2
2
3
37
Example: Puma 560
•
Get D, H, C terms
n
C i    m j gU ji r j
j
ji
C 1   m 1 gU 11 r1  m 2 gU
1
C 2   m 2 gU
C 3   m 3 gU
r  m 3 gU
21 2
r  m 3 gU
22 2
2
2
3
r
31 3
3
r
32 3
3
r
33 3
The City College of New York
38
Thank you!
Homework 4 posted on the web.
Next class: Manipulator Control
z
y
z
y
z
x
z
y
x
x
y
x
The City College of New York
39