25-Nov-23

HCMC University of Tech. & Education Faculty of Mechanical Engineering

ROBOTICS
THEORY OF APPLIED
ROBOTICS
KINEMATICS, DYNAMICS,
AND CONTROL

4. DYNAMICS

Lecture notes
Ha Le Nhu Ngoc Thanh, Ph.D.
Faculty of Mechanical Engineering
HCMC University of Tech. & Education

Ha Le Nhu Ngoc Thanh, Ph.D. 2 Lecture notes: Introduction to Robotics

HCMC University of Tech. & Education Faculty of Mechanical Engineering HCMC University of Tech. & Education Faculty of Mechanical Engineering

Dynamics The Lagrange Formulation

Given:
Forward dynamics : Given , , and , find   for simulation
+ a set of generalized coordinates, q  q1, q 2 ,, qn 
Inverse dynamics : Given , , and , find   for design controller and control robot
Approachs to solve dynamic problem: + a set of generalized velocity, q  q1, q2, , qn 
 Largangian formulation: based on potential enegy and kinetic enegy + a set of generalized torque,   1, 2 ,, n 
 Newton-Euler formulation: analysis based on isolating each link and considering all for a prismatic joint,  i
is a force acting along the axis of the joint
forces acting on it Kinetic energy (𝑲)

Lagrange’s equations : L q, q   K q, q   U q 

 
Kineticenergy Potential energy
Potential
For nDOFs, the Lagrange formulation n - equation: energy U(𝒒)

Lagrangian 𝑳 𝒒, 𝒒̇ d  L  L
  
dt  q  q

Ha Le Nhu Ngoc Thanh, Ph.D. 3 Lecture notes: Introduction to Robotics Ha Le Nhu Ngoc Thanh, Ph.D. 4 Lecture notes: Introduction to Robotics
 25-Nov-23

HCMC University of Tech. & Education Faculty of Mechanical Engineering HCMC University of Tech. & Education Faculty of Mechanical Engineering

The Lagrange Formulation  For a multi-link (mass M (q) is a matrix) with a velocity q ,
The Lagrange Formulation
L q, q   K q, q   U q 
 Kinetic energy is a scalar given by the quadratic form:

Kinetic energy

Potential energy  V (q, q )  G(q)  
 Equation of motion: M (q)q (**)
d  L  L
1 T
   
K q M (q)q
dt  q  q 2
K   1 T  HOW TO DETERMINE EQUATION OF MOTION (**)
   q M (q)q   M (q)q
q q  2   Determine M, V, G
d     
K q, q  U q  d  K   d  d 
  K (q, q ) U (q)        M (q ) q  M (q )  q 
If the mass matrix M (q) is determined, we can determine K , and V
dt  q   q   
dt  q  dt  dt 


d  K   
K  1 q T M (q)q

U q  0 
d  K  K     
 M (q )q  M (q)q 
   
q
   q, q   q q, q   q U q dt  q 
from M (q)  
V (q, q2)
dt  q   M (q) 
q T q 
Gravity vector G(𝒒)  q1 
K   1 T  1  
d  K  K U    q M (q)q     
where V is the vector of centrifugal and Coriolis forces:
Simpler form :    () q q  2  2   coefficients of
  T M (q) 
dt  q  q q
 q q   M (q)  Coriolis forces
  qn   q T 
Inertial force Inertial forces Gravity forces  M (q)   q 
 q T q   q
 1 1 
 
d  K  K 1
  M (q)q  
q1  V (q, q )  M (q)q      V (q, q )  C (q)[q 2 ]  B(q)[qq
 ]
 For a single link (mass m is scalar) with a velocity v, Inertia l forces :   
dt  q  q
 M (q)q
2
 
 2 
 T M (q)   T M (q) 
 Kinetic : K 
1 T 1
v mv  mv 2
 q q   q q 
2 2
 q n


 qn  coefficients of
V (q,q )
n n 1 centrifugal forces
(*)
   V (q, q )  G (q)  
 M (q )q n
where C (q)   nn , B(q)   2

Ha Le Nhu Ngoc Thanh, Ph.D. 5 Lecture notes: Introduction to Robotics Ha Le Nhu Ngoc Thanh, Ph.D. 6 Lecture notes: Introduction to Robotics

