Robotics 2

Dynamic model of robots:

Analysis, properties, extensions, uses

Prof. Alessandro De Luca

 Analysis of inertial couplings
𝑚!! 0
n Cartesian robot 𝑀=
0 𝑚""

𝑚!! 𝑚!"
n Cartesian “skew” robot 𝑀= 𝑚
!" 𝑚""

𝑚!! 𝑚!" (𝑞" )

n PR robot 𝑀=
𝑚!" (𝑞" ) 𝑚""

𝑚!! (𝑞" ) 𝑚!" (𝑞" )

n 2R robot 𝑀=
𝑚!" (𝑞" ) 𝑚""

n 3R articulated robot 𝑚!! (𝑞" , 𝑞# ) 0 0

(under simplifying 𝑀= 0 𝑚"" (𝑞# ) 𝑚"# (𝑞# )
assumptions on the CoMs) 0 𝑚"# (𝑞# ) 𝑚##

Robotics 2 2
 Analysis of gravity term
n absence of gravity
n constant 𝑈! (motion on horizontal plane)
n applications in remote space
n static balancing 𝑔 𝑞 ≈0
n distribution of masses (including motors)
n mechanical compensation
n articulated system of springs
n closed kinematic chains

Robotics 2 3
 Bounds on dynamic terms
n for an open-chain (serial) manipulator, there always exist
positive real constants 𝑘0 to 𝑘7 such that, for any value of
𝑞 and 𝑞̇
#
𝑘! ≤ 𝑀 𝑞 ≤ 𝑘" + 𝑘# 𝑞 + 𝑘$ 𝑞 inertia matrix
factorization matrix of
𝑆 𝑞, 𝑞̇ ≤ 𝑘% + 𝑘& 𝑞 𝑞̇ Coriolis/centrifugal terms

𝑔 𝑞 ≤ 𝑘' + 𝑘( 𝑞 gravity vector

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

𝑘! ≤ 𝑀 𝑞 ≤ 𝑘" 𝑆 𝑞, 𝑞̇ ≤ 𝑘% 𝑞̇ 𝑔 𝑞 ≤ 𝑘'
(the same holds true with bounds 𝑞$,&$' ≤ 𝑞$ ≤ 𝑞$,&() on prismatic joints)

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 𝑙+
3 center of mass:
𝑞$ − 𝜋 arbitrary 𝑙*$
𝑙*# 𝑦 𝑙*,
5 E-E parallelogram:
1
𝑙*!
𝑙' = 𝑙(
𝑞#
𝑙$ = 𝑙)
2 𝑞" 𝑥
𝑙*"
direct kinematics
𝑙' 𝑐' 𝑙+ cos 𝑞$ − 𝜋 𝑙' 𝑐' 𝑙+ 𝑐$
𝑝** = + = −
𝑙' 𝑠' 𝑙+ sin 𝑞$ − 𝜋 𝑙' 𝑠' 𝑙+ 𝑠$

position of center of masses

𝑙*! 𝑐! 𝑙*" 𝑐" 𝑙" 𝑐" 𝑙*# 𝑐! 𝑙! 𝑐! 𝑙*, 𝑐"
𝑝*! = 𝑝*" = 𝑝*# = + 𝑝*, = −
𝑙*! 𝑠! 𝑙*" 𝑠" 𝑙" 𝑠" 𝑙*# 𝑠! 𝑙! 𝑠! 𝑙*, 𝑠"

Robotics 2 7
 Kinetic energy
linear/angular velocities
−𝑙*! 𝑠! −𝑙*# 𝑠! −𝑙" 𝑠"
𝑣*! = 𝑞̇ ! 𝑣*# = 𝑞̇ ! + 𝑞̇ " 𝜔! = 𝜔# = 𝑞̇ !
𝑙*! 𝑐! 𝑙*# 𝑐! 𝑙" 𝑐"
−𝑙*" 𝑠" −𝑙! 𝑠! 𝑙 𝑠
𝑣*" = 𝑞̇ " 𝑣*, = 𝑞̇ ! + *, " 𝑞̇ " 𝜔" = 𝜔, = 𝑞̇ "
𝑙*" 𝑐" 𝑙! 𝑐! −𝑙*, 𝑐"

Note: a (planar) 2D notation is used here!

' ' ' '

𝑇) $ $
𝑇' = $ 𝑚' 𝑙,' 𝑞̇ ' + $ 𝐼,',.. 𝑞̇ '$ 𝑇$ = 𝑚 $ 𝑙 $ $
,$ 𝑞̇ $ + 𝐼,$,.. 𝑞̇ $
$
$ $
' '
𝑇( = 𝑚( 𝑙$$ 𝑞̇ $$ + 𝑙,(
$ $
𝑞̇ ' + 2𝑙$ 𝑙,( 𝑐$/' 𝑞̇ ' 𝑞̇ $ + 𝐼,(,.. 𝑞̇ '$
$ $
' '
𝑇) = 𝑚 𝑙'$ 𝑞̇ '$ + $ $
𝑙,) 𝑞̇ $ − 2𝑙' 𝑙,) 𝑐$/' 𝑞̇ ' 𝑞̇ $ + 𝐼,),.. 𝑞̇ $$
$ ) $

Robotics 2 8
 Robot inertia matrix
,
1 .
𝑇 = 4 𝑇$ = 𝑞̇ 𝑀 𝑞 𝑞̇
2
$-!

% +𝐼 % %
𝐼!",$$ + 𝑚" 𝑙!" !&,$$ + 𝑚& 𝑙!& + 𝑚' 𝑙" symm
𝑀 𝑞 = % +𝐼 % %
𝑚& 𝑙% 𝑙!& − 𝑚' 𝑙" 𝑙!' 𝑐%(" 𝐼!%,$$ + 𝑚% 𝑙!% !',$$ + 𝑚' 𝑙!' + 𝑚& 𝑙%

structural condition
𝑚$ 𝑙# 𝑙*$ = 𝑚% 𝑙" 𝑙*% (*)
in mechanical design

𝑀(𝑞) diagonal and constant ⇒ centrifugal and Coriolis terms ≡ 0

mechanically DECOUPLED and LINEAR 𝑀"" 0 𝑞̈ " 𝑢"
= 𝑢
dynamic model (up to the gravity term 𝑔(𝑞)) 0 𝑀%% 𝑞̈ % %

big advantage for the design of a motion control law!

Robotics 2 9
 Potential energy and gravity terms
from the 𝑦-components of vectors 𝑝*$
𝑈)
𝑈' = 𝑚' 𝑔0 𝑙,' 𝑠' 𝑈$ = 𝑚$ 𝑔0 𝑙,$ 𝑠$
𝑈( = 𝑚( 𝑔0 𝑙$ 𝑠$ + 𝑙,( 𝑠' 𝑈) = 𝑚) 𝑔0 𝑙' 𝑠' − 𝑙,) 𝑠$
,

𝑈 = 4 𝑈$
$-! gravity
.
𝜕𝑈 𝑔/ 𝑚! 𝑙*! + 𝑚# 𝑙*# + 𝑚, 𝑙! 𝑐! 𝑔 𝑞 components
𝑔 𝑞 = = = ! ! are always
𝜕𝑞 𝑔/ 𝑚" 𝑙*" + 𝑚# 𝑙" − 𝑚, 𝑙*, 𝑐" 𝑔" 𝑞"
“decoupled”

in addition, 𝑚"" 𝑞̈ " + 𝑔" 𝑞" = 𝑢" 𝑢$ are

(non-conservative) torques
when (*) holds 𝑚## 𝑞̈ # + 𝑔# 𝑞# = 𝑢# performing work on 𝑞$

further structural conditions in the mechanical design lead to 𝑔(𝑞) ≡ 0!!

Robotics 2 10
 Adding dynamic terms ...
1) dissipative phenomena due to friction at the joints/transmissions
n viscous, Coulomb, stiction, Stribeck, LuGre (dynamic)...

n local effects at the joints

n difficult to model in general, except for:

𝑢+,) = −𝐹+,) 𝑞̇ ) 𝑢-,) = −𝐹-,) sgn 𝑞̇ )

