robotics 2 - università di romadeluca/rob2_en/05_lagrangiandynamics_3.pdf · robotics 2 18 !...
TRANSCRIPT
![Page 1: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/1.jpg)
Robotics 2
Prof. Alessandro De Luca
Dynamic model of robots: Analysis, properties, extensions,
parametrization, identification, uses
![Page 2: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/2.jpg)
Analysis of inertial couplings
! Cartesian robot
! Cartesian “skew” robot
! PR robot
! 2R robot
! 3R articulated robot (under simplifying assumptions on the CoM)
! dynamic model turns out to be linear if ! g ≡ 0 ! B constant ⇒ c ≡ 0
!
B =b11 00 b22
"
# $
%
& '
!
B(q) =b11 b12(q2)
b21(q2) b22
"
# $
%
& '
!
B =b11 b12
b21 b22
"
# $
%
& '
d = 0 in PR d2 = 0 in 2R
center of mass of link 2
on joint 2 axis
!
B(q) =b11 q2,q3( ) 0 0
0 b22(q3) b23(q3)0 b23(q3) b33
"
#
$ $ $
%
&
' ' '
Robotics 2 2
!
B(q) =b11(q2) b12(q2)b21(q2) b22
"
# $
%
& '
![Page 3: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/3.jpg)
Analysis of gravity term
! static balancing ! distribution of masses (including motors)
! mechanical compensation ! articulated system of springs ! closed kinematic chains
! absence of gravity ! applications in space ! constant U (motion on horizontal plane)
!
g(q) " 0
Robotics 2 3
![Page 4: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/4.jpg)
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
!
k 0 " B(q) " k1 + k2 q + k3 q2
Robotics 2 4
! if the robot has only revolute joints, these simplify to
.
!
g(q) " k 6 + k7 q
!
S(q, ˙ q ) " ˙ q k 4 + k5 q( )inertia matrix
factorization matrix of Coriolis/centrifugal terms
gravity vector
!
k 0 " B(q) " k1
!
g(q) " k 6
!
S(q, ˙ q ) " k 4˙ q
NOTE: norms are either for vectors or for matrices (induced norms)
![Page 5: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/5.jpg)
Robots with closed kinematic chains - 1
Comau Smart NJ130 MIT Direct Drive Mark II and Mark III
Robotics 2 5
![Page 6: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/6.jpg)
Robots with closed kinematic chains - 2
MIT Direct Drive Mark IV (planar five-bar linkage)
UMinnesota Direct Drive Arm (spatial five-bar linkage)
Robotics 2 6
![Page 7: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/7.jpg)
Robot with parallelogram structure (planar) kinematics and dynamics
q1 q2
q2 - !
1
2
3 4
!
! c1
!
! c2
!
! c3
!
! c4
center of mass:
parallelogram:
!
!1 = ! 3
!
! 2 = ! 4
!
! ciarbitrary
!
pc1 =! c1c1
! c1s1
"
# $
%
& '
!
pc2 =! c2c2
! c2s2
"
# $
%
& '
!
pc3 =! 2c2
! 2s2
"
# $
%
& ' +! c3c1
! c3s1
"
# $
%
& '
!
pc4 =!1c1
!1s1
"
# $
%
& ' (! c4c2
! c4s2
"
# $
%
& '
position of center of masses
5
!
pEE =!1c1
!1s1
"
# $
%
& ' +! 5 cos(q2 ())! 5 sin(q2 ())
"
# $
%
& ' =!1c1
!1s1
"
# $
%
& ' (! 5 c2
! 5 s2
"
# $
%
& '
!
! 5
E-E
x
y
direct kinematics
Robotics 2 7
![Page 8: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/8.jpg)
!
vc1 ="! c1s1
! c1c1
#
$ %
&
' ( ̇ q 1
!
vc3 ="! c3s1
! c3c1
#
$ %
&
' ( ̇ q 1 +
"! 2s2
! 2s2
#
$ %
&
' ( ̇ q 2
!
vc4 ="!1s1
!1c1
#
$ %
&
' ( ̇ q 1 "
"! c4s2
! c4c2
#
$ %
&
' ( ̇ q 2
Kinetic energy
linear/angular velocities
!
vc2 ="! c2s2
! c2c2
#
$ %
&
' ( ̇ q 2
!
"1 ="3 = ˙ q 1
!
"2 ="4 = ˙ q 2
!
T1 =12
m1! c12 ˙ q 1
2 +12
Ic1,zz˙ q 12
!
T2 =12
m2! c22 ˙ q 2
2 +12
Ic2,zz ˙ q 22
!
T3 =12
Ic3,zz ˙ q 12 +
12
m3 ! 22 ˙ q 2
2 + ! c32 ˙ q 1
2 + 2!2! c3c2"1˙ q 1 ˙ q 2( )
!
T4 =12
Ic4,zz ˙ q 22 +
12
m4 !12 ˙ q 1
2 + ! c42 ˙ q 2
2 "2!1! c4c2"1˙ q 1 ˙ q 2( )
Ti
Robotics 2 8
Note: a 2D notation is used here!
![Page 9: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/9.jpg)
Robot inertia matrix
!
T = Ti =12
˙ q TB(q) ˙ q i=1
4
"
!
B(q) =Ic1,zz +m1! c1
2 + Ic3,zz +m3! c32 +m4!1
2 symmm3! 2! c3 "m4!1! c4( )c2"1 Ic2,zz +m2! c2
2 + Ic4,zz +m4! c42 +m3! 2
2
#
$ %
&
' (
!
m3! 2! c3 = m4!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!
(*) structural condition in mechanical design
Robotics 2 9
![Page 10: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/10.jpg)
Potential energy and gravity terms from the y-components of vectors pci
!
U1 = m1g0! c1s1
!
U2 = m2g0! c2s2
!
U3 = m3g0 ! 2s2 + ! c3s1( )
!
U4 = m4g0 !1s1 " ! c4s2( )
Ui
!
U = Uii=1
4
"
!
g(q) ="U"q
#
$ %
&
' (
T
=g0 m1! c1 + m3! c3 + m4!1( )c1
g0 m2! c2 + m3! 2 )m4! c4( )c2
#
$ %
&
' ( =
g1(q1)g2(q2)#
$ %
&
' (
!
b11˙ ̇ q 1 + g1(q1) = u1
b22˙ ̇ q 2 + g2(q2) = u2
in addition, when (*) holds
gravity components are always “decoupled”
ui (non-conservative) torque
producing work on qi
further structural conditions in the mechanical design lead to g(q) ≡ 0!!
Robotics 2 10
![Page 11: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/11.jpg)
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:
! 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)
!
uV,i = "FV,i ˙ q i
!
uS,i = "FS,isgn(˙ q i)
Robotics 2 11
![Page 12: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/12.jpg)
Placement of motors along the chain
RF0 RF1
RFN-1
link 0 (base)
link 1
joint1
link N - 1
link 2 link N joint 2
RFw (world frame)
joint N
RFN
motor 1
motor 2
motor N
!m2 .
!m1 .
!mN .
!mi = nri !i
. .
"i = nri "mi
Robotics 2 12
![Page 13: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/13.jpg)
Resulting dynamic model ! simplifying assumption: in the rotational part of kinetic
energy, only the “spinning” rotor velocity is considered
! including all added terms, the robot dynamics becomes
!
B(q) + Bm( )˙ ̇ q + c(q, ˙ q ) + g(q) + FV˙ q + FS sgn(˙ q ) = "
constant does NOT contribute to c
Fv > 0, FS > 0 diagonal
motor torques (after
reduction gears)
moved to the left ...
Robotics 2 13
!
Tmi =12
Imi˙ " mi
2 =12
Iminri2 ˙ q i
2 =12
Bmi˙ q i
2
!
Tm = Tmii=1
N
" =12
˙ q T Bm˙ q
diagonal, >0
! scaling by the reduction gears, looking from the motor side
motor torques (before
reduction gears)
!
Im + diagbii(q)nri
2
" # $
% & '
(
) *
+
, - ̇ ̇ . m + diag
1nri
" # $
% & '
b j(q)˙ ̇ q jj=1,..,n/ + f(q, ˙ q )
(
) * *
+
, - - = 0m
diagonal except the terms bjj
![Page 14: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/14.jpg)
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
![Page 15: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/15.jpg)
Robots with joint elasticity
Dexter with cable transmissions
DLR LWR-III with harmonic drives
Stanford DECMMA with micro-macro actuation
Quanser Flexible Joint (1-dof linear, educational)
motor load/link elastic spring
(stiffness K)
Robotics 2 15
video
![Page 16: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/16.jpg)
Dynamic model of robots with elastic joints
! introduce 2N generalized coordinates ! q = N link positions ! ! = N motor positions (after reduction, !i = !mi/nri)
! add motor kinetic energy Tm to that of the links
! add elastic potential energy Ue to that due to gravity Ug(q)
! K = matrix of joint stiffness (diagonal, >0)
! apply Euler-Lagrange equations w.r.t. (q, !)
!
B(q) ˙ ̇ q + c(q, ˙ q ) +g(q) +K(q"#) = 0
Bm˙ ̇ # + K(# "q) = $
2N 2nd-order differential equations
!
Tmi =12
Imi˙ " mi
2 =12
Iminri2 ˙ " i
2 =12
Bmi˙ " i2
!
Tq =12
˙ q T B(q) ˙ q
!
Tm = Tmii=1
N
" =12
˙ # T Bm˙ #
diagonal, >0
!
Uei =12
Ki qi" #mi /nri( )( )2=
12
Ki qi"# i( )2
!
Ue = Ueii=1
N
" =12
q-#( )T K q-#( )
no external torques performing work on q
Robotics 2 16
![Page 17: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/17.jpg)
Dynamic parameters of robots - 1
Robotics 2 17
! consider a generic link of a fully rigid robot
joint i
link i
CoM Ci
xi
yi
zi
kinematic frame i (DH or modified DH)
Center of Mass (CoM) frame i
base frame 0
mig0
rCi
ICi
! however, the robot dynamics depends in a nonlinear way on some of these parameters (e.g., through the combination Ici,zz+mirxi
2)
! each link is characterized by 10 dynamic parameters
mi rCi
!
=rxiryi
rzi
"
#
$ $
%
&
' '
ICi
!
=Ici,xx Ici,xy Ici,xz
Ici,yy Ici,yz
symm Ici,zz
"
#
$ $
%
&
' '
ri
Oi
![Page 18: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/18.jpg)
Dynamic parameters of robots - 2
Robotics 2 18
! 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)
! kinetic energy of link i
!
Ti = 12 mi vci
Tvci +12" i
TIci " i = 12 mi vi #S(rci)" i( )T
v i #S(rci)" i( ) + 12" i
TIci " i
= 12 mi vi
Tv i +12" i
T Ici +miST(rci)S(rci)( ) " i # vi
TS(mirci)" i
!
vci = vi +" i # rci = vi + S(" i)rci = vi $S(rci)" i
! fundamental kinematic relation
!
= Ici +mi rci2E - rci
T rci( ) = Ii =Ii,xx Ii,xy Ii,xz
Ii,yy Ii,yz
symm Ii,zz
"
#
$ $
%
&
' '
Steiner theorem 3!3 identity matrix
![Page 19: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/19.jpg)
Dynamic parameters of robots - 3
Robotics 2 19
! gravitational potential energy of link i
!
Ui = "mig0Tr0,ci = "mig0
T ri +rci( ) = "mig0Tri " g0
T#mirci
! 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$
mass of link i
(0-th order moment)
mass ! CoM position of link i
(1-st order moment)
inertia of link i
(2-nd order moment)
!
" i =mi
miirci
vect iIi{ }
#
$
% % %
&
'
( ( (
= mi miirci,x mi
irci,y miirci,z
iIi,xxiIi,xy
iIi,xziIi,yy
iIi,yziIi,zz( )
T
!
Ti = 12 mi
iv iT iv i +mi
irciT S(iv i)
i" i +12
i" iT iIi
i" i
! by expressing vectors and matrices in frame i, both Ti and Ui are linear in the set of 10 (constant) standard parameters "i
!
Ui = "mig0Tri"
ig0T #mi
irci
![Page 20: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/20.jpg)
Dynamic parameters of robots - 4
Robotics 2 20
! 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
!
" T = "1T "2
T ! " NT( )
!
B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = Y"(q, ˙ q ,˙ ̇ q ) " = u
! 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
!
Y"(q, ˙ q ,˙ ̇ q ) =
Y1,1 Y1,2 … Y1,N
0 Y2,2 … Y2,N
! " !0 0 YN,N
#
$
% % %
&
'
( ( (
with row vectors Yi,j of size 1!10
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}
![Page 21: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/21.jpg)
Dynamic parameters of robots - 5
Robotics 2 21
! 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
! 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)
!
Y"(q, ˙ q ,˙ ̇ q ) " = Yindep Ydep( ) " indep
"dep
#
$ %
&
' ( = Yindep YindepT( ) " indep
"dep
#
$ %
&
' (
= Yindep " indep + T"dep( ) = Y(q, ˙ q ,˙ ̇ q ) a
![Page 22: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/22.jpg)
Linear parameterization of robot dynamics
it is always possible to rewrite the dynamic model in the form
!
B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = Y(q, ˙ q ,˙ ̇ q ) a = u
N ! p p ! 1
a = vector of dynamic coefficients
regression matrix
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
e.g., the heuristic grouping (found by inspection) on a 2R planar robot
!
a1 = Ic1,zz +m1d12 + Ic2,zz +m2d2
2 +m2!12
!
a2 = m2!1d2
!
a3 = Ic2,zz +m2d22
!
a4 = g0 m1d1 + m2!1( )
!
˙ ̇ q 1 c2 2˙ ̇ q 1 + ˙ ̇ q 2( ) " s2˙ q 2
2 + 2˙ q 1˙ q 2( ) ˙ ̇ q 2 c1 c12
0 c2˙ ̇ q 1 + s2
˙ q 12 ˙ ̇ q 1+ ˙ ̇ q 2 0 c12
#
$ %
&
' (
a1
a2
a3
a4
a5
#
$
% % % %
&
'
( ( ( (
=u1
u2
#
$ %
&
' (
!
a5 = g0m2d2
![Page 23: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/23.jpg)
Linear parametrization of a 2R planar robot (N=2)
Robotics 2 23
! being the kinematics known (i.e., and g0), the number of dynamic coefficients can be reduced since we can merge the two coefficients
! therefore, after regrouping, p = 4 dynamic coefficients are sufficient
! 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
!
a2 = m2!1d2
!
a5 = g0 m2d2
!
!1
⇒
!
a2 = m2d2
!
a1 = Ic1,zz +m1d12 + Ic2,zz +m2d2
2 +m2!12
!
a2 = m2d2
!
a3 = Ic2,zz +m2d22
!
a4 = m1d1 +m2!1
!
˙ ̇ q 1 !1c2 2˙ ̇ q 1 + ˙ ̇ q 2( ) " !1s2˙ q 2
2 + 2˙ q 1˙ q 2( ) + g0c12˙ ̇ q 2 g0c1
0 !1c2˙ ̇ q 1 + !1s2
˙ q 12 + g0c12
˙ ̇ q 1+ ˙ ̇ q 2 0
#
$ %
&
' (
a1
a2
a3
a4
#
$
% % %
&
'
( ( (
= Ya = u =u1
u2
#
$ %
&
' (
(factoring out and g0)
!
!1
![Page 24: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/24.jpg)
Linear parametrization of a 2R planar robot (N=2)
Robotics 2 24
! 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)
! in the 2!5 matrix Y" , the 3rd column (associated to m2) is ! after regrouping/reordering, again p = 4 dynamic coefficients are sufficient
!
m1d1 I1,zz = Ic1,zz +m1d12 (link 1) m2 m2d2 I2,zz = Ic2,zz +m2d2
2 (link 2)
! 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
!
Y"3 = Y"1!1 + Y"2!12
!
a2 = I1,zz +m2!12 = Ic1,zz +m1d1
2( ) +m2!12
!
a3 = m2d2
!
a4 = I2,zz = Ic2,zz +m2d22
!
a1 = m1d1 +m2!1
!
g0c1˙ ̇ q 1 !1c2 2˙ ̇ q 1 + ˙ ̇ q 2( ) " !1s2
˙ q 22 + 2˙ q 1˙ q 2( ) + g0c12
˙ ̇ q 1+ ˙ ̇ q 20 0 !1c2
˙ ̇ q 1 + !1s2˙ q 1
2 + g0c12˙ ̇ q 1+ ˙ ̇ q 2
#
$ %
&
' (
a1
a2
a3
a4
#
$
% % %
&
'
( ( (
= Ya = u =u1
u2
#
$ %
&
' (
![Page 25: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/25.jpg)
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
![Page 26: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/26.jpg)
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(tk), ˙ q (tk),˙ ̇ q (tk)) a = u(tk) k = 1,…,nc
.
. .
Robotics 2 26
![Page 27: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/27.jpg)
Least Squares (LS) identification
! set up the system of linear equations
! 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
! one can also use a weighted pseudoinverse, to take into account different levels of noise in the collected measures
!
Y(q(t1), ˙ q (t1),˙ ̇ q (t1))!
Y(q(tnc), ˙ q (tnc
),˙ ̇ q (tnc))
"
#
$ $ $
%
&
' ' '
a =
u(t1)
u(tnc)
"
#
$ $ $
%
&
' ' '
!
Y a = u
_ _
!
a = Y # u = Y TY ( )-1Y T u
nc ! N
Robotics 2 27
![Page 28: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/28.jpg)
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
!
Y1(q(t1), ˙ q (t1),˙ ̇ q (t1))!
Y1(q(tnc), ˙ q (tnc
),˙ ̇ q (tnc))
Y2(q(t1), ˙ q (t1),˙ ̇ q (t1))!
YN(q(t1), ˙ q (t1),˙ ̇ q (t1))!
YN(q(tnc), ˙ q (tnc
),˙ ̇ q (tnc))
"
#
$ $ $ $ $ $ $ $
%
&
' ' ' ' ' ' ' '
a =
u1(t1)!
u1(tnc)
u2(t1)!
uN(t1)!
uN(tnc)
"
#
$ $ $ $ $ $ $ $
%
&
' ' ' ' ' ' ' '
!
Y a = u N!
Robotics 2 28
! 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
_
nc
nc
" numerical check of full column rank is more robust ⇔ rank = p (# of col’s)
!
Y =
nc
nc
![Page 29: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/29.jpg)
Summary on dynamic identification
Robotics 2 29
J. Swevers, W. Verdonck, and J. De Schutter: “Dynamic model identification for industrial robots”
IEEE Control Systems Mag., Oct 2007
KUKA IR 361 robot and optimal
excitation trajectory
results after identification (first three joints only)
![Page 30: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/30.jpg)
Dynamic identification of KUKA LWR4
Robotics 2 30
C. Gaz, F. Flacco, A. De Luca: “Identifying the dynamic model used by the KUKA LWR:
A reverse engineering approach” IEEE ICRA 2014, Hong Kong, June 2014
validation after identification (for all 7 joints): on new desired trajectories, compare
torques computed with the identified model and torques measured by joint torque sensors
data acquisition for identification dynamic coefficients: 30 inertial, 12 for gravity
video
![Page 31: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/31.jpg)
Dynamic identification of KUKA LWR4
Robotics 2 31
J. Hollerbach, W. Khalil, M. Gautier: “Ch. 6: Model Identification”, Springer Handbook of Robotics (2nd Ed), 2016 free access to multimedia extension: http://handbookofrobotics.org
using more dynamic robot motions for model identification
video
![Page 32: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/32.jpg)
Adding a payload to the robot
Robotics 2 32
! in several industrial applications, changes in the robot payload are often needed ! using different tools for various technological operations
such as polishing, welding, grinding, ... ! pick-and-place tasks of objects having unknown mass
! what is the rule of change for dynamic parameters when there is an additional payload? ! do we obtain again a linearly parameterized problem? ! does this property rely on some specific choice of
reference frames (e.g., conventional or modified D-H)?
![Page 33: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/33.jpg)
Rule of change in dynamic parameters
Robotics 2 33
! only the dynamic parameters of the link where a load is added will change (typically, added to the last one –link n– as payload)
! last link dynamic parameters: mn (mass), cn = (cnx cny cnz)T (center of
mass), In (inertia tensor w.r.t. frame n)
! payload dynamic parameters: mL (mass), cL = (cLx cLy cLz)T (center of
mass), IL (inertia tensor w.r.t. frame n)
! mass
! center of mass
! inertia tensor
(weighted average) where i = x, y, z
valid only if tensors are expressed w.r.t. the same reference frame (i.e., frame n)!
" linear parametrization is preserved with any kinematic convention (the parameters of the last link will always appear in the form shown above)
![Page 34: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/34.jpg)
Example: 2R planar robot with payload
Robotics 2 34
gravity acceleration
robot dynamics robot dynamics
g0
Note1: the positions of the center of mass of the two links and payload may also be asymmetric Note2: link inertias & centers of mass are expressed in the DH kinematic frame attached to the link;
thus, e.g., I2zz + m2a22 is the inertia of the second link w.r.t. z1 (parallel axis theorem)
![Page 35: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/35.jpg)
Validation on the KUKA LWR4 robot
Robotics 2 35
video
C. Gaz, A. De Luca: “Payload estimation based on identified coefficients of robot dynamics – with an application to collision detection” IEEE IROS 2017, Vancouver, September 2017
see block of slides!
![Page 36: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/36.jpg)
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
..
! 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
!
"d = B(qd) + Bm( )˙ ̇ q d + c(qd, ˙ q d) + g(qd) + FV˙ q d + FS sgn(˙ q d)
Robotics 2 36
![Page 37: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/37.jpg)
Dynamic scaling of trajectories uniform time scaling of motion
Robotics 2 37
! 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
! uniform scaling of the trajectory occurs when r(t) = k t
Q: what is the new input torque needed to execute the scaled trajectory? (suppose dissipative terms can be neglected)
!
qd(t) = qs(r(t))
!
˙ q d(t) =dqd
dt=
dqs
drdrdt
= qs' (r) ˙ r
!
˙ ̇ q d(t) =d˙ q ddt
=dqs
'
drdrdt
"
# $
%
& ' ̇ r +qs
' (r)˙ ̇ r = qs'' (r) ˙ r 2 + qs
' (r)˙ ̇ r
same path executed (at different instants of time)
!
˙ q d(t) = k qs' (kt)
!
˙ ̇ q d(t) =k2qs'' (kt)
![Page 38: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/38.jpg)
Dynamic scaling of trajectories inverse dynamics under uniform time scaling
Robotics 2 38
! the new torque could be recomputed through the inverse dynamics, for every r = k t ∈[0,T’]=[0,kT] along the scaled trajectory, as
! however, being the dynamic model linear in the acceleration and quadratic in the velocity, it is
! 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
!
"d(t) = B(qd)˙ ̇ q d + c(qd, ˙ q d) + g(qd) = B(qs)k2qs
'' + c(qs,kqs' ) + g(qs)
= k2 B(qs)qs'' + c(qs,qs
' )( ) + g(qs) = k2 "s(kt) #g(qs)( ) + g(qs)
!
"s(kt) = B(qs)qs'' + c(qs,qs
' ) + g(qs)
!
"s(kt) =1k2 "d(t) #g(qd(t))( ) + g(qd(t))
k>1: slow down ⇒ reduce torque
k<1: speed up ⇒ increase torque
gravity term (only position-dependent): does NOT scale!
![Page 39: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/39.jpg)
Dynamic scaling of trajectories numerical example
Robotics 2 39
! 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
total torque
at equilibrium = zero gravity
torque
gravity torque component
torque only due to non-zero initial
acceleration
for both joints
![Page 40: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/40.jpg)
Dynamic scaling of trajectories numerical example
Robotics 2 40
! scaling with k=2 (slower) → T’ = 1 s original total
torque
scaled total
torque
gravity torque component
does not scale
gravity torque to sustain the link
at steady state
T=0.5 s
T’=1 s
*
*
!
"d(0.1) #g(qd(0.1)) = 20 Nm
!
"s(2# 0.1) $g(qs(2# 0.1)) =2022 = 5 Nm
T’=1 s T=0.5 s
!
k = 2
* *
0 Nm
!
14
![Page 41: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/41.jpg)
!
˜ x =q
B(q)˙ q "
# $
%
& '
State equations Direct dynamics
!
B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = u
state equations
N differential 2nd-order equations
defining the vector of state variables as
!
x =x1
x2
"
# $
%
& ' =
q˙ q "
# $ %
& ' ( IR2N
!
˙ x =˙ x 1˙ x 2
"
# $
%
& ' =
x2
(B(1(x1) c(x1,x2) + g(x1)[ ]"
# $
%
& ' +
0B(1(x1)"
# $
%
& ' u
= f(x) + G(x)u
2N!1 2N!N
another choice... x = ... (do it as exercise) ~ .
Lagrangian dynamic model
generalized momentum
2N differential 1st-order equations
Robotics 2 41
![Page 42: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/42.jpg)
Dynamic simulation
u
!
˙ q 1(0)
!
q1(0)
q1
!
˙ ̇ q 1
!
˙ q 1
g(q)
!
˙ q 2 (0)
!
q2 (0)
q2
!
˙ ̇ q 2
!
˙ q 2
!
q, ˙ q
!
q
!
c(q, ˙ q )
B-1(q)
!
"
!
"
!
"
!
"
here, a generic 2-dof robot
!
q
+
_
_
" 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)
input torque command (open-loop
or in feedback)
including “inv(B)”
Simulink block
scheme
Robotics 2 42
![Page 43: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/43.jpg)
… an incorrect realization
u1
c1+ g1
!
1b11
!
"
!
˙ q 1(0)
!
q1(0)
q1
!
˙ ̇ q 1
!
˙ q 1
u2
c2 + g2
!
1b22
!
˙ q 2 (0)
!
q2 (0)
q2
!
˙ ̇ q 2
!
˙ q 2
b12
b12 algebraic loop!
causality principle
is violated!!
_
_
_
_
!
"
!
"
!
"
inversion of the robot inertia matrix in toto is needed (and not only of its elements on the diagonal...)
Robotics 2 43
![Page 44: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/44.jpg)
Approximate linearization
!
B(qd)˙ ̇ q d + c(qd, ˙ q d) + g(qd) = ud
! 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
!
g(qe) = ueequilibrium state (q,q) = (qe,0) [ q=0 ] . ..
equilibrium trajectory (q,q) = (qd (t),qd(t)) [ q=qd(t) ] . .
Robotics 2 44
.. ..
![Page 45: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/45.jpg)
Linearization at an equilibrium state
! variations around an equilibrium state
! keeping into account the quadratic dependence of c terms on velocity (thus, neglected around the zero velocity)
! in state-space format
!
q = qe + "q ˙ q = ˙ q e + "˙ q = "˙ q ˙ ̇ q = ˙ ̇ q e + "˙ ̇ q = "˙ ̇ q u = ue + "u
!
B qe( )"˙ ̇ q + g(qe) +# g# q
q=qe
"q + o "q , "˙ q ( ) = ue + "u
!
G(qe)infinitesimal terms
of second or higher order
!
"˙ x =0 I
-B-1 qe( )G(qe) 0#
$ %
&
' ( "x +
0B-1 qe( )#
$ %
&
' ( "u = A "x + B "u
!
"x ="q"˙ q #
$ %
&
' (
Robotics 2 45
![Page 46: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/46.jpg)
Linearization along a trajectory
! variations around an equilibrium trajectory
! developing terms in the dynamic model ...
!
q = qd + "q ˙ q = ˙ q d + "˙ q ˙ ̇ q = ˙ ̇ q d + "˙ ̇ q u = ud + "u
!
B qd + "q( )(˙ ̇ q d + "˙ ̇ q ) + c(qd + "q, ˙ q d + "˙ q ) + g(qd + "q) = ud + "u
!
B(qd + "q) #B(qd) +$ bi
$ qi=1
N
%q=qd
eiT"q
!
c(qd + "q, ˙ q d + "˙ q ) # c(qd, ˙ q d) +$ c$ q q=qd˙ q = ˙ q d
"q +$ c$ ˙ q q=qd˙ q = ˙ q d
"˙ q
!
g(qd + "q) # g(qd) + G(qd)"q
!
C1(qd, ˙ q d)
!
C2(qd, ˙ q d)
i-th row of the identity matrix
Robotics 2 46
![Page 47: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/47.jpg)
Linearization along a trajectory (cont)
! after simplifications …
with
! in state-space format
!
B qd( )"˙ ̇ q + C2(qd, ˙ q d)"˙ q + D(qd, ˙ q d,˙ ̇ q d)"q = "u
!
"˙ x =0 I
-B-1 qd( )D(qd, ˙ q d,˙ ̇ q d) -B-1 qd( )C2(qd, ˙ q d)#
$ %
&
' ( "x +
0B-1 qd( )#
$ %
&
' ( "u
= A t( )"x + B t( )"u
!
D(qd, ˙ q d,˙ ̇ q d) = G(qd) + C1(qd, ˙ q d) +" bi
" qi=1
N
#q=qd
˙ ̇ q d eiT
a linear, but time-varying system!!
Robotics 2 47
![Page 48: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/48.jpg)
Coordinate transformation
!
B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = B(q)˙ ̇ q + n(q, ˙ q ) = uq
!
q" IRN
if we wish/need to use a new set of generalized coordinates p
!
p" IRN
!
p = f(q)
!
q = f "1(p)
!
˙ ̇ p = J(q)˙ ̇ q + ˙ J (q) ˙ q
!
˙ ̇ q = J"1(q) ˙ ̇ p " ˙ J (q)J"1(q)˙ p [ ]
1
1
!
B(q)J"1(q)˙ ̇ p "B(q)J"1(q)˙ J (q)J"1(q) ˙ p + n(q, ˙ q ) = JT (q)up
!
˙ p = "f(q)"q
˙ q = J(q) ˙ q
!
˙ q = J"1(q) ˙ p
!
uq = JT (q)up,
!
J"T (q) # pre-multiplying the whole equation... Robotics 2 48
![Page 49: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/49.jpg)
Robot dynamic model after coordinate transformation
!
J"T (q)B(q)J"1(q)˙ ̇ p + J"T (q) n(q, ˙ q ) "B(q)J"1(q)˙ J (q)J"1(q)˙ p ( ) = up
!
Bp(p)˙ ̇ p + cp(p, ˙ p ) + gp(p) = up
!
Bp = J"TBJ"1
!
cp = J"T c "BJ"1˙ J J"1 ˙ p ( ) = J"Tc "Bp˙ J J"1 ˙ p
!
gp = J"Tg
!
cp p, ˙ p ( ) = Sp p, ˙ p ( )˙ p
!
˙ B p "2Sp skew - symmetric
!
q"p
!
q, ˙ q "p, ˙ p for actual computation, these inner substitutions
are not necessary
symmetric, positive definite
(out of singularities)
quadratic dependence on p
.
when p = E-E pose, this is the robot dynamic model in Cartesian coordinates
non-conservative generalized forces
performing work on p
Robotics 2 49
Q: What if the robot is redundant with respect to the Cartesian task?
![Page 50: Robotics 2 - Università di Romadeluca/rob2_en/05_LagrangianDynamics_3.pdf · Robotics 2 18 ! kinetic energy and gravity potential energy can both be rewritten so that a new set of](https://reader031.vdocuments.us/reader031/viewer/2022030614/5adf342e7f8b9ac0428bdb98/html5/thumbnails/50.jpg)
Optimal point-to-point robot motion considering the dynamic model
Robotics 2 50
" 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
Tmin= 1.32 s, E = 306 T = 1.60 s, Emin = 6.14
video video