Divergent Components of Motion . Stphane Caron October 29, 2019 - PowerPoint PPT Presentation

Divergent Components of Motion . Stéphane Caron October 29, 2019 JRL Seminar, CNRS-AIST Joint Robotics Laboratory, Tsukuba

warmup : some control theory .

1 ˙ x = λ x

first-order scalar case Trajectories . 2 Autonomous system 2.5 ˙ 2.0 x = λ x 1.5 1.0 x ( t ) = e λ t x (0) 0.5 • We want to control x → 0 0.0 • Pole of the system : λ 0.5 • Stability : x − t →∞ 0 iff λ < 0 → 1.0 0.0 0.5 1.0 1.5 2.0 To control x toward a time-varying reference x d ( t ) , apply the same to the error e = x − x d

3 ˙ x = Ax

first-order vectorial case Trajectories . 4 Autonomous system 3.0 ˙ 2.5 x = Ax 2.0 x ( t ) = e At x (0) 1.5 1.0 • Poles : eig ( A ) = { λ 1 , . . . , λ n } 0.5 • Stability : all poles λ i < 0 0.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0

5 ˙ x = Ax + Bu

first-order linear system Linear feedback . • Pole placement 6 Linear system 3.0 ˙ x = Ax + Bu 2.5 2.0 u = − Kx 1.5 • Closed loop : ˙ x = ( A − BK ) x 1.0 • Poles : eig ( A − BK ) 0.5 0.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0

application to legged robots .

linear inverted pendulum (lip) . IEEE/RSJ International Conference on Intelligent Robots and Systems . 2001. 1. Shuuji Kajita, Fumio Kanehiro, Kenji Kaneko, Kazuhito Yokoi et Hirohisa Hirukawa. « The 3D 1 same as system dynamics. Linear system : error dynamics is the 7 Equation of motion Real robot and reference trajectories : c = ω 2 ( c − z ) ¨ c = ω 2 ( c − z ) ¨ c d = ω 2 ( c d − z d ) ¨ Tracking error ∆ y = y − y d : c = ω 2 (∆ c − ∆ z ) ∆¨ Linear Inverted Pendulum Mode : A simple modeling for a biped walking pattern generation ». In :

linear inverted pendulum tracking c Kaneko, Fumio Kanehiro et Kazuhito Yokoi. « Biped walking stabilization based on linear inverted 2. Shuuji Kajita, Mitsuharu Morisawa, Kanako Miura, Shin'ichiro Nakaoka, Kensuke Harada, Kenji 2 c Linear feedback : u . c 8 Equation of motion ¨ c = ω 2 ( c − z ) With x = [ c ˙ c ] and u = z : [ ] [ ] [ ] ˙ 0 1 0 ˙ x = = x + ω 2 − ω 2 ¨ 0 ∆ u = k p ∆ c + k v ∆˙ How to choose the gains k p and k v ? pendulum tracking ». In : IEEE/RSJ International Conference on Intelligent Robots and Systems . 2010.

diagonalization (1/2) The diagonalization of A would give : x are decoupled . u . 9 What if we can diagonalize A ? Linear system ˙ x = Ax + Bu u = − Kx [ ] [ ] 0 1 λ 1 0 P − 1 A = = P ω 2 0 0 λ 2 x = P − 1 x so that : We could then change variable � [ ] 0 λ 1 ˙ x = P − 1 ˙ x = P − 1 AP � x + P − 1 Bu = x + � � � B � 0 λ 2 The coordinates of �

diagonalization (2/2) We then change variable : So that our system becomes : c . c Good news ! We can diagonalize A : Linear system 10 ˙ x = Ax + Bu u = − Kx [ ] [ ] [ ] − 1 A = 1 1 1 − ω 0 1 ω + 1 2 − ω ω 0 + ω 1 ω [ ] [ ] c − ˙ ζ = P − 1 x = � ω x = c + ˙ ξ ω [ ˙ ] [ ] [ ] [ ] { ˙ ζ − ω 0 ζ + ω ζ = ω ( u − ζ ) ˙ � x = = + u ⇐ ⇒ ˙ ˙ ξ 0 + ω ξ − ω ξ = ω ( ξ − u )

components of motion • No feedback required . Divergent component of motion 11 Convergent component of motion ˙ ˙ ζ = ω ( u − ζ ) ξ = ω ( ξ − u ) • Feedback u = k ξ ξ required ! • Stable as long as k ξ > 1 • Stable : ζ → 0 as long as u → 0 Diag. DCM – + We only apply u = k ξ ξ since ξ → 0 ⇒ u → 0 ⇒ ζ → 0 .

back to our initial question c mation . 2009. 3. Tomomichi Sugihara. « Standing stabilizability and stepping maneuver in planar bipedalism 3 Best possible linear feedback [Sug09]. We now know how to select our gains : u . c 12 Equation of motion c = ω 2 ( c − z ) ¨ With x = [ c ˙ c ] and u = z : [ ] [ ] [ ] ˙ 0 1 0 ˙ x = = x + ¨ ω 2 − ω 2 0 u = k p c + k v ˙ c = k ξ ξ k v = k p / ω k p > 1 based on the best COM-ZMP regulator ». In : IEEE International Conference on Robotics and Auto-

13 ˙ x = A ( t ) x

exponential dichotomy . 3609707. Berlin : Springer, 1978. 97 p. 4. William A. Coppel. Dichotomies in stability theory . Lecture notes in mathematics 629. OCLC : 4 Pendular models for locomotion fall into this category of systems. 14 Exponential dichotomy (Coppel, 1967) The idea behind this approach has been explored in control theory. The system ˙ x = A ( t ) x has an exponential dichotomy iff there exists a projection Π and constants K , L , α, β > 0 such that its solutions satisfy : | Π x ( t ) | ≤ Ke − α ( t − t 0 ) | Π x ( t 0 ) | t 0 ≤ t | ( I − Π) x ( t ) | ≥ Le + β ( t − t 0 ) | ( I − Π) x ( t 0 ) | t 0 ≤ t • Π projects on CCMs that converge in forward time • ( I − Π) projects on DCMs that diverge in forward time

15 ˙ x = f ( x )

height variation strategy . 5 5. Twan Koolen, Michael Posa et Russ Tedrake. « Balance control using center of mass height Humanoid Robots . 2016. 16 F ext F ext Rest position Ankle strategy Height variation strategy variation : Limitations imposed by unilateral contact ». In : IEEE-RAS International Conference on

variable-height inverted pendulum (vhip) c Conference on Humanoid Robots . IEEE, 2014, p. 266–272. 6. Michael A. Hopkins, Dennis W. Hong et Alexander Leonessa. « Humanoid locomotion on une- 6 . c 17 Equation of motion ¨ c = λ ( c − z ) + g Nonlinear : both z and λ are inputs [ ] [ ] [ ] ˙ 0 0 I 3 ˙ x = = x + ( λ z − g ) ¨ 0 − I 3 λ I 3 = A ( λ ) x + B ( λ z − g ) Planning : fix λ ( t ) to make system linear time-variant : ˙ x = A ( t ) x + B λ ( t ) z − Bg What about feedback on λ ? ven terrain using the time-varying Divergent Component of Motion ». In : IEEE-RAS International

a problem of motorcycle balance . on Decision and Control . Nassau, Bahamas : IEEE, 2004, 3944–3949 Vol.4. 7. J. Hauser, A. Saccon et R. Frezza. « Achievable motorcycle trajectories ». In : IEEE Conference 7 from [HSF04]. Figure 1 : Figure adapted A is diagonal... b with : 18 c Equation of motion c [ ] [ ] [ ] ˙ 0 0 I 3 ˙ x = = x + ( λ z − g ) ¨ λ I 3 0 − I 3 x = P − 1 x with : Let � [ ] [ ] 1 γ I 3 − I 3 ⇒ P − 1 = I 3 I 3 P = ⇐ γ + ω − ω I 3 + γ I 3 + I 3 ω I 3 Changing variable yields ˙ x = � Ax + � � [ ] γ + γ 2 − λ ) I 3 1 ( ˙ γ − γω − λ ) I 3 ( ˙ � A = ω − ω 2 + λ ) I 3 γ + ω ( ˙ ( ˙ ω + ωγ + λ ) I 3 We have a dichotomy if �

exponential dichotomy of the vhip c c c Then we have an exponential dichotomy : A diagonal ! Choose : . 19 c Equation of motion [ ] [ ] [ ] ˙ 0 0 I 3 ˙ x = = x + ( λ z − g ) ¨ λ I 3 0 − I 3 So let's make � γ = λ − γ 2 ˙ ω = ω 2 − λ ˙ ζ = c − ˙ ζ = λ ˙ γ ( z − ζ ) − g γ γ ξ = c + ˙ ξ = λ ˙ ω ( ξ − z ) + g ω ω

curious fact . We achieved the exponential dichotomy by choosing : 8 8. Stéphane Caron, Adrien Escande, Leonardo Lanari et Bastien Mallein. « Capturability-based 20 ω = ω 2 − λ ˙ This natural frequency ω behaves like a DCM : • λ is an input • ω diverges under constant input Pattern Generation for Walking with Variable Height ». In : IEEE Transactions on Robotics (juil. 2019).

interpretation . https://en.wikipedia.org/wiki/Duck_test 21

4d dcm for the vhip . 9. Stéphane Caron. « Biped Stabilization by Linear Feedback of the Variable-Height Inverted 9 Nonlinear system. g z 22 = Divergent component of motion [ ] [ ] [ ] [ ] [ ] x = 1 0 x − 1 0 + 1 ξ λ I 3 λ I 3 ⇒ ˙ x = ω 2 ω ω 0 ω 0 ω λ ω 0 Pendulum Model ». submitted. Sept. 2019.

4d dcm for the vhip z 9. Stéphane Caron. « Biped Stabilization by Linear Feedback of the Variable-Height Inverted Take its linearized error dynamics : Nonlinear system. g . 22 = Divergent component of motion [ ] [ ] [ ] [ ] [ ] ξ x = 1 λ I 3 0 x − 1 λ I 3 0 + 1 ⇒ ˙ x = ω 2 ω ω 0 ω 0 ω λ ω 0 [ ] [ ] [ ] ( ξ d − z d ) − ¨ c d / ω d ∆ z x = 1 λ d I 3 ∆ x − 1 λ d I 3 ∆˙ 2( ω d ) 2 ω d 0 ω d 0 ω d ∆ λ We are back to ∆˙ x = A ∆ x + B ∆ u . 9 Pendulum Model ». submitted. Sept. 2019.

least-squares pole placement . Least squares problem : Subject to : 23 Place poles of the closed-loop system : [ ] x ∗ = (1 − k ) 1 λ d I 3 0 ∆˙ ∆ x ( ω d ) 2 ω d 0 x ∗ ∥ 2 Minimize : ∥ ∆˙ x − ∆˙ • Linearized dynamics : ∆˙ x = A ∆ x + B ∆ u • ZMP support area : C ( z d + ∆ z ) ≤ d • Reaction force : λ min ≤ λ d + ∆ λ ≤ λ max • Kinematics : h min ≤ ξ d z + ∆ ξ z ≤ h max ⇒ Constrained optimal gains ∆ u = − K ∆ x

behavior . https://github.com/stephane-caron/pymanoid/blob/master/examples/vhip_stabilization.py 24

controller adjusts the dcm . One last observation : previously, the DCM was a measure : Now, the controller adjusts the DCM based on state and constraints : 25 Diag. DCM – + Pole placement Diag. constrained opt. – +

tests on HRP-4 .

overall system . 26 Commanded Desired Kinematic Kinematic Commanded Footstep Walking Whole-body Targets Targets Joint Angles Locations Inverse Pattern Admittance Kinematics Generation Control Desired Measured Wrench Wrench CoM Position Joint Angles DCM Desired DCM CoM CoM Velocity IMU Orientation Control Observer