𝑭𝑽 𝒒̇

𝑭𝑪 𝒔𝒈𝒏(𝒒)
̇

𝑭𝒔𝒕𝒊𝒄𝒕𝒊𝒐𝒏

Robotics 2 11
 Adding dynamic terms ...
2) inclusion of electrical actuators (as additional rigid bodies)
n motor 𝑖 mounted on link 𝑖 − 1 (or before) , with very few exceptions

n often with its spinning axis aligned with joint axis 𝑖

n (balanced) mass of motor included in total mass of carrying link

n (rotor) inertia is to be added to robot kinetic energy

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

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

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

Unimation PUMA family

𝑚𝑜𝑡𝑜𝑟 3
𝑗𝑜𝑖𝑛𝑡 3

𝑚𝑜𝑡𝑜𝑟 2
𝑗𝑜𝑖𝑛𝑡 2

Mitsubishi RV-3S
Robotics 2 12
 Placement of motors along the chain
𝜃̇&0
𝜃̇&! rotor N
frame
motor 1 motor N

rotor 1
frame joint 2
link 2 link N
RFW link 1 RFN-1
(world frame)

RFN

RF0 RF1
link N - 1

link 0 motor 2 joint N 𝜃̇&$ = 𝑛1$ 𝜃̇$

(base) joint1
𝜏$ = 𝑛1$ 𝜏&$

