Professional Documents
Culture Documents
Dynamic model
!
uCartesian(t)
q(t)
) = u
"(q, q , q
2
Direct dynamics
!
direct relation
" u1 %
u(t) = $ ! '
$$ ''
# uN &
" q1 %
q(t) = $ ! '
$$ ''
# qN &
q(0),q (0)
initial state at t != 0
!
!
experimental solution
!
! apply torques/forces with motors and measure joint variables
with encoders (with sampling time Tc )
) = u
"(q, q , q
solution by simulation
! use dynamic model and integrate numerically the differential
equations (with simulation step Ts ! Tc )
Robotics 2
Inverse dynamics
!
inverse relation
ud(t)
required input
for t [0,T]
desired motion
for t [0,T]
!
!
experimental solution
! repeated motion trials of direct dynamics using uk(t), with
iterative learning of nominal torques updated on trial k+1 based
on the error in [0,T] measured in trial k: uk(t) ud(t)
) = u
"(q, q , q
analytic solution
! use dynamic model and compute algebraically the values ud(t)
at every time instant t
Robotics 2
dynamic equations in
symbolic/closed form
best for study of dynamic
properties and analysis of
control schemes
Newton-Euler method
(balance of forces/torques)
!
dynamic equations in
numeric/recursive form
best for implementation of
control schemes (inverse
dynamics in real time)
Euler-Lagrange method
(energy-based approach)
q " IR N
!
!
Euler-Lagrange
equations
d " L "L
#
= ui
dt "q i "qi
i = 1, ..., N
Robotics 2
b!
"
" m = n "
" = n"m
bm
!m, "m
1!
1 2
2
2
2
Robotics 2
- d cos "
!m, "m
n!m, "
Fx
p x = ! sin"
p x = ! cos " # " = J x!
"
!
potential energy
1 2
L = T " U = I # + mg0 dcos # " U0
2
"L
= I #
"#
d "L
= I #
dt "#
"L
= $mg0 dsin#
"#
sum of
non-conservative
torques
8
n!m, "
"
+
g
d
sin
=
#
$
+
b
"
+
cos
+ Fx
' 2
0
m
m* m
2 m
n
n
n
n
n
&n
)
Robotics 2
mass m = # "(x,y,z)dx dy dz = # dm
B
rc
RF0
1
position of
rc = " r dm
mB
center of mass (CoM)
when!all vectors are referred to a body frame
RFc attached to the CoM, then
!
rc = 0 " B r dm = 0
RFc
kinetic energy
(fundamental)
kinematic relation
for a rigid body
!
Robotics 2
mass density
T=
1
2
"
T
v
(x,y,z) v(x,y,z)dm
B
v = v c + " # r = v c + S(")r
skew-symmetric matrix
10
1
T
v
+
S(")r
[
]
[v c + S(")r ] dm
c
#
2B
1
1 T T
T
T
= # v c v c dm + # v c S(")r dm + # r S (")S(")r dm
2B
2B
B
1
T
= m vc vc
2
!
!
translational
kinetic energy
!
(point mass
in CoM)
Knig theorem
Robotics 2
= v c S(") # r dm = 0
B
1
2
sum of elements
on the diagonal
of a matrix
aTb=trace{abT}
$ trace{ S(")r# r
S T (")} dm
%
( T
1
T
= trace{ S(")' $ r# r dm* S (")}
2
&B
)
+
rotational
kinetic energy
(of the whole body)
1
= trace{ S(") Jc S T (")}
2
1
= " T Ic " Euler matrix
Ex #1: provide the expressions
2
11
y
a
"I
xx
$
Ic =
$
#
!
b
" 1 m(a2 + b2 )
$2
Ic = $
$
#
x
a
Steiner theorem
!
I = Ic + m (r r" E3#3 $ rr
!
Robotics 2
Iyy
1
12
m(a2 + c2 )
%
'
'
1
m(a2 + b2 )'&
12
2
2
% " 12 m(b + c )
' =$
' $
Izz & $
#
)=I
identity
matrix
+ m S (r)S(r)
Ex #3: prove the
last equality
1
12
m 3(a2 + b2 ) + h2
%
'
'
Izz '
&
Izz = Iyy
T = " Ti
i=1
Ti = Ti (q j , q j , j " i)
!
vci
Knig theorem
!i
RFci
i-th link (body)
of the robot
Robotics 2
1
1 T
T
Ti = mi v ci v ci + " i Ici " i
2
2
absolute velocity of
the center of mass (CoM)
absolute
angular velocity
of whole body
13
constant!
Robotics 2
# " x y dm
2
2
x
+
z
)dm
"(
14
.
Dependence of T from q and q
!i
vci
link i-1
qN
link 1
q1
q2
qi-1
link i
link N
qi
" !
v ci = JLi (q) q = $ 1 ! i
$$
# !
(partial) Jacobians
typically expressed in RF0
# !
" i = JAi(q) q = % 1 ! i
%%
!
$ !
Robotics 2
qi+1
0 ! 0%
0 ! 0 ' q
''
0 ! 0&
3 rows
0 ! 0&
0 ! 0 ( q
((
0 ! 0'
3 rows
15
Final expression of T
1 N
T = # (mi v ciT v ci + " iT Ici " i)
2 i=1
NOTE:
in practice, NEVER
!
use this formula
(or partial Jacobians)
for computing T;
a better method
!
is available...
N
&
1 T#
T
T
= q % "mi JLi (q)JLi(q) + JAi(q)Ici JAi(q)( q
2 $ i=1
'
1 T
T(q, q ) = q B(q)q
2
constant if ! i
is expressed in RFci
else
0
Ici(q)= 0 R i (q) i Ici 0 R iT (q)
! matrix
robot (generalized) inertia
" symmetric
" positive definite,q always invertible
Robotics 2
16
U = " Ui
i=1
Ui = Ui (q j , j " i)
!
!
Ui = " mi g Tr0,ci
gravity acceleration
position of the
vector
center of mass of link i
dependence
! on q
"r0,ci % 0
"ri,ci %
1
i(1
$ 1 ' = A1 (q1 ) A2 (q2 )! A i (qi ) $ 1 '
# &
# &
Robotics 2
typically
expressed
in RF0
constant
in RFi
17
Summarizing ...
positive definite
1 T
1
T = q B(q)q = " bij (q)q i q j # 0 quadratic form
2
2 i,j
T = 0 q = 0
U = U(q)
kinetic
energy
potential
energy
Lagrangian
Euler-Lagrange !
equations
d "L
"L
#
= uk
dt "q k "qk
k = 1,...,N
non-conservative (active/dissipative)
generalized forces performing work on qk coordinate
!
Robotics 2
18
L(q, q ) =
"L
= # b kj q j
"q k
j
!
(dependences on q
are not shown)
1
" bij (q)q iq j # U(q)
2 i,j
"b kj
d "L
j + #
= # b kjq
q i q j
dt "q k
j
i,j "qi
"L 1 "bij
"U
= #
q i q j $
"qk 2 i,j "qk
"qk
..
LINEAR terms in ACCELERATION q
.
QUADRATIC terms in VELOCITY q
19
% # b kj 1 # b ij (
#U
exchanging
indices i,j
Robotics 2
20
INERTIAL
terms
i, j
#U
= uk
# qk
CENTRIFUGAL (i=j)
and CORIOLIS (i#j) terms
k = 1,,N
GRAVITY
terms gk(q)
!
bkk(q) = inertia at joint k when joint k accelerates (bkk > 0!!)
21
1.
+ c(q, q ) + g(q) = u
B(q)q
c k (q, q ) = q T C k (q)q
k-th column
of matrix B(q)
2.
NOTE:
!
these models
are in the form
) = u
"(q, q , q
as expected
Robotics 2
T
#
1 " bk # " bk &
"B &
Ck (q) = %
+%
)
(
(
2 % " q $ " q ' " qk (
$
'
k-th component
of vector c
symmetric
matrix
+ S(q, q )q + g(q) = u
B(q)q
NOT a
symmetric
matrix
factorization of c
by S is not unique!
22
A structural property
matrix B " 2S is skew-symmetric
Proof
kj
b = # "b
! q i
kj
i "qi
% # b kj # b ki # b ij (
1
2s kj = 2" c kjiq i = 2" '
+
$
* q i
# qj # qk )
i
i 2 & # qi
$ # b ik # b ji '
because of the
x T B " 2S x = 0, #x
Robotics !
2
23
Energy conservation
!
E = 0
E = q Tu
q T B " 2S q = 0, #q, q
!Robotics 2
here, any
factorization
of vector c
by a matrix S
can be used
y
d
dc1
pc2
q2
!
x
q1
" q1 + d cos q2 %
p c2 = $ d sinq2 '
$
'
0
#
&
Robotics 2
U = constant
(on horizontal plane)
v c1
T
= p c1
p c1 = q 12
1
T1 = m1q 12
2
!
1
1 T
T
T2 = m2 v c2 v c2 + "2 Ic2 "2
2
2
! q2 q 2 &
#0&
# q 1 " d sin
v c2 = % d cos q2 q 2 ( "2 = % 0 (
%
(
%% ((
%
(
0
$
'
$ q 2 '
1
1
2
2 2
25
" c1 (q, q ) %
c(q, q ) = $
'
c
q,
q
# 2( )&
c k (q, q ) = q T C k (q)q
T
#
1 " bk # " bk &
"B &
where Ck (q) = %
+%
)
(
(
!
%
2 " q $ " q ' " qk (
$
'
!
0
0
& #0
& #0 0&&
1 ##0
C1 (q) = % %
+%
"%
(
(
(
2 $ $ 0 "m2d cosq2 ' $ 0 "m2d cosq2 ' $ 0 0 (' '
!
c1 (q, q ) = "m2d cos q2 q 22
0
1 # # 0 "m2d cos q2 & #
C2 (q) = % %
+%
(
2 $$0
0
' $ "m2d cos q2
=0
Robotics 2
0& #
0
"m2d cos q2 & &
"%
((
(
0 ' $ "m2d cos q2
0
''
!
c2 (q, q ) = 0
26
+ c(q, q ) = u
B(q)q
# m1 + m2
% "m d sin q
$ 2
2
27
Appendix:
Vector format derivation of dynamic model
T
T
d #" L & #" L &
% ( )% ( = u
dt $ " q ' $ " q '
1
L = q TB(q)q " U(q)
[0
2
... 1 ... 0]
i=1
i-th
position
dyadic expansion
!
! T
T
N #
#
&
#" L &
T
d
"
L
" bi &
T
+ B(q)q = B(q)q
+ )% ( q i q
% ( = B(q)q
% ( = (q B(q)) = B(q)q
dt $ " q '
$ " q '
i=1 $ " q '
!
T
T
T
T
N #
# " L & T # 1 T # N " bi T & " U & # 1 T # N " bi & " U &
&
#
&
1
" bi
"U
e i (q * (( = %% q %)
qi ( * (( = )% ( qi q * % (
% ( = %% q % )
2 i=1 $ " q '
$ " q' $ 2 $ i=1 " q ' " q ' $ 2 $ i=1 " q ' " q '
$ " q'
!
T' .
T
+N $
$
'
$
'
" bi 1 " bi ) 0
"U
&
B(q)q + *&
# & ) )qi q + & ) = u
-, i=1 % " q 2 % " q ( ( 0/ % " q (
Robotics 2
S(q, q )
g(q)
28