Robotics 2

Dynamic model of robots:

Analysis, properties, extensions,
parametrization, identification, uses

Prof. Alessandro De Luca

 Analysis of inertial couplings
"b 0%
!  Cartesian robot B = $ 11 '
# 0 b22 &
"b11 b12 %
!  Cartesian “skew” robot B =$ '
#b21 b22 &

PR robot " !b11 b12 (q2 )%

!  B(q) = $ '
#b21 (q2 ) b22 &
"b!
11 (q2 ) b12 (q2 )%
!  2R robot B(q) = $ '
#b21 (q2 ) b22 &
! "b (q ,q )
!  3R articulated robot 11 2 3 0 0 %
$ '
(under simplifying B(q) = $ 0 b22 (q3 ) b23 (q3 )'
! $ 0 b23 (q3 ) b33 '&
assumptions on the CoM) #

!  dynamic model turns out to be linear if

center of mass
!  g≡0 ! d = 0 in PR of link 2
!  B constant ⇒ c ≡ 0 d2 = 0 in 2R on joint 2 axis
Robotics 2 2
 Analysis of gravity term
!  static balancing
!  distribution of masses (including motors)
!  mechanical compensation
!  articulated system of springs
g(q) " 0
!  closed kinematic chains
!  absence of gravity
!  applications in space
!  constant U (motion on horizontal plane)
!

Robotics 2 3
 Bounds on dynamic terms
!  for an open-chain (serial) manipulator, there always exist
positive. real constants k0 to k7 such that, for any value of
q and q
2
k 0 " B(q) " k 1 + k 2 q + k 3 q inertia matrix

factorization matrix of
S(q, q˙ ) " q˙ k 4 + k5 q
( ) Coriolis/centrifugal terms

! g(q) " k 6 + k 7 q gravity vector

!  if the robot has only revolute joints, these simplify to

!
k 0 " B(q) " k 1 S(q, q˙ ) " k 4 q˙ g(q) " k 6
!
NOTE: norms are either for vectors or for matrices (induced norms)
Robotics 2 4
! ! !
 Robots with closed kinematic chains -1

Comau Smart NJ130 MIT Direct Drive Mark II and Mark III

Robotics 2 5
 Robots with closed kinematic chains -2

MIT Direct Drive Mark IV UMinnesota Direct Drive Arm

(planar five-bar linkage) (spatial five-bar linkage)
Robotics 2 6
 Robot with parallelogram structure
(planar) kinematics and dynamics

4 !5 center of mass:
3
q2 - ! arbitrary ! ci
! c3 y ! c4
1
5 E-E parallelogram:
! c1 ! !1 = ! 3
q2 ! !2 = ! 4
! 2 ! q1 x
! c2
direct kinematics
!
" ! 1c1 % " ! 5 cos(q2 ( ) ) % " ! 1!
c1 % " ! 5 c2 %
p EE = $ + =
' $ ! sin(q ( ) ) ' $ !! ' ( $! s '
! s
# 1 1& # 5 2 s
& # 1 1& # 5 2&
!
position of center of masses
!" ! c % "! c % "! c % "! c % "! c % "! c %
p c1 = $ c1 1 ' p c2 = $ c2 2 ' p c3 = $ 2 2 ' + $ c3 1 ' p c4 = $ 1 1 ' ( $ c4 2 '
# ! c1s1 & # ! c2s 2 & # ! 2s 2 & # ! c3s1 & # ! 1s1 & # ! c4s 2 &

Robotics 2 7
! ! ! !
 Kinetic energy
linear/angular velocities
# "! s & # "! s & # "! s & # "! s &
v c1 = % c1 1 ( q˙ 1 v c2 = % c2 2 ( q˙ 2 v c3 = % c3 1 ( q˙ 1 + % 2 2 ( q˙ 2
$ ! c1c1 ' $ ! c2c2 ' $ ! c3c1 ' $ ! 2s 2 '
# "! s & # "! s &
v c4 = % 1 1 ( q˙ 1 " % c4 2 ( q˙ 2 "1 = " 3 = q˙ 1 " 2 = " 4 = q˙ 2
$ ! 1c1 ' $ ! c4c2 '
! ! !
Note: a 2D notation is used here!

! 1 2 ! 1 ! 1 1
Ti 2
T1 = m1! c1q1 + Ic1,zzq1
˙ ˙ 2
T2 = m2! c2q2 + Ic2,zz q˙ 22
2
˙ 2

2 2 2 2
1 1
T3 = Ic3,zz q1 + m3 (!22q˙ 22 + !2c3q˙ 12 + 2! 2! c3c2"1q˙ 1q˙ 2 )
˙ 2

2 2
! 1 2 1! 2 2 2 2
T4 = Ic4,zz q˙ 2 + m4 (! 1q˙ 1 + ! c4q˙ 2 " 2! 1! c4c2"1q˙ 1q˙ 2 )
2 2
!
Robotics 2 8

!
 Robot inertia matrix
4
1
T = " Ti = q˙ TB(q)q˙
i=1 2
# Ic1,zz + m1!2c1 + Ic3,zz + m3!2c3 + m4!21 symm &
B(q) = % (
$ (m3! 2! c3! " m4! 1! c4 )c2"1 Ic2,zz + m2!2c2 + Ic4,zz + m4!2c4 + m3!22 '

structural condition
in mechanical design
m 3 ! 2 ! c3 = m 4 ! 1! c4 (*)
!

B(q) diagonal and constant ⇒ centrifugal and Coriolis terms ≡ 0

! mechanically DECOUPLED and LINEAR
dynamic model (up to the gravity term g(q))

big advantage for the design of a motion control law!

Robotics 2 9
 Potential energy and gravity terms
from the y-components of vectors pci
Ui U1 = m1g0! c1s1 U2 = m2g0! c2s 2
U3 = m3g0 (! 2s 2 + ! c3s1 ) U4 = m4g0 (! 1s1 " ! c4s 2 )

4
! !
U= " Ui
! ! i=1 gravity
# " U & T # g0 (m1! c1 + m3! c3 + m4! 1 )c1 & # g1 (q1 ) & components
g(q) = % ( = % (=% ( are always
$ " q ' $ g0 (m2! c2 + m3! 2 ) m4! c4 )c2 ' $g2 (q2 )' “decoupled”

in addition,
! b11q ˙˙1 + g1 (q1 ) = u1 ui
(non-conservative) torque
when (*) holds b22˙q˙2 + g2 (q2 ) = u2 producing work on qi
!
further structural conditions in the mechanical design lead to g(q) ≡ 0!!

Robotics 2 ! 10
 Adding dynamic terms ...
!  dissipative phenomena due to friction at the joints/transmissions
!  viscous, dry, Coulomb, ...

!  local effects at the joints

!  difficult to model in general, except for:

uV,i = "FV,i q˙ i uS,i = "FS,i sgn(q˙ i)

!  inclusion of electrical motors (as additional rigid bodies)
!  motor i mounted on link i-1 (or before), typically with its motion

!(spinning) axis aligned

! with joint i axis
!  (balanced) mass of motor included in total mass of carrying link

!  (rotor) inertia has to be added to robot kinetic energy

!  transmissions with reduction gears (often, large reduction ratios)

!  in some cases, multiple motors cooperate in moving multiple links:

use a transmission coupling matrix T (with off-diagonal elements)

Robotics 2 11
 Placement of motors along the chain
. .
!m1 !mN
motor 1 motor N

joint 2
RFw link 2 link N
link 1 RFN-1
(world frame)
RF1
RF0
RFN

link N - 1
. .
link 0 motor 2 joint N !mi = nri !i
(base) joint1
"i = nri "mi

.
!m2
Robotics 2 12
 Resulting dynamic model
!  simplifying assumption: in the rotational part of kinetic
energy, only the “spinning” rotor velocity is considered
1 2 1 1 N
1
Tmi = Imi "˙mi = Imi n2ri q˙ 2i = Bmi q˙ 2i Tm = " Tmi = q˙ T Bm q˙
2 2 2 i=1 2
diagonal, >0
!  including all added terms, the robot dynamics becomes
moved to
! ! the left ...
(B(q) + Bm )˙q˙ + c(q, q˙ ) + g(q) + FV q˙ + FS sgn(q˙ ) = "
motor torques
constant does NOT Fv > 0, FS > 0 (after
contribute to c diagonal reduction gears)
!  scaling by the reduction gears, looking from the motor side
diagonal except the terms b
! ( " b ii(q) %+ "1 %( + jj motor torques
*Im + diag# 2 &- ˙.˙m + diag # & ** / bj (q)q ˙˙ j + f(q, q˙ )-- = 0m (before
) n
$ ri ', n
$ ri ' ) j=1,..,n , reduction gears)
Robotics 2 13
 Including joint elasticity
!  in industrial robots, use of motion transmissions based on
!  belts
!  harmonic drives
!  long shafts
introduces flexibility between actuating motors (input) and
driven links (output)
!  in research robots for human cooperation, compliance in
the transmissions is introduced on purpose for safety
!  actuator relocation by means of (compliant) cables and pulleys
!  harmonic drives and lightweight (but rigid) link design

!  redundant (macro-mini or parallel) actuation, with elastic couplings

!  in both cases, flexibility is modeled as concentrated at the joints

!  most of the times, assuming small joint deformation (elastic domain)
Robotics 2 14
 Robots with joint elasticity

DLR LWR-III
with harmonic drives
Dexter
with cable transmissions

elastic
motor load/link
spring
(stiffness K)

Quanser Flexible Joint video Stanford DECMMA

(1-dof linear, educational) with micro-macro actuation
Robotics 2 15
 Dynamic model
of robots with elastic joints
!  introduce 2N generalized coordinates
!  q = N link positions
!  ! = N motor positions (after reduction, !i = !mi/nri)
1
!  add motor kinetic energy Tm to that of the links Tq = q˙ T B(q) q˙
N 2
1 1 1 1
Tmi = Imi "˙mi
2
= Imi n2ri "˙i2 = Bmi "˙i2 Tm = " Tmi = #˙ T Bm #˙
2 2 2 i=1
2
diagonal, >0
!  add elastic potential energy Ue to that due
! to gravity Ug(q)
!  K = matrix of joint stiffness (diagonal, >0)
!
1 !
N
1 2 2 1 T
Uei = K i (qi " (#mi /nri)) = K i (qi " # i) Ue = "Uei = (q - # ) K (q - # )
2 2 i=1
2
!  apply Euler-Lagrange equations w.r.t. (q, !)
no external torques
2N 2nd-order B(q) ˙q˙ + c(q, q˙ ) + g(q) +K(q " # ) = 0 performing work on q
! differential !
equations B #˙˙ + K(# " q) = $
m
Robotics 2 16
 Dynamic parameters of robots -1

!  consider a generic link Center of Mass

of a fully rigid robot (CoM) frame i
link i ICi zi

joint i
rCi kinematic frame i
CoM Ci yi
(DH or modified DH)
mig0 Oi
xi
base ri
frame 0
"r % " Ici,xx Ici,xy Ici,xz %
xi
!  each link is characterized by 10 mi rCi = $ryi ' ICi = $$ Ici,yy
'
Ici,yz '
dynamic parameters $ '
#rzi & #symm Ici,zz &
!  however, the robot dynamics depends in a nonlinear way on some of these
parameters (e.g., through the combination Ici,zz+mirxi2)
Robotics 2 ! ! 17
 Dynamic parameters of robots -2

!  kinetic energy and gravity potential energy can both be rewritten so

that a new set of dynamic parameters appears only in a linear way
"  need to re-express link inertia and CoM position in (any) known kinematic
frame attached to the link (same orientation as the barycentric frame)

!  fundamental kinematic relation

v ci = v i + " i # rci = v i + S(" i)rci = v i $ S(rci)" i
!  kinetic energy of link i
T
Ti = mi v v ci + " I " i = mi ( v i # S(rci)" i ) ( v i # S(rci)" i) + 12 " iT Ici " i
1
2
T
ci
1
2
T
i ci
1
2
!
= 12 mi v iT v i + 12 " iT (Ici + mi S T (rci)S(rci)) " i # v iT S(mi rci )" i
" Ii,xx Ii,xy Ii,xz %
$ '
Steiner theorem
( 2
)
= Ici + mi rci E - rciT rci = Ii = $ Ii,yy Ii,yz '
! # symm Ii,zz &
3!3 identity matrix
Robotics 2 18
 Dynamic parameters of robots -3

!  gravitational potential energy of link i

Ui = " mi g0Tr0,ci = " mi g0T (ri + rci) = " mi g0Tri " g0T # mi rci
!  by expressing vectors and matrices in frame i, both Ti and Ui are
linear in the set of 10 (constant) standard parameters "i
! Ti = 12 mi iv iT i v i + mi irciT S(i v i ) i " i + 12 i" iT iIi i" i Ui = "mi g0Tri " ig0T # mi irci
mass ! CoM
mass inertia
position
of link i of link i
(0-th order of link i
(1-st order (2-nd order
! moment) ! moment)
# & moment)
% m i ( T
" i = % mi rci ( = mi mi irci,x
i
( i
mi rci,y i
mi rci,z i
Ii,xx i
Ii,xy i
Ii,xz i
Ii,yy i
Ii,yz i
Ii,zz )
%vect{ iI }(
$ i '

!  since the E-L equations involve only linear operations on T and U, also
the robot dynamic model is linear in the standard parameters " # R10N$
! Robotics 2 19
 Dynamic parameters of robots -4

!  using a N!10N regression matrix Y" that depends only on kinematic

quantities, the robot dynamic equations can always be rewritten linearly
in the standard dynamic parameters as
B(q)˙q˙ + c(q, q˙ ) + g(q) = Y" (q, q˙ , ˙q˙) " = u
" T = ( " 1T " 2T ! " NT )
!  the open kinematic chain structure of the manipulator implies that the i-th
dynamic equation may depend only on the standard dynamic parameters
!
of links i to N ⇒ Y" has a block upper triangular structure
! # Y1,1 Y1,2 … Y1,N &
% 0 Y2,2 … Y2,N (
Y" (q, q˙ , q
˙˙) = % ( with row vectors Yi,j of size 1!10
% ! " ! (
$ 0 0 YN,N '
A nice property: element bij of B(q) is function of at most (qk+1, ... ,qN), with k=min{i,j},
and of at most the inertial parameters of links r to N, with r=max{i,j}
Robotics 2 20

!
 Dynamic parameters of robots -5

!  many standard parameters do not appear (“play no role”) in the dynamic

model of the given robot ⇒ the associated columns of Y" are 0!
!  some standard parameters may appear only in fixed combinations with
others ⇒ the associated columns of Y" are linearly dependent!
!  one can isolate p << 10N independent groups of parameters "
(associated to p functionally independent columns Yindep of Y") and partition
matrix Y" in two blocks, the second containing dependent (or zero) columns
as Ydep = YindepT, for a suitable constant p!(10N-p) matrix T
# " indep & # " indep &
Y" (q, q˙ , q ( )
˙˙) " = Yindep Ydep %
"
$ dep '
(
( = Yindep Yindep T )
%" (
$ dep '
( )
= Yindep " indep + T" dep = Y(q, q˙ , q
˙˙) a
!  these grouped parameters are called dynamic coefficients a # Rp,
“the only that matter” in robot dynamics (= base parameters by W. Khalil)

!!  the minimal number p of dynamic coefficients that is needed can also be
checked numerically (see later → Identification)
Robotics 2 21
 Linear parameterization
of robot dynamics
it is always possible to rewrite the dynamic model in the form
regression a = vector of
matrix dynamic coefficients

B(q)˙q˙ + c(q, q˙ ) + g(q) = Y(q, q˙ , ˙q˙) a = u

N!p p!1
e.g., the heuristic grouping (found by inspection) on a 2R planar robot
# a1 & a1 = Ic1,zz + m1d12 + Ic2,zz + m2d22 + m2!21
! #q
˙˙ c2 (2q ˙˙2 ) " s 2 (q˙ 22 + 2q˙ 1q˙ 2 )
˙˙1 + q q
˙˙2 c1 &
%a (
c12 % 2 ( #u1 & a2 = m2! 1d2 a3 = Ic2,zz + m2d22
% 1 ( a3 = % (
$0 c2q˙˙1 + s 2q˙ 12 ˙q˙1+ q˙˙2 0 c12 ' %a ( $u2 ' a4 = g0 (m1d1 + m2! 1 )
% 4! (
$ a5 ' a5 = g0 m2d2
! !
NOTE: 4 more coefficients will be present when including the coefficients Fv,i and Fs,i of viscous and
!
dry friction at the joints (“decoupled” terms, each appearing only in the respective equation i=1,2)
! !
Robotics 2 22
 Linear parametrization
of a 2R planar robot (N=2)
!  being the kinematics known (i.e., ! 1 and g0), the number of dynamic
coefficients can be reduced since we can merge the two coefficients
a2 = m2! 1d2 a5 = g0 m2d2 ⇒ a2 = m2d2 (factoring out ! 1 and g0)
!  therefore, after regrouping, p = 4 dynamic coefficients are sufficient
!
# a1 &
! #q˙˙1 c2 (2q
! 1! ˙˙1 + q˙˙2 ) " ! 1s 2 (q˙ 22 + 2q
˙ 1q˙ 2 ) + g0c12
! q
˙˙2 g0c1 & %!a2 ( #u1 &
% (% ( = Ya =u =% (
$0 ! 1c2q˙˙1 + ! 1s 2q˙ 12 + g0c12 ˙q˙1+ q˙˙2 0 ' % a3 ( $u2 '
$ a4 '
a1 = Ic1,zz + m1d12 + Ic2,zz + m2d22 + m2!21 a2 = m2d2
a3 = Ic2,zz + m2d22 a4 = m1d1 + m2! 1
!
! this (minimal) linear parametrization
!  ! of robot dynamics is not unique,
both in terms of the chosen set of dynamic coefficients a and for the
associated
! regression matrix Y !
!  a systematic procedure for its derivation would be preferable
Robotics 2 23
 Linear parametrization
of a 2R planar robot (N=2)
!  as alternative, consider the following general procedure
!  10N = 20 standard parameters are defined for the two links
!  from the assumptions made on CoM locations, only 5 such parameters actually
appear, namely (with di = rci,x)
m1d1 I1,zz = Ic1,zz + m1d12 (link 1) m2 m2d2 I2,zz = Ic2,zz + m2d22 (link 2)
!  in the 2!5 matrix Y" , the 3rd column (associated to m2) is Y"3 = Y"1 ! 1 + Y"2 !21
!  after regrouping/reordering, again p = 4 dynamic coefficients are sufficient
!
# a1 &
#g c q c 2q q s q 2
˙˙ & % a2 (
2 + 2q1q2 ) + g0c12 q 1+ q2
˙˙ ! ( ˙˙ + ˙˙ ) " ! ( ˙ ˙ ˙ ˙˙! #u1 &
% 0 1 1 1 2 1 2 1 2
( %a ( = Y a = u = % (
$ 0 0 ˙˙ ˙ 2
! 1c2q1 + ! 1s 2q1 + g0c12 q1+ q2 ' % 3 (
˙˙ ˙˙ $u2 '
$ a4 '
a1 = m1d1 + m2! 1 a2 = I1,zz + m2!21 = (Ic1,zz + m1d12 ) + m2!21 a3 = m2d2 a4 = I2,zz = Ic2,zz + m2d22

!  determining a minimal parameterization (i.e., minimizing p) is important for

!
!  experimental identification of dynamic coefficients
! ! 
!
adaptive/robust
! !
control design in the presence of uncertain parameters
Robotics 2 24
 Identification of dynamic coefficients
!  in order to “use” the model, one needs to know the numeric values of
the robot dynamic coefficients
!  robot manufacturers provide at most only a few principal dynamic

parameters (e.g., link masses)

!  estimates can be found with CAD tools (e.g., assuming uniform mass)
!  friction coefficients are (slowly) varying over time
!  lubrication of joints/transmissions

!  for an added payload (attached to the E-E)

!  a change in the 10 dynamic parameters of last link

!  this implies a variation of (almost) all robot dynamic coefficients

!  preliminary identification experiments are needed

!  robot in motion (dynamic issues, not just static or geometric ones!)

!  only the robot dynamic coefficients can be identified (not all link

standard parameters!)
Robotics 2 25
 Identification experiments
1.  choose a motion trajectory that is sufficiently “exciting”, i.e.,
"  explores the robot workspace and involves all components in the
robot dynamic model
"  is periodic, with multiple frequency components
2.  execute this motion (approximately) by means of a control law
"  taking advantage of any available information on the robot model
. .
"  usually, u = KP(qd-q)+KD(qd-q) (PD, no model information used)
.
3.  measure q (encoder) and, if available, q in nc time instants
"  joint velocity and acceleration can be later estimated off line by
numerical differentiation (use of non-causal filters is feasible)
4.  with such measures/estimates, evaluate the regression matrix Y (on
the left) and use the applied commands (on the right) in the
expression
Y(q(t k ), q˙ (t k ), ˙q˙(t k )) a = u(t k ) k = 1,…,nc
Robotics 2 26
 Least Squares (LS) identification
!  set up the system of linear equations
" Y(q(t1 ), q˙ (t1 ), ˙q˙(t1 )) % " u(t1 ) %
$ ' $ '
nc ! N $ ! ' a = $ ' Ya = u
$ Y(q(t ), q˙ (t ), ˙q˙(t )) ' $ u(t ) '
# n c n c n c & # nc &
!  sufficiently “exciting” trajectories, large enough number of

samples (nc ! N_≫ p), and their suitable selection/position,

guarantee rank(Y) = p (full column! rank)
_
!
!  solution by pseudoinversion of matrix Y
-1
#
a=Y u= Y Y ( T
) YT u
!  one can also use a weighted pseudoinverse, to take into
account different levels of noise in the collected measures

Robotics 2 ! 27
 Additional remarks on LS identification
!  it is convenient to preserve the block (upper) triangular structure of
the regression matrix, by “stacking” all time evaluations sequenced by
the rows of the original Y matrix
nc
" Y1 (q(t1 ), q˙ (t1 ), ˙q˙(t1 )) % " u1(t1 ) %
nc $ ! ' $ ! '
$ Y (q(t ), q˙ (t ), ˙q˙(t )) ' $ u (t ) '
$ 1 n c n c n c ' $ 1 nc ' Y=
N! $ Y2 (q(t1 ), q˙ (t1 ), q ˙˙(t1 )) ' u (t )
a =$ 2 1 ' Ya = u
$ ! ' $ ! '
$ YN (q(t1 ), q˙ (t1 ), ˙q˙(t1 )) ' $ uN(t1 ) '
nc $ ! ' $ ! ' nc
$ ˙˙(t n ))'& $ ' !
# YN (q(t n c ), q˙ (t n c ), q c #u N (t n )
c &
! "  numerical check of full column rank is
more robust ⇔ rank = p (# of col’s)
!  further practical hints _
!  outlier data can be eliminated in advance (when building Y)
!
!  if complex friction models are not included in Y䏙a, discard the data

collected at joint velocities that are close to zero

Robotics 2 28
 Summary on dynamic identification
KUKA IR 361
robot and
optimal
excitation
trajectory

J. Swevers, W. Verdonck, and J. De Schutter:

“Dynamic model identification for industrial robots”
IEEE Control Systems Mag., Oct 2007
results after identification (first three joints only)
Robotics 2 29
 Dynamic identification of KUKA LWR4
video

data acquisition for identification

dynamic coefficients: 30 inertial, 12 for gravity
validation after identification (for all 7 joints):
C. Gaz, F. Flacco, A. De Luca: on new desired trajectories, compare
“Identifying the dynamic model used by the KUKA LWR: torques computed with the identified model
A reverse engineering approach” and torques measured by joint torque sensors
IEEE ICRA 2014, Hong Kong, June 2014
Robotics 2 30
 Use of the dynamic model
Inverse dynamics
!  given a desired trajectory qd(t) of motion
..
!  twice differentiable (∃ qd(t))

!  possibly obtained from a task/Cartesian trajectory rd(t), by

(differential) kinematic inversion

the input torque needed to execute it is

" d = (B(qd ) + Bm )˙q˙ d + c(qd , q˙ d ) + g(qd ) + FV q˙ d + FS sgn(q˙ d )

!  useful also for control (e.g., nominal feedforward)

!  however, this algebraic computation (∀t) is not efficient when
performed using the above Lagrangian approach
!
!  symbolic terms grow much longer, quite rapidly for larger n

!  in real time, better a numerical computation based on

Newton-Euler method
Robotics 2 31
 Dynamic scaling of trajectories
uniform time scaling of motion

!  given a smooth original trajectory qd(t) of motion for t ∈ [0,T]

!  suppose to rescale time as t → r(t) (a strictly increasing function of t)

!  in the new time scale, the scaled trajectory qs(r) satisfies

dqd dqs dr
qd (t) = qs (r(t)) q˙ d (t) = = = q's (r) r˙
same path executed dt dr dt
(at different instants of time)

dq˙ d " dq's dr %

˙q˙ d (t) = =$ ' r˙ + q's (r)˙r˙ = q''s (r) r˙2 + q's (r)˙r˙
! ! dt # dr dt &
!  uniform scaling of the trajectory occurs when r(t) = k t

q˙ d (t) = k q's (kt) ˙q˙ d (t) = k 2 q''s (kt)

!
Q: what is the new input torque needed to execute the scaled trajectory?
(suppose dissipative terms can be neglected)
Robotics 2 32
! !
 Dynamic scaling of trajectories
inverse dynamics under uniform time scaling

!  the new torque could be recomputed through the inverse dynamics,

for every r = k t ∈[0,T’]=[0,kT] along the scaled trajectory, as
" s (kt) = B(qs )q''s + c(qs,q's ) + g(qs )
!  however, being the dynamic model linear in the acceleration and
quadratic in the velocity, it is
" d (t) = B(qd )˙q˙ d + c(qd , q˙ d ) + g(qd ) = B(qs )k 2q''s + c(qs,kq's ) + g(qs )
! 2
= k (B(qs )q''s + c(qs ,q's )) + g(qs ) = k 2 (" s (kt) # g(qs )) + g(qs )
!  thus, saving separately the total torque %d(t) and gravity torque gd(t)
in the inverse dynamics computation along the original trajectory, the
new input torque is obtained directly as
k>1: slow down
! 1 ⇒ reduce torque
" s (kt) = 2 (" d (t) # g(qd (t))) + g(qd (t)) k<1: speed up
k ⇒ increase torque
gravity term (only position-dependent): does NOT scale!
Robotics 2 33
 Dynamic scaling of trajectories
numerical example

!  rest-to-rest motion with cubic polynomials for planar 2R robot under gravity
(from downward equilibrium to horizontal link 1 & upward vertical link 2)
!  original trajectory lasts T=0.5 s (but maybe violates the torque limit at joint 1)
only joint 1 torque is shown
torque only due
to non-zero initial
acceleration

total torque gravity torque

component

at equilibrium
= zero gravity
torque

for both joints

Robotics 2 34
 Dynamic scaling of trajectories
numerical example

original gravity torque !  scaling with k=2 (slower) → T’ = 1 s

total to sustain the link
torque at steady state

" d (0.1) # g(qd (0.1)) = 20 Nm

! 20
" s (2# 0.1) $ g(qs (2# 0.1)) = = 5 Nm
T=0.5 s 22
*
gravity torque
component * *
does not scale 0 Nm
scaled !
total
torque 1
4

!
T=0.5 s k =2 T’=1 s
T’=1 s
Robotics 2 35
!
 State equations
Direct dynamics
N differential
Lagrangian
dynamic model
B(q)˙q˙ + c(q, q˙ ) + g(q) = u 2nd-order
equations
" x1 % " q % 2N
defining the vector of state variables as x = =
$ x ' $ q˙ ' ( IR
# 2& # &
state equations
!
" x˙ 1 % " x2 % " 0 % 2N differential
x˙ = $ ' = $ (1 ! ' + $ (1 ' u 1st-order
# x 2 & # (B (x1 )[c(x1 ,x 2 ) + g(x1 )] & # B (x1 ) &
˙ equations

= f(x) + G(x)u
2N!1 2N!N
" q % .
~
! another choice... x = $
˜ ' generalized x = ... (do it as exercise)
# B(q)q˙ & momentum
Robotics 2 36
 Dynamic simulation
Simulink here, a generic 2-dof robot
block c(q, q˙ ) q, q˙ q˙ 1 (0) q1 (0)
scheme
q˙˙1 q˙ 1
input torque ! _
" " q1
! !
command
! +
(open-loop u B-1(q)
or in _ !
q˙˙2
!
q˙ 2
feedback)
!
"! " q2

g(q) q q! ! q˙ 2 (0) q2 (0)

including “inv(B)”
!
"  initialization (dynamic coefficients and initial state) !
!
! Matlab
! !
"  calls to (user-defined) functions for the evaluation of model terms
"  choice of a numerical integration method (and of its parameters)

Robotics 2 37
 … an incorrect realization

c1+ g1
q˙ 1 (0) q1 (0)
_ 1 q˙˙1 q˙ 1
u1
_ b11 " " q1
! !
b12 causality
! algebraic
! loop! principle
is violated!!
b!12 q˙ 2 (0) q2 (0)
_ !1 q˙˙2 ! q˙ 2
u2
_ b22 " " q2
! !

c2 + g2 ! !
!
inversion of the ! matrix in toto is needed
! robot inertia
(and not only of its elements on the diagonal...)
Robotics 2 38
 Approximate linearization
!  we can derive a linear dynamic model of the robot, which is valid
locally around a given operative condition
!  useful for analysis, design, and gain tuning of linear (or, the linear

part of) control laws

!  approximation by Taylor series expansion, up to the first order

!  linearization around a (constant) equilibrium state or along a

(nominal, time-varying) equilibrium trajectory

!  usually, we work with (nonlinear) state equations; for mechanical

systems, it is more convenient to directly use the 2nd-order model

!  same result, but easier derivation
. ..
equilibrium state (q,q) = (qe,0) [ qe=0 ] g(qe ) = ue
. .
equilibrium trajectory (q,q) = (qd (t),qd(t))

B(qd )˙q˙ d + c(qd , q˙ d ) + g(qd ) = ud

Robotics 2 ! 39
 Linearization at an equilibrium state
!  variations around an equilibrium state
q = qe + "q q˙ = q˙ e + "q˙ = "q˙ ˙q˙ = ˙q˙ e + "˙q˙ = "˙q˙ u = ue + "u

!  keeping into account the quadratic dependence of c terms

on velocity (thus, neglected around the zero velocity)
! #g
B(qe ) "q + g(qe ) +
˙˙ "q + o( "q , "q˙ ) = ue + "u
# q q=q
e
infinitesimal terms
of second or higher order
G(qe )
!  in state-space format
!
# "q & # 0 I& # 0 &
"x = % ( "x˙ = % -1 "x + % -1 "u = A "x + B "u
$ "q˙ ' ! $ -B (qe )G(qe ) 0 (' (
$ B (qe ) '

Robotics 2 40

!
 Linearization along a trajectory
!  variations around an equilibrium trajectory
q = qd + "q q˙ = q˙ d + "q˙ ˙˙ = q
q ˙˙ d + "q
˙˙ u = ud + "u

!  developing terms in the dynamic model ...

! B(qd + "q)(˙q˙ d + "˙q˙ ) + c(qd + "q, q˙ d + "q˙ ) + g(qd + "q) = ud + "u

i-th row of the
$b N
identity matrix
B(qd + "q) # B(qd ) + % i e iT "q
i=1 $ q q=q
d
! C1 (qd , q˙ d )
$c $c
c(qd + "q, q˙ d + "q˙ ) # c(qd , q˙ d ) + "q + "q˙
! $ q q=q d $ q˙ q=q d
q˙ = q˙ d q˙ = q˙ d
!
g(qd + "q) # g(qd ) + G(qd )"q
C2 (qd , q˙ d )
Robotics 2 41
!
 Linearization along a trajectory (cont)

!  after simplifications …
B(qd ) "˙q˙ + C2 (qd , q˙ d )"q˙ + D(qd , q˙ d , ˙q˙ d )"q = "u
with
N
" bi ˙˙ d e iT
D(qd , q˙ d , q
˙˙ d ) = G(qd ) + C1 (qd , q˙ d ) + # q
i=1 " q q=q
d
! !  in state-space format
# 0 I & # 0 &
"x˙ = % -1 -1 ( "x + % -1 ( "u
! $ -B (qd )D(qd , qd , qd ) -B (qd )C2 (qd , qd ) '
˙ ˙˙ ˙ $ B (qd ) '
= A(t ) "x + B (t ) "u

a linear, but time-varying system!!

! Robotics 2 42
 Coordinate transformation
q " IR N B(q)q
˙˙ + c(q, q˙ ) + g(q) = B(q)q
˙˙ + n(q, q˙ ) = u q 1

if we wish/need to use a new set of generalized coordinates p

p " IR N p = f(q) q = f (p)

"1
! !
"f(q) T
˙p = q˙ = J(q)q˙ q˙ = J"1 (q)p˙ , u q = J (q)u p 1
"q
!p˙˙ = J(q)q˙˙ + J˙(q)
! q˙ ˙˙ = J"1 (q) p˙˙ " J˙ (q)J"1 (q)p˙
q [ ]
˙ ! "1 ˙ T
B(q)J (q)p " B(q)J (q) J(q)J (q)p + n(q, q) = J (q)u p
˙
˙ "1
! "1
˙

! J"T!
(q) # pre-multiplying the whole equation...
Robotics 2 43
 Robot dynamic model
after coordinate transformation

(
˙˙ + J" T (q) n(q, q˙ ) " B(q)J"1 (q) ˙J(q)J"1 (q)p˙ = up
J" T (q)B(q)J"1 (q)p )
for actual computation,
q"p these inner substitutions q, q˙ " p,p˙
are not necessary non-conservative
generalized forces
B p (p)p
˙˙ + c p (p,p˙ ) + gp (p) = up performing work on p

symmetric,
! B p = J BJ "T "1
!
positive definite gp = J"T g
(out of singularities)
quadratic .
cp = J "T
c " BJ JJ p˙ = J"T c " B p J˙ J"1p˙
( "1 ˙ "1
) dependence on p
!
c p (p,p˙ ) = S p (p,p˙ )p˙ B˙ p " 2Sp skew - symmetric
! !
when p = E-E pose, this is the robot dynamic model in Cartesian coordinates
Q: What if the robot is redundant with respect to the Cartesian task?
! Robotics 2 44
! !
 Optimal point-to-point robot motion
considering the dynamic model

"  given the initial and final robot configurations (at rest) and
actuator torque bounds, find
!  the minimum-time Tmin motion

!  the (global/integral) minimum-energy Emin motion

and the associated command torques needed to execute them

"  a complex nonlinear optimization problem solved numerically
video video

Tmin= 1.32 s, E = 306 T = 1.60 s, Emin = 6.14

Robotics 2 45