HCMC University of Tech. & Education Faculty of Mechanical Engineering HCMC University of Tech. & Education Faculty of Mechanical Engineering

Total Kinetic Energy of manipulator:

Determine M Determine V
link i  1, n 
n
K   Ki , Centrifugal and Coriolis Forces
Ci i 1
where each link -i includes linear motion C and rotational motion i  M (q) 
q T mij
q  where the matrix M (q)  mij 
Central of link i i

 
1    mijk , k  1, n
Ki  m T   Ti IC i  q1    nn  qk
2 i C i Ci 1
i
V (q, q )  M (q)q     ,
  d  mij 
1 n 2  dmij  n
K   m T   Ti ICi i ()  T M (q) 
q q  and time derivative of mij :      m q
2 i 1 i C i C i
 qn  dt dt  qk  k 1 ijk k

   J 
q 1 T
we also have :  C v
  i  J iq , where J v at C i , and K  q M (q)q
Kinetic energy : 
 i 2 For instance, consider the case of 2DOF manipulator

 i i
dmij n

 
1 1 n
m           mijkqk  m 11  m111q1  m112q2
work done by external forces to  ( )
  q T M (q)q   mi q T J vT J v q  q T J T IC J  q  11 m12  q1    v1   g1    1  dt k 1

m21 m22 q2   v2  g 2  2 
bring the system from the rest 2 2 i 1 i i i i i
 m m12 

 
 11 
to its current state 1 1 n       m11 m12   q1
 q T M (q)q    q T miJ vT J v  J T IC J  q 
M q1  m111 m121  m
 m121 
     
m22  m211 m221 
symmetric  111 
m121 m221 
q1 q1 m21 m22   m21
2 2 i 1  i i i i i  M (q ) V (q ,q ) G (q ) 
 q1

q1 
 Linear motion : n   
1
K  m 2
1
2
1
2  i 1 

 q T M (q)q  q T   miJ vT J v  J T IC J    q
i i i i i 

   T M (q)   T M (q) 
2 q q   q q 
 Rotation motion : 1   m 11 m 12  1 
  q 1 q1 
n
   
 M (q)    miJ vT J v  J T IC J   V (q, q )  M (q )q      

 q  
1
K  T IC  i 1 
i i i i i  2 q T M (q) q  m21 m 22  2 q T M (q) q 
2    
(IC : inertial moment ) The mass matrix M (q) is symmetric positive definite  q 2   q 2  m m122 
 meaning q T M (q)q  0, M (q)  M T (q)  112 
m122 m222 
 

Ha Le Nhu Ngoc Thanh, Ph.D. 7 Lecture notes: Introduction to Robotics Ha Le Nhu Ngoc Thanh, Ph.D. 8 Lecture notes: Introduction to Robotics
 25-Nov-23

HCMC University of Tech. & Education Faculty of Mechanical Engineering HCMC University of Tech. & Education Faculty of Mechanical Engineering

Centrifugal and Coriolis Forces dmij n Centrifugal and Coriolis Forces

dt
  mijk qk  m ij  m111q1  m112q2 n n 1
k 1 n
m
  11
m 12  m111q1  m112q2 m121q1  m122q2 
  
 
  ] , where C (q)  nn , B(q )  