𝜃̇&"
Robotics 2 13
 Resulting dynamic model
n simplifying assumption: in the rotational part of the kinetic
energy, only the “spinning” rotor velocity is considered
0
1 1 1 1 .
𝑇&$ ̇
= 𝐼&$ 𝜃&$ = 𝐼&$ 𝑛1$ 𝑞̇ $ = 𝐵&$ 𝑞̇ $"
" " "
𝑇& = 4 𝑇&$ = 𝑞̇ 𝐵& 𝑞̇
2 2 2 2
$-!
diagonal, > 0
n including all added terms, the robot dynamics becomes
moved to
the left ...
𝑀 𝑞 + 𝐵. 𝑞̈ + 𝑐 𝑞, 𝑞̇ + 𝑔 𝑞 + 𝐹+ 𝑞̇ + 𝐹- sgn 𝑞̇ = 𝜏
motor torques
constant does NOT 𝐹𝑉 > 0, 𝐹1 > 0 (after
contribute to 𝑐 diagonal reduction gears)
n scaling by the reduction gears, looking from the motor side
diagonal 7
𝑚33 (𝑞) 1 motor torques
𝐼2 + diag ̈
𝜃2 + diag C5 (𝑞)𝑞̈ 5 + 𝑓 𝑞, 𝑞̇
B𝑀 = 𝜏2 (before
%
𝑛43 𝑛43 reduction gears)
56"
Robotics 2 except the terms 𝑚!! 14
 Special actuation and associated coordinates
planar 2R robot with remotely driven forearm

n motor 1 moves link 1 by 𝑝"

n motor 2 at the base moves the absolute
angle 𝑝# of link 2
n derive the model in the 𝒑 coordinates
𝑢8%
𝑀 𝑝 𝑝̈ + 𝑐 𝑝, 𝑝̇ + 𝑔 𝑝 = 𝑢0
𝑢8" 𝑎" − 𝑎$ 𝑎# 𝑐#/"
𝑀(𝑝) = 𝑎 𝑐 𝑎$
# #/"

−𝑎# 𝑠#/" 𝑝̇ ## no more

