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 .

˙ x = λx

1

first-order scalar case .

Autonomous system ˙ x = λx Trajectories x(t) = eλtx(0)

• We want to control x → 0
• Pole of the system : λ
• Stability : x −

→

t→∞ 0 iff λ < 0

0.0 0.5 1.0 1.5 2.0 1.0 0.5 0.0 0.5 1.0 1.5 2.0 2.5

To control x toward a time-varying reference xd(t), apply the same to the error e = x − xd

2

˙ x = Ax

3

first-order vectorial case .

Autonomous system ˙ x = Ax Trajectories x(t) = eAtx(0)

• Poles : eig(A) = {λ1, . . . , λn}
• Stability : all poles λi < 0

0.0 0.5 1.0 1.5 2.0 2.5 3.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0

4

˙ x = Ax + Bu

5

first-order linear system .

Linear system ˙ x = Ax + Bu Linear feedback u = −Kx

• Closed loop : ˙

x = (A − BK)x

• Poles : eig(A − BK)
• Pole placement

0.0 0.5 1.0 1.5 2.0 2.5 3.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0

6

application to legged robots .

linear inverted pendulum (lip) .

Equation of motion ¨ c = ω2(c − z) Real robot and reference trajectories : ¨ c = ω2(c − z) ¨ cd = ω2(cd − zd) Tracking error ∆y = y − yd : ∆¨ c = ω2(∆c − ∆z) Linear system : error dynamics is the same as system dynamics.

1

• 1. Shuuji Kajita, Fumio Kanehiro, Kenji Kaneko, Kazuhito Yokoi et Hirohisa Hirukawa. « The 3D

Linear Inverted Pendulum Mode : A simple modeling for a biped walking pattern generation ». In : IEEE/RSJ International Conference on Intelligent Robots and Systems. 2001.

7

linear inverted pendulum tracking .

Equation of motion ¨ c = ω2(c − z) With x = [c ˙ c] and u = z : ˙ x = [ ˙ c ¨ c ] = [ 1 ω2 ] x + [ −ω2 ] u Linear feedback : ∆u = kp∆c + kv∆˙ c How to choose the gains kp and kv ?

2

• 2. Shuuji Kajita, Mitsuharu Morisawa, Kanako Miura, Shin'ichiro Nakaoka, Kensuke Harada, Kenji

Kaneko, Fumio Kanehiro et Kazuhito Yokoi. « Biped walking stabilization based on linear inverted pendulum tracking ». In : IEEE/RSJ International Conference on Intelligent Robots and Systems. 2010.

8

diagonalization (1/2) .

What if we can diagonalize A ? Linear system ˙ x = Ax + Bu u = −Kx The diagonalization of A would give : A = [ 1 ω2 ] = P [ λ1 λ2 ] P−1 We could then change variable x = P−1x so that : ˙

• x = P−1˙

x = P−1AP x + P−1Bu = [ λ1 λ2 ]

• x +

B u The coordinates of x are decoupled.

9

diagonalization (2/2) .

Linear system ˙ x = Ax + Bu u = −Kx Good news ! We can diagonalize A : A = 1 2 [ 1 1 −ω ω ] [ −ω +ω ] [ 1 − 1

ω

1 + 1

ω

] We then change variable :

• x =

[ ζ ξ ] = P−1x = [ c − ˙

c ω

c + ˙

c ω

] So that our system becomes : ˙

• x =

[ ˙ ζ ˙ ξ ] = [ −ω +ω ] [ ζ ξ ] + [ +ω −ω ] u ⇐ ⇒ { ˙ ζ = ω(u − ζ) ˙ ξ = ω(ξ − u)

10

components of motion .

Convergent component of motion ˙ ζ = ω(u − ζ)

• No feedback required
• Stable : ζ → 0 as long as u → 0

Divergent component of motion ˙ ξ = ω(ξ − u)

• Feedback u = kξξ required !
• Stable as long as kξ > 1

Diag. DCM

– +

We only apply u = kξξ since ξ → 0 ⇒ u → 0 ⇒ ζ → 0.

11

back to our initial question .

Equation of motion ¨ c = ω2(c − z) With x = [c ˙ c] and u = z : ˙ x = [ ˙ c ¨ c ] = [ 1 ω2 ] x + [ −ω2 ] u We now know how to select our gains : u = kpc + kv˙ c = kξξ kv = kp/ω kp > 1 Best possible linear feedback [Sug09].

3

• 3. Tomomichi Sugihara. « Standing stabilizability and stepping maneuver in planar bipedalism

based on the best COM-ZMP regulator ». In : IEEE International Conference on Robotics and Auto-

• mation. 2009.

12

˙ x = A(t)x

13

exponential dichotomy .

The idea behind this approach has been explored in control theory. Exponential dichotomy (Coppel, 1967) 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−t0)|Πx(t0)| t0 ≤ t |(I − Π)x(t)| ≥ Le+β(t−t0)|(I − Π)x(t0)| t0 ≤ t