In general case for nDOFs: V (q, q )  C (q)[q 2 ]  B(q )[ qq 2
m 21 m 22  m211q1  m212q2 m221q1  m222q2 

 T M (q)   m m121  
q  q T  111
q    m  q 
m221   b  q2 
1
V (q, q )  M (q)q  

  q1
 m 
m  1
    11  12  q    121
   1,11 b1,22  b1,nn   12 
2 q T M (q) q  m21 m22  2  T m112 m122   b2,11 b2,22  b2,nn  q 
 
q   q  C q ]  
(q)[ 2  2 ,
 q 2 
  m
 m222              q q 
122    2  12 
b b  b
n n n 1
 n,11 n ,22 n,nn  qn   q1q3 
   
Apply :
1
 
m  mikj  m jki  bijk
2 ijk
  
 
 2b 2b1,13  2b1,1n 2b1,23  2b1,2n 
 2b1,(n 1)n   q1qn 
 1,12
1 1   2b 2b2,13  2b2,1n 2b1,12  2b2,2n  2b2,(n 1)n   q2q3 
 m  m  m  m  m122  m221  q12  m112  m121  m121    B(q) [   ]   2,12
qq 
 V (q, q )   2
111 111 111
2 122              q2q4 
  2   m  m  m  q1q2   
 1 m  m  m  1
m  m222  m222  q2   212 221 122  2bn,12 2bn,13  2bn,1n 2b1,12  2bn ,2n  2bn,(n 1)n    
(n 1)n ( n 1)n
n 1

 2
211 211 112
2 222 
2 2  
 q q 
 2 n 
  
b b  q 2  2b  q 2   
 V (q, q )   111 122   12    112  q1q2  V (q, q )  C  1   B q q 
q 2   1 2  q q 
 n 1 n 
b211 b222  q  2b212  


 2 
   2 
C q 2 B
where bijk 
1

m  mikj  m jki
2 ijk

Ha Le Nhu Ngoc Thanh, Ph.D. 9 Lecture notes: Introduction to Robotics Ha Le Nhu Ngoc Thanh, Ph.D. 10 Lecture notes: Introduction to Robotics

HCMC University of Tech. & Education Faculty of Mechanical Engineering HCMC University of Tech. & Education Faculty of Mechanical Engineering

Determine G Potential energy  U i  mi gT pc  i

 Example: 2-DOF RP Manipulator Link ai
0 1 0
i di
 2 0
i
 2  1
where vector g , pc depend Global frame coordinate x2 z2 12 0 0 d2 0
i

example as picture g  0, g, 0 , pc  0, hi , 0, g  9.81

T
0 l c  d c  0 c 
   1 1   2 1     1 
n
i
x1 C2  
 we have p0 = 00 p = 0, pc = l1s1 , pc = 20 p  d2s1 , z0 = 0 z 0  0, z1  01 R 1 z1  s1 
 U  U i C1 0
1
 0 
2
 0     
        1   0 
i 1
 for each link i z1
n  pc  Jacobian matrix of C1 Jacobian matrix of C2
U  i 
0 0 0 
  mi gT 
Gi  
   
    
qi  qi  I  0 0 0  J 0 z0  pc  p0 0 J J v 2  z0  pc  p0 z1 
i 1 

z0
 
0 0 I zz 
 
 J1  J1 0   v 1
0
J 1
   
0 
1
z0

0
 
 0 J2  J1 J2   v 1
   
J 1 J  2  
2
z0

0 
   
0 1 0l c  l s  0 1 0d c  d s 
  1 1   1 1    2 1   2 1 

s
 1

z0  pc  p0  1 0 0l1s1    l1c1 
    
z0  p2  p0   1 0 0d2s1    d2c1 
    
0 c1 0 s 0  0  l1c1  0 0 0 0   0  0 0 0 0   0 
Potential energy of link i :  1
c 0 s1 0
 1
c
0 c1
0 s1 0  0  l1s1 
0
T  1 , p   1        
U i  mi g 0hi  U 0 1  0 1 0 0 c1  0 1 0 0 l1   0  l1s1 0 d2s1 c1 
        l c   d c 
 0

0 0 1

 0

0 0 1  1   1 
     1 1 0 l s 0 0 0  2 1 s1  d s c  0 0
l c   0 0  1 1     0 0   2 1 1   
Where U0 represents the potential energy  1 1 

 pc  l1s1 
0
J1   ,  J v1   l1c1 0, J  1  0 0 0
J2   ,  J v 2   d2c1 s1 , J  2  0 0
   0 0      0 0     
at some reference level (normal U0 =0) 1

 0     0 0 1 0    0 0  1 0

1 0 0 0  s 0 c1 d2c1  d c   0 0
  0 0 

   1  2 1  1 0   1 0 
0 1 0 0  c 0 s1 d2s1  d s   
1
T    02T   1   pc   2 1 
2  0 0 1 d2   0 1 0 0   0 

G   J vT m1g   J vT m2g    J vT mn g 

     
2

 0 0 0 1   0 0 0 1 
 
1 

 1 2 n  d c 
 2 1 
 pc  d 2s1 
2  
 0 

Ha Le Nhu Ngoc Thanh, Ph.D. 11 Lecture notes: Introduction to Robotics Ha Le Nhu Ngoc Thanh, Ph.D. 12 Lecture notes: Introduction to Robotics
 25-Nov-23

HCMC University of Tech. & Education Faculty of Mechanical Engineering HCMC University of Tech. & Education Faculty of Mechanical Engineering

 Compute matrix V (q, q) G   J vT m1g  J vT m2g    J vT mn g

Example: 2-DOF RP Manipulator bijk 
1
m  mikj  m jki
2 ijk
 Example: 2-DOF RP Manipulator  1 2 n 
n  2  B(q)  21, [qq
  ]  11, 𝑀 𝑞 𝑞̈ + 𝑉 𝑞 𝑞̇ + 𝐺 𝑞 = 𝜏
 Compute matrix M (q ) C (q)   22, [ q 2 ]  21,
 Compute matrix G (q )
 

2
 2b 
M (q)    miJ vT J v  J T IC J     ]  q1q2 ,
 B(q)   112 , [qq
G (q)   J vT m1g   J vT m2g , g  0, g, 0
T
i 1 
i i i i i  2b212 
   
 1 2 
 m1J vT J v  J T IC J   m2J vT J v  J T IC J 
1 1 1 1 1 2 2 2 2 2
1

b112  m112  m121  m121   1 m11
2 q 2

1 m11
2 d2
 m2d2 l s l c 0 
 0 
 d s d c 0  0 
 
  1 1 1 1  m1g    2 1 2 1  m2g 
2
where
1 1  m21 m22 m12   0 0 0   
  0   1c s 0 
  0 
  b212  m212  m221  m122      0 1
l s l c 0 l1s1 0 m l 2 0 2 2  q2 q1 q 2     
m1 J vT J v  m1  1 1 1 1   l1c1 0   1 1  m l  m d  gc 
 0 0 0    0 0 2m d 
    ]  1d2  G (q)   1 1 1