𝑐(𝑝, 𝑝)̇ = Coriolis forces!
𝑎# 𝑠#/" 𝑝̇"#
𝑎% 𝑐"
𝑔(𝑝) = 𝑎 𝑐
& #
𝑐" = cos 𝑝" , 𝑐% = cos 𝑝% ,
𝑐%(" = cos 𝑝% − 𝑝" , 𝑠%(" = sin 𝑝% − 𝑝"
Robotics 2 15
 Including joint elasticity
n in industrial robots, use of motion transmissions based on
n belts
n harmonic drives
n long shafts
introduces flexibility between actuating motors (input) and driven
links (output)
n in research robots, compliance in transmissions is introduced on
purpose for safety (human collaboration) and/or energy efficiency
n actuator relocation by means of (compliant) cables and pulleys
n harmonic drives and lightweight (but rigid) link design
n redundant (macro-mini or parallel) actuation, with elastic couplings
n in both cases, flexibility is modeled as concentrated at the joints
n in most cases, assuming small joint deformation (elastic domain)
Robotics 2 16
 Robots with joint elasticity

DLR LWR-III
with harmonic drives
Dexter
with cable transmissions

elastic
motor spring load/link
(stiffness K)

Quanser Flexible Joint video Stanford DECMMA

(1-dof linear, educational) with micro-macro actuation
Robotics 2 17
 Dynamic model
of robots with elastic joints
n introduce 2𝑁 generalized coordinates
n 𝑞 = 𝑁 link positions
n 𝜃 = 𝑁 motor positions (after reduction, 𝜃$ = 𝜃&$ /𝑛1$ )
1 '
n add motor kinetic energy 𝑇𝑚 to that of the links 𝑇& = 𝑞̇ 𝑀(𝑞)𝑞̇
0 2
1 1 1 1 .
𝑇&$ = 𝐼&$ 𝜃&$ = 𝐼&$ 𝑛1$ 𝜃$ = 𝐵&$ 𝜃̇$"
̇ " " ̇"
𝑇& = 4 𝑇&$ = 𝜃̇ 𝐵& 𝜃̇
2 2 2 2
$-! diagonal, > 0
n add elastic potential energy 𝑈𝑒 to that due to gravity 𝑈$ (𝑞)
n 𝐾 = matrix of joint stiffness (diagonal, > 0)
" 0
1 𝜃&$ 1 " 1
𝑈2$ = 𝐾$ 𝑞$ − = 𝐾$ 𝑞$ − 𝜃$ 𝑈2 = 4 𝑈2$ = 𝑞 − 𝜃 . 𝐾 𝑞 − 𝜃
2 𝑛1$ 2 2
$-!
n apply Euler-Lagrange equations w.r.t. (𝑞, 𝜃)
no external torques
2𝑁 2nd-order 𝑀 𝑞 𝑞̈ + 𝑐 𝑞, 𝑞̇ + 𝑔 𝑞 + 𝐾 𝑞 − 𝜃 = 0 performing work on 𝑞
differential
equations 𝐵. 𝜃̈ + 𝐾 𝜃 − 𝑞 = 𝜏
Robotics 2 18
 Use of the dynamic model
inverse dynamics

n given a desired trajectory 𝑞( (𝑡)

n twice differentiable (∃ 𝑞̈ 3 (𝑡))

n possibly obtained from a task/Cartesian trajectory 𝑟3 (𝑡), by

(differential) kinematic inversion
the input torque needed to execute this motion (in free space) is

𝜏1 = 𝑀 𝑞1 + 𝐵. 𝑞̈ 1 + 𝑐 𝑞1 , 𝑞̇ 1 + 𝑔 𝑞1 + 𝐹+ 𝑞̇ 1 + 𝐹- sgn 𝑞̇ 1
5
(in contact, with an external wrench) … − 𝐽234 𝑞1 𝐹234,1
n useful also for control (e.g., nominal feedforward)
n however, this way of performing the algebraic computation (∀𝑡 ) is
not efficient when using the Lagrangian modeling approach
n symbolic terms grow much longer, quite rapidly for larger 𝑁

n in real time, numerical computation is based on Newton-Euler method

Robotics 2 19
 State equations
direct dynamics

Lagrangian 𝑁 differential
dynamic model 𝑀 𝑞 𝑞̈ + 𝑐 𝑞, 𝑞̇ + 𝑔 𝑞 = 𝑢 2nd order
equations
𝑥" 𝑞 #6
defining the vector of state variables as 𝑥 = = ∈ ℝ
𝑥# 𝑞̇
state equations

𝑥̇ " 𝑥# 0
𝑥̇ = = + 𝑢
𝑥̇ # −𝑀 /" (𝑥" ) 𝑐 𝑥" , 𝑥# + 𝑔(𝑥" ) 𝑀 /" (𝑥" )

= 𝑓(𝑥) + 𝐺(𝑥)𝑢 2𝑁 differential

1st order
2𝑁 × 1 2𝑁 × 𝑁 equations
𝑞 generalized
another choice... 𝑥B = 𝑀(𝑞)𝑞̇
momentum
𝑥Ḃ = ... (do it as exercise)

Robotics 2 20
 Dynamic simulation
Simulink here, a generic 2-dof robot
block 𝑐(𝑞, 𝑞)
̇ 𝑞, 𝑞̇ 𝑞̇ ! (0) 𝑞! (0)
scheme
𝑞̈ " 𝑞̇ "
𝑞"
input torque _
command +
(open-loop 𝑢 𝑀!" (𝑞)
or in _ 𝑞̈ # 𝑞̇ #
feedback) 𝑞#

𝑔(𝑞) 𝑞 𝑞 𝑞̇ " (0) 𝑞" (0)

including “inv(M)”
§ initialization (dynamic coefficients and initial state)
§ calls to (user-defined) Matlab functions for the evaluation of model terms
§ choice of a numerical integration method (and of its parameters)
e.g., 4th-order Runge-Kutta (ode45)
Robotics 2 21
 Approximate linearization
n we can derive a linear dynamic model of the robot, which is valid
locally around a given operative condition
n useful for analysis, design, and gain tuning of linear (or of the

linear part of) control laws

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

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

(nominal, time-varying) equilibrium trajectory

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

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

n same result, but easier derivation

equilibrium state (𝑞, 𝑞)

̇ = (𝑞) , 0) [ 𝑞̈ = 0 ] 𝑔 𝑞2 = 𝑢2

equilibrium trajectory (𝑞, 𝑞)

̇ = (𝑞( (𝑡), 𝑞̇ ( (𝑡)) [ 𝑞̈ = 𝑞̈ ( (𝑡) ]
𝑀 𝑞1 𝑞̈ 1 + 𝑐 𝑞1 , 𝑞̇ 1 + 𝑔 𝑞1 = 𝑢1
Robotics 2 22
 Linearization at an equilibrium state
n variations around an equilibrium state
𝑞 = 𝑞) + Δ𝑞 𝑞̇ = 𝑞̇ ) + Δ𝑞̇ = Δ𝑞̇ 𝑞̈ = 𝑞̈ ) + ∆𝑞̈ = ∆𝑞̈ 𝑢 = 𝑢) + Δ𝑢

n keeping into account the quadratic dependence of c terms

on velocity (thus, neglected around the zero velocity)
𝜕𝑔
𝑀 𝑞) ∆𝑞̈ + 𝑔 𝑞) + J ∆𝑞 + o ∆𝑞 , ∆𝑞̇ = 𝑢) + ∆𝑢
𝜕𝑞 &*&
9
infinitesimal terms
of second or higher order
𝐺(𝑞) )
∆𝑞
n in state-space format, with ∆𝑥 =
∆𝑞̇
̇ 0 𝐼 0
∆𝑥 = /" ∆𝑥 + /" ∆𝑢 = 𝐴 ∆𝑥 + 𝐵 ∆𝑢
−𝑀 𝑞2 𝐺(𝑞2 ) 0 𝑀 (𝑞2 )
Robotics 2 23
 Linearization along a trajectory
n variations around an equilibrium trajectory
𝑞 = 𝑞( + Δ𝑞 𝑞̇ = 𝑞̇ ( + Δ𝑞̇ 𝑞̈ = 𝑞̈ ( + ∆𝑞̈ 𝑢 = 𝑢( + Δ𝑢

n developing to 1st order the terms in the dynamic model ...

𝑀(𝑞( + ∆𝑞) 𝑞̈ ( + ∆𝑞̈ + 𝑐(𝑞( + ∆𝑞, 𝑞̇ ( + ∆𝑞)
̇ + 𝑔 𝑞( + ∆𝑞 = 𝑢( + ∆𝑢
- 𝑖-th row of the
𝜕𝑀, identity matrix
𝑀 𝑞( + ∆𝑞 ≅ 𝑀 𝑞( +N J 𝑒,' ∆𝑞
𝜕𝑞 &*&
,*" :

𝑔 𝑞( + ∆𝑞 ≅ 𝑔 𝑞( + 𝐺(𝑞( )∆𝑞 𝐶! (𝑞3 , 𝑞̇ 3 )

𝜕𝑐 𝜕𝑐
𝑐 𝑞( + ∆𝑞, 𝑞̇ ( + ∆𝑞̇ ≅ 𝑐 𝑞( , 𝑞̇ ( + J ∆𝑞 + J ∆𝑞̇
𝜕𝑞 &*&: 𝜕 𝑞̇ &*&:
̇ &̇ :
&* ̇ &̇ :
&*

𝐶" (𝑞3 , 𝑞̇ 3 )
Robotics 2 24
 Linearization along a trajectory (cont)

n after simplifications …
𝑀(𝑞! )∆𝑞̈ + 𝐶" (𝑞! , 𝑞̇ ! )∆𝑞̇ + 𝐷 𝑞! , 𝑞̇ ! , 𝑞̈ ! ∆𝑞 = ∆𝑢
with 6
𝜕𝑀)
𝐷 𝑞1 , 𝑞̇ 1 , 𝑞̈ 1 = 𝐺 𝑞1 + 𝐶" 𝑞1 , 𝑞̇ 1 +M N 𝑞̈ 1 𝑒)5
𝜕𝑞 878
)7" 4
n in state-space format

̇ 0 𝐼
∆𝑥 = −𝑀#$ 𝑞 𝐷 𝑞 , 𝑞̇ , 𝑞̈ #$ 𝑞 𝐶 𝑞 , 𝑞̇ ∆𝑥
! ! ! ! −𝑀 ! " ! !
0
+ 𝑀#$ (𝑞 ) ∆𝑢 = 𝐴(𝑡) ∆𝑥 + 𝐵(𝑡) ∆𝑢
!
a linear, but time-varying system!!

Robotics 2 25
 Coordinate transformation
𝑞 ∈ ℝ% 𝑀 𝑞 𝑞̈ + 𝑐 𝑞, 𝑞̇ + 𝑔 𝑞 = 𝑀 𝑞 𝑞̈ + 𝑛 𝑞, 𝑞̇ = 𝑢& 1

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

𝑝 ∈ ℝ% 𝑝 = 𝑓(𝑞) 𝑞 = 𝑓 #$ (𝑝) by duality

(principle of virtual work)
𝜕𝑓
𝑝̇ = 𝑞̇ = 𝐽(𝑞)𝑞̇ 𝑞̇ = 𝐽#$ (𝑞)𝑝̇ 𝑢& = 𝐽' (𝑞)𝑢( 1
𝜕𝑞
̇ 𝑞̇
𝑝̈ = 𝐽 𝑞 𝑞̈ + 𝐽(𝑞) ̇
𝑞̈ = 𝐽#$ (𝑞) 𝑝̈ − 𝐽(𝑞)𝐽 #$ (𝑞)𝑝̇

̇
𝑀 𝑞 𝐽#$ 𝑞 𝑝̈ − 𝑀(𝑞)𝐽#$ (𝑞)𝐽(𝑞)𝐽 #$ (𝑞)𝑝̇ + 𝑛 𝑞, 𝑞̇ = 𝐽' (𝑞)𝑢
(

𝐽#' (𝑞) ? pre-multiplying the whole equation...

Robotics 2 26
 Robot dynamic model
after coordinate transformation

̇
𝐽/' 𝑞 𝑀 𝑞 𝐽/" 𝑞 𝑝̈ + 𝐽/' 𝑞 𝑛 𝑞, 𝑞̇ − 𝑀(𝑞)𝐽/" (𝑞)𝐽(𝑞)𝐽 /"
(𝑞)𝑝̇ = 𝑢0

for actual computation,

𝑞→𝑝 these inner substitutions (𝑞, 𝑞)
̇ → (𝑝, 𝑝)̇
are not necessary non-conservative
generalized forces
𝑀# 𝑝 𝑝̈ + 𝑐# 𝑝, 𝑝̇ + 𝑔# 𝑝 = 𝑢# performing work on 𝑝

symmetric,
𝑀0 = 𝐽/5 𝑀𝐽/" positive definite 𝑔0 = 𝐽/5 𝑔
(out of singularities)
quadratic
𝑐0 = 𝐽/5 𝑐− 𝑀𝐽/" 𝐽 ̇ 𝐽/" 𝑝̇ = 𝐽/5 𝑐 − 𝑀0 𝐽 ̇ 𝐽/" 𝑝̇ dependence on 𝑝̇

𝑐0 (𝑝, 𝑝)̇ = 𝑆0 (𝑝, 𝑝)̇ 𝑝̇ 𝑀̇ 0 − 2𝑆0 skew-symmetric

when 𝑝 = E-E pose, this is the robot dynamic model in Cartesian coordinates
NOTE: in this case, we have implicitly assumed than 𝑀 = 𝑁 (no redundancy!)
Robotics 2 27
 Example of coordinate transformation
planar 2R robot using absolute coordinates

n motor 1 at joint 1, motor 2 at joint 2

n in place of DH angles 𝒒, use the absolute
angles 𝑝" = 𝑞" and 𝑝# = 𝑞" + 𝑞#
1 0 a linear
𝑞" 𝑝= 𝑞 = 𝐽 𝑞 transformation
𝑢" 1 1
𝑢! 1 0 1 −1
𝐽/" = 𝐽/5 =
= 𝑞# −1 1 0 1
n from 𝑀 𝑞 𝑞̈ + 𝑐 𝑞, 𝑞̇ + 𝑔 𝑞 = 𝑢&
obtained with DH relative coordinates

/5 𝑎" − 𝑎$ − 2𝑎# 𝑐#
/" 𝑎# 𝑐# 𝑎% 𝑐"
𝑀0 = 𝐽 𝑀 𝐽 = 𝑔0 = 𝐽/5 𝑔
= 𝑎 𝑐
𝑎# 𝑐# 𝑎$ & "#
# 𝑢" − 𝑢#
−2𝑎 # 𝑠# 𝑞̇ " + 𝑞 ̇ #
𝑐0 = 𝐽/5 𝑐 = /5
𝑢0 = 𝐽 𝑢8 = 𝑢#
𝑎# 𝑠# 𝑞̇ "#
Robotics 2 28
 Robot dynamic model
in the task/Cartesian space, with redundancy

dynamic model in the joint space 𝑞 ∈ ℝ- second-order task kinematics

𝑀 𝑞 𝑞̈ + 𝑛 𝑞, 𝑞̇ = 𝜏 𝑟 = 𝑓 𝑞 ∈ ℝ2 ̇ 𝑞̇
𝑟̈ = 𝐽 𝑞 𝑞̈ + 𝐽(𝑞)
𝑀<𝑁 𝐽 is full rank = 𝑀

1) isolate the joint acceleration from the dynamics 𝑞̈ = 𝑀 /" 𝑞 𝜏 − 𝑛 𝑞, 𝑞̇

2) decompose the joint torques in two complementary spaces
𝜏 = 𝐽' 𝑞 𝐹 + 𝐼 − 𝐽' 𝑞 𝐻(𝑞) 𝜏3 𝐻 is a generalized inverse of 𝐽.
∈ ℛ 𝐽' ∉ ℛ 𝐽' 𝐽. 𝐻𝐽. = 𝐽.
torques coming from
… and joint torques 𝜏; that do not!
generalized forces 𝐹
in the task space … 𝜏/ = 𝐽. 𝑞 𝐹, ∀𝐹 ∈ ℝ5 ⇒ 𝐼 − 𝐽. 𝑞 𝐻 𝑞 𝐽. 𝑞 𝐹 = 0
3) substitute 1) and 2) in the differential task kinematics
𝑟̈ = 𝐽 𝑞 𝑀 /" 𝑞 𝐽' 𝑞 𝐹 + 𝐼 − 𝐽' 𝑞 𝐻(𝑞) 𝜏3 − 𝑛 𝑞, 𝑞̇ ̇ 𝑞̇
+ 𝐽(𝑞)

4) isolate on the right-hand side the generalized forces 𝐹 in the task space …
Robotics 2 29
 Robot dynamic model
in the task/Cartesian space, with redundancy
6!
𝐽 𝑞 𝑀 6! 𝑞 𝐽. 𝑞 𝑟̈ = 𝐹 +
6!
𝐽 𝑞 𝑀 6! .
𝑞 𝐽 𝑞 𝐽 𝑞 𝑀 6! 𝑞 𝐼 − 𝐽. 𝑞 𝐻(𝑞) 𝜏/ − 𝑛 𝑞, 𝑞̇ ̇ 𝑞̇
+ 𝐽(𝑞)
.
5) choose as generalized inverse 𝐻 = 𝐽𝑀 6! 𝐽. 6! 𝐽𝑀 6! = 𝐽5 #
, i.e., the transpose
of the inertia-weighted pseudoinverse of the task Jacobian (see block of slides #2)
in this way, the joint torque component 𝜏; will NOT affect the task acceleration 𝑟̈
6! 6!
𝐽 𝑞 𝑀 6! 𝑞 𝐽. 𝑞 𝑟̈ = 𝐹 + 𝐽 𝑞 𝑀 6! 𝑞 𝐽. 𝑞 𝐽 ̇ 𝑞 𝑞̇ − 𝐽 𝑞 𝑀 6! 𝑞 𝑛 𝑞, 𝑞̇

6) the resulting (𝑀 −dimensional) task dynamics is then

external forces can be added
𝑀9 𝑞 𝑟̈ + 𝑛9 𝑞, 𝑞̇ = 𝐹 … + 𝐹234 on the rhs of the equations in
with a dynamically consistent way!
/" ' /"
𝑀4 𝑞 = 𝐽 𝑞 𝑀 𝑞 𝐽 𝑞 task inertia matrix for 𝑀 = 𝑁, these terms
𝑛4 𝑞, 𝑞̇ = 𝑀4 𝑞 ̇ 𝑞̇
𝐽 𝑞 𝑀 /" 𝑞 𝑛 𝑞, 𝑞̇ − 𝐽(𝑞) are identical to slide #27

7) an additional 𝑁 − 𝑀 -dimensional second-order dynamics is needed to describe

the full robot!
Robotics 2 30
 Dynamic scaling of trajectories
uniform time scaling of motion

n given a smooth original trajectory 𝑞( (𝑡) of motion for 𝑡 ∈ [0, 𝑇]

n suppose to rescale time as 𝑡 → 𝑟(𝑡) (a strictly increasing function of 𝑡 )

n in the new time scale, the scaled trajectory 𝑞5 (𝑟) satisfies

𝑑𝑞1 𝑑𝑞: 𝑑𝑟
𝑞1 𝑡 = 𝑞: 𝑟(𝑡) 𝑞̇ 1 𝑡 = = = 𝑞:; (𝑟) 𝑟(𝑡)
̇
same path executed
𝑑𝑡 𝑑𝑟 𝑑𝑡
(at different instants of time)

𝑑 𝑞̇ 1 𝑑𝑞:; 𝑑𝑟 ;
𝑑 𝑟̇
𝑞̈ 1 𝑡 = = 𝑟̇ + 𝑞: = 𝑞:;; (𝑟)𝑟̇ # (𝑡) + 𝑞:; (𝑟)𝑟(𝑡)
̈
𝑑𝑡 𝑑𝑟 𝑑𝑡 𝑑𝑡
n uniform scaling of the trajectory occurs when 𝑟 𝑡 = 𝑘𝑡
𝑞̇ 1 𝑡 = 𝑘𝑞:; (𝑘𝑡) 𝑞̈ 1 𝑡 = 𝑘 # 𝑞:;; (𝑘𝑡)
Q: what is the new input torque needed to execute the scaled trajectory?
(suppose dissipative terms can be neglected)

Robotics 2 31
 Dynamic scaling of trajectories
inverse dynamics under uniform time scaling

n the new torque could be recomputed through the inverse dynamics, for
every 𝑟 = 𝑘𝑡 ∈ 0, 𝑇 6 = [0, 𝑘𝑇] along the scaled trajectory, as
𝜏: 𝑘𝑡 = 𝑀 𝑞: 𝑞:;; + 𝑐 𝑞: , 𝑞:; + 𝑔(𝑞: )
n however, being the dynamic model linear in the acceleration and
quadratic in the velocity, it is
𝜏( 𝑡 = 𝑀 𝑞( 𝑞̈ ( + 𝑐 𝑞( , 𝑞̇ ( + 𝑔 𝑞( = 𝑀 𝑞5 𝑘 # 𝑞566 + 𝑐 𝑞5 , 𝑘𝑞56 + 𝑔(𝑞5 )
= 𝑘 # 𝑀 𝑞5 𝑞566 + 𝑐 𝑞5 , 𝑞56 + 𝑔 𝑞5 = 𝑘 # 𝜏5 𝑘𝑡 − 𝑔 𝑞5 + 𝑔 𝑞5
n thus, saving separately the total torque 𝜏( (𝑡) and gravity torque 𝑔( (𝑡)
in the inverse dynamics computation along the original trajectory, the
new input torque is obtained directly as
𝑘 > 1: slow down
1 ⇒ reduce torque
𝜏) 𝑘𝑡 = " 𝜏! 𝑡 − 𝑔 𝑞! (𝑡) + 𝑔 𝑞! (𝑡)
𝑘 𝑘 < 1: speed up
⇒ increase torque
gravity term (only position-dependent): does NOT scale!
Robotics 2 32
 Dynamic scaling of trajectories
numerical example

n rest-to-rest motion with cubic polynomials for planar 2R robot under gravity
(from downward equilibrium to horizontal link 1 & upward vertical link 2)
n original trajectory lasts 𝑇 = 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 33
 Dynamic scaling of trajectories
numerical example

original gravity torque n scaling with 𝑘 = 2 (slower) → 𝑇 < = 1 s

total to sustain the link
torque at steady state

𝜏! 0.1 − 𝑔 𝑞! 0.1 = 20 Nm
*

#$
𝜏" 2 A 0.1 − 𝑔 𝑞" 2 A 0.1 = = 5 Nm
#"
𝑇 = 0.5 s
*
gravity torque
component * *
does not scale 0 Nm
scaled
total
torque 1
4

𝑇 = 0.5 s 𝑘=2 𝑇=1s

𝑇! = 1 s
Robotics 2 34
 Optimal point-to-point robot motion
considering the dynamic model

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

n 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 35