• Π projects on CCMs that converge in forward time
• (I − Π) projects on DCMs that diverge in forward time

Pendular models for locomotion fall into this category of systems.

4

• 4. William A. Coppel. Dichotomies in stability theory. Lecture notes in mathematics 629. OCLC :
• 3609707. Berlin : Springer, 1978. 97 p.

14

˙ x = f(x)

15

height variation strategy .

Rest position Ankle strategy Height variation strategy

Fext

Fext

5

• 5. Twan Koolen, Michael Posa et Russ Tedrake. « Balance control using center of mass height

variation : Limitations imposed by unilateral contact ». In : IEEE-RAS International Conference on Humanoid Robots. 2016.

16

variable-height inverted pendulum (vhip) .

Equation of motion ¨ c = λ(c − z) + g Nonlinear : both z and λ are inputs ˙ x = [ ˙ c ¨ c ] = [ I3 λI3 ] x + [ −I3 ] (λz − g) = 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 λ ?

6

• 6. Michael A. Hopkins, Dennis W. Hong et Alexander Leonessa. « Humanoid locomotion on une-

ven terrain using the time-varying Divergent Component of Motion ». In : IEEE-RAS International Conference on Humanoid Robots. IEEE, 2014, p. 266–272.

17

a problem of motorcycle balance .

Equation of motion ˙ x = [ ˙ c ¨ c ] = [ I3 λI3 ] x + [ −I3 ] (λz − g) Let x = P−1x with : P = 1 γ + ω [ I3 I3 −ωI3 +γI3 ] ⇐ ⇒ P−1 = [ γI3 −I3 ωI3 +I3 ] Changing variable yields ˙

• x =

Ax + b with :

• A =

1 γ + ω [ ( ˙ γ − γω − λ)I3 ( ˙ γ + γ2 − λ)I3 ( ˙ ω − ω2 + λ)I3 ( ˙ ω + ωγ + λ)I3 ] We have a dichotomy if A is diagonal...

Figure 1 : Figure adapted from [HSF04].

7

• 7. J. Hauser, A. Saccon et R. Frezza. « Achievable motorcycle trajectories ». In : IEEE Conference
• n Decision and Control. Nassau, Bahamas : IEEE, 2004, 3944–3949 Vol.4.

18

exponential dichotomy of the vhip .

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

19

curious fact .

We achieved the exponential dichotomy by choosing : ˙ ω = ω2 − λ This natural frequency ω behaves like a DCM :

• λ is an input
• ω diverges under constant input

8

• 8. Stéphane Caron, Adrien Escande, Leonardo Lanari et Bastien Mallein. « Capturability-based

Pattern Generation for Walking with Variable Height ». In : IEEE Transactions on Robotics (juil. 2019).

20

interpretation .

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

21

4d dcm for the vhip .

Divergent component of motion x = [ ξ ω ] = ⇒ ˙ x = 1 ω [ λI3 ω2 ] x − 1 ω [ λI3 ω ] [ z λ ] + 1 ω [ g ] Nonlinear system.

9

• 9. Stéphane Caron. « Biped Stabilization by Linear Feedback of the Variable-Height Inverted

Pendulum Model ». submitted. Sept. 2019.

22

4d dcm for the vhip .

Divergent component of motion x = [ ξ ω ] = ⇒ ˙ x = 1 ω [ λI3 ω2 ] x − 1 ω [ λI3 ω ] [ z λ ] + 1 ω [ g ] Nonlinear system. Take its linearized error dynamics : ∆˙ x = 1 ωd [ λdI3 −¨ cd/ωd 2(ωd)2 ] ∆x − 1 ωd [ λdI3 (ξd − zd) ωd ] [ ∆z ∆λ ] We are back to ∆˙ x = A∆x + B∆u. 9

• 9. Stéphane Caron. « Biped Stabilization by Linear Feedback of the Variable-Height Inverted