1 1
 0 0  B(q)   2 2 , [ qq 2 2
 0  m2gs1 
    
0 0 10 0 0  0 0 I  b b122  2  2
J T IC J        zz 1 0  q1 
0 0 0  0 0    C (q)   ,[q ]    2 , q1  1, q2  d2
1 11

1 1 1 0 0 00 0 I  1 0  0 0 b211 b222  q2 
 zz 1  

d s d c 0 d2s1 c1  m d 2 0 
 1

b111  m111  m111  m111 
2
 1 m11
2 q1
0 Equation of motion :
m2 J vT J v  m2  2 1 2 1   d2c1 s1    2 2 
c
 1 s 0  
 0   0
0  
m2  1
b122  m122  m122  m221   2
1  m12 m22  m l 2  m d 2  I  I
 1 1 0   1  2m2d2     0 0 12  m1l1  m2d2  gc1  1 
 d   m d
2 2 1
  0       
   
 2 2 zz 1 zz 2
2 2  q2 q1 
 m2  d2   0  1 2 0 d 2   m2gs1
  2 
   0  2 2  2 
0 0 10 0 0 0 0 I 0 1 1  m21 m11 
J T IC J    0 0 0 0 0   zz 2  b211  m211  m211  m112   2    m2d2
0 0 0    0 0 2 2  q1 d2 
1 0 
2 2 2
0 0 I zz 2 
1  m22 
m l 2  m d 2  I  I

M (q)   1 1
0   
1

b222  m222  m222  m222  
2
 2  q2 
0
2 2 zz 1 zz 2
 m
 0 m2   ij  22  0 0 2 12   
2m2d2   

 0 0 12 
,[q ]    2   V (q, q )   0  1d2   m d
 C (q)     
m2d2 0   2 2 0 d 2 
d2    
2

Ha Le Nhu Ngoc Thanh, Ph.D. 13 Lecture notes: Introduction to Robotics Ha Le Nhu Ngoc Thanh, Ph.D. 14 Lecture notes: Introduction to Robotics

HCMC University of Tech. & Education Faculty of Mechanical Engineering

Example: 2-DOF RR Manipulator

Find Equation motion of Robot?

Ha Le Nhu Ngoc Thanh, Ph.D. 15 Lecture notes: Introduction to Robotics