Pendulum Model ». submitted. Sept. 2019.

22

least-squares pole placement .

Place poles of the closed-loop system : ∆˙ x∗ = (1 − k) 1 ωd [ λdI3 (ωd)2 ] ∆x Minimize : ∥∆˙ x − ∆˙ x∗∥2 Subject to :

• Linearized dynamics : ∆˙

x = A∆x + B∆u

• ZMP support area : C(zd + ∆z) ≤ d
• Reaction force : λmin ≤ λd + ∆λ ≤ λmax
• Kinematics : hmin ≤ ξd

z + ∆ξz ≤ hmax

Least squares problem : ⇒ Constrained optimal gains ∆u = −K∆x

23

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 :

Diag. DCM

– +

Now, the controller adjusts the DCM based on state and constraints :

Diag. Pole placement constrained opt.

– +

25

tests on HRP-4 .

• verall system

.

Footstep Locations

Walking Pattern Generation Whole-body Admittance Control

Commanded Joint Angles Joint Angles

Inverse Kinematics DCM Control CoM Observer

CoM Position IMU Orientation Desired DCM Desired Kinematic Targets Desired Wrench Commanded Kinematic Targets Measured Wrench CoM Velocity

26

force control .

LIP tracking VHIP tracking

https://github.com/stephane-caron/vhip_walking_controller

27

balance control .

https://github.com/stephane-caron/vhip_walking_controller

28

wrap-up .

what have we seen ? .

• Ground zero : ˙

x = λx

• Linear time invariant : ˙

x = Ax

• Linear feedback : ˙

x = Ax + Bu

• Diagonalize A : apply feedback to divergent components
• General case : exponential dichotomy
• If it looks like a DCM, it is a DCM !
• Linearized error dynamics
• Least-squares pole placement
• Ask the robot !

29

thanks ! .

Thank you for your attention !

Thanks to Dr Kajita and Mr Vasalya for correcting my mistakes in previous versions of these slides, and to Dr Escande for suggesting improvements. Discussion : https://scaron.info/talks/jrl-2019.html#discussion

30

references i .

[Car+19] Stéphane Caron, Adrien Escande, Leonardo Lanari et Bastien Mallein. « Capturability-based Pattern Generation for Walking with Variable Height ». In : IEEE Transactions on Robotics (juil. 2019). [Car19] Stéphane Caron. « Biped Stabilization by Linear Feedback of the Variable-Height Inverted Pendulum Model ». submitted. Sept. 2019. [Cop78] William A. Coppel. Dichotomies in stability theory. Lecture notes in mathematics 629. OCLC : 3609707. Berlin : Springer, 1978. 97 p. [HHL14] Michael A. Hopkins, Dennis W. Hong et Alexander Leonessa. « Humanoid locomotion

• n uneven terrain using the time-varying Divergent Component of Motion ». In :

IEEE-RAS International Conference on Humanoid Robots. IEEE, 2014, p. 266–272. [HSF04]

• J. Hauser, A. Saccon et R. Frezza. « Achievable motorcycle trajectories ». In : IEEE

Conference on Decision and Control. Nassau, Bahamas : IEEE, 2004, 3944–3949 Vol.4. [Kaj+01] Shuuji Kajita, Fumio Kanehiro, Kenji Kaneko, Kazuhito Yokoi et Hirohisa Hirukawa. « The 3D Linear Inverted Pendulum Mode : A simple modeling for a biped walking pattern generation ». In : IEEE/RSJ International Conference on Intelligent Robots and

• Systems. 2001.

31

references ii .

[Kaj+10] Shuuji Kajita, Mitsuharu Morisawa, Kanako Miura, Shin'ichiro Nakaoka, Kensuke Harada, Kenji Kaneko, Fumio Kanehiro et Kazuhito Yokoi. « Biped walking stabilization based on linear inverted pendulum tracking ». In : IEEE/RSJ International Conference on Intelligent Robots and Systems. 2010. [KPT16] Twan Koolen, Michael Posa et Russ Tedrake. « Balance control using center of mass height variation : Limitations imposed by unilateral contact ». In : IEEE-RAS International Conference on Humanoid Robots. 2016. [Sug09] Tomomichi Sugihara. « Standing stabilizability and stepping maneuver in planar bipedalism based on the best COM-ZMP regulator ». In : IEEE International Conference

• n Robotics and Automation. 2009.

32