Walking motion Control: theory and implementation Pierre-Brice - - PowerPoint PPT Presentation

Walking motion Control: theory and implementation

Pierre-Brice Wieber INRIA Grenoble

Who am I ?

• Master thesis on biped robots in 1997
• PhD thesis on biped robots in 2000
• My algorithms implemented in HRP-2, Nao

and tested on other robots

• Never participated to any RoboCup

What are we going to see today ?

• Dynamics of legged locomotion
• Generation of dynamic walking motions
• Motion and force control
• Numerical implementation details

Milestones in legged robotics

• 1960s : Walking Truck (R. Mosher)
• 1970s : Waseda university (I. Kato)
• 1980s : Adaptive Suspension

Vehicle (R. McGhee)

• 1980s : MIT LegLab (M. Raibert)
• 1996 : Honda P2 (K. Hirai, T. Takenaka...)

The walking truck

The adaptive suspension vehicle

MIT LegLab

The Honda P2

The dynamics of legged locomotion

Structure of the minimal coordinates

• Joint positions
• Position and orientation with respect to

the environment

q =   ˆ q x0 θ0  

Structure of the Lagrangian dynamics

M(q) @ 2 4 ¨ ˆ q ¨ x0 ¨ θ0 3 5 + 2 4 g 3 5 1 A + n(q, ˙ q) = 2 4 u 3 5 + X

i

C

i(q)T f i

m (¨ c + g) = X

i

f

i

˙ L = X

i

(pi − c) × f

i

L = X

k

(xk − c) × mk ˙ xk + Ikωk

And yet it moves

< 1 degree/step

12

Structure of the Lagrangian dynamics

M(q) @ 2 4 ¨ ˆ q ¨ x0 ¨ θ0 3 5 + 2 4 g 3 5 1 A + n(q, ˙ q) = 2 4 u 3 5 + X

i

C

i(q)T f i

m (¨ c + g) = X

i

f

i

˙ L = X

i

(pi − c) × f

i

L = X

k

(xk − c) × mk ˙ xk + Ikωk

On a flat ground, the Center of Pressure

m c × (¨ c + g) + ˙ L m(¨ cz + gz) = P

i pi × f i

P

i f z i

cx,y − cz ¨ cz + gz (¨ cx,y + gx,y) + 1 m(¨ cz + gz)S ˙ Lx,y = P

i f z i px,y i

P

i f z i

On a flat ground, the Center of Pressure

z y x z p1 p2 p3 p4 f z

1

f z

2

f z

3

f z

4

On a flat ground, the Center of Pressure

cx,y − cz ¨ cz + gz (¨ cx,y + gx,y) + 1 m(¨ cz + gz)S ˙ Lx,y = P

i f z i px,y i

P

i f z i

cz ¨ cz + gz (¨ cx,y + gx,y) = (cx,y − zx,y) + 1 m(¨ cz + gz)S ˙ Lx,y

On a flat ground, the Center of Pressure

cx,y zx,y βS ˙ L

x,y

−αgx,y α¨ cx,y

On a flat ground, the Center of Pressure

cx,y − cz ¨ cz + gz (¨ cx,y + gx,y) + 1 m(¨ cz + gz)S ˙ Lx,y = P

i f z i px,y i

P

i f z i

cz ¨ cz + gz (¨ cx,y + gx,y) = (cx,y − zx,y) + 1 m(¨ cz + gz)S ˙ Lx,y

Not just a «Linear Inverted Pendulum Model»

Walking horizontally

cx,y − cz gz ¨ cx,y = zx,y cz ¨ cz + gz (¨ cx,y + gx,y) = (cx,y − zx,y) + 1 m(¨ cz + gz)S ˙ Lx,y

The dynamics of falling

20

a CoM

aT (cx,y(t) − cx,y(t0)) ≥ aT ˙ cx,y(t0) ω sinh (ω(t − t0))

Viability

fall fall fall fall

21

The dynamics of falling

22

a CoM

aT (cx,y(t) − cx,y(t0)) ≥ aT ˙ cx,y(t0) ω sinh (ω(t − t0))

Viability, capturability

fall fall fall fall

23

Z ∞

t0

• c(n)(t)
• dt

The Capture Point

ξ = c + 1 ω ˙ c ˙ c = ω(ξ − c) ˙ ξx,y = ω(ξx,y − zx,y)

The Capture Point

cx,y zx,y ξx,y 1 ω ˙ cx,y 1 ω ˙ ξ

x,y

1 ω2 ¨ cx,y

Generation of dynamic walking motions

Early offline schemes

• Trajectory optimization
• Artificial synergy synthesis & ZMP

approach

• Templates & anchors

cz ¨ cz + gz (¨ cx,y + gx,y) = (cx,y − zx,y) + 1 m(¨ cz + gz)S ˙ Lx,y

Online motion generation

• Necessary for reactivity
• How to make sure you are stable in the

long term?

Viability, capturability

fall fall fall fall

29

Z ∞

t0

• c(n)(t)
• dt

Optimal and Model Predictive Control

Optimal feedback

• is asymptotically stabilizing if the

system is controllable

• as Lyapunov function

xk+1 = f(xk, uk) V ∗(x0) = min

u0,... ∞

X l(xk, uk) u∗

0(x0)

V ∗(x0)

Terminal constraint

• Keerthi 1988 JOTA
• as Lyapunov function

V ∗

N(x0) = min u0,... N−1

X l(xk, uk) with xN = 0 V ∗

N(x0) ≥ V ∗ N+1(x0) ≥ · · · ≥ V ∗(x0)

V ∗

N(x0) ≥ l(x0, u∗ 0) + V ∗ N(f(x0, u∗ 0))

V ∗

N(x0)

Feasibility is sufficient

• Alamir 1999 EJC
• Compute a new plan only in case of a

diverging perturbation

find u0, . . . such that xN = 0

Horizon long enough

• Alamir 1995 A
• You can do without explicit terminal cost

and constraint with a horizon long enough

V ∗

N(x0) = min u0,... N−1

X l(xk, uk) V ⇤

N(x0) = l(x0, u⇤ 0) + V ⇤ N(f(x0, u⇤ 0)) − l(x0 N, u⇤0 N)

∀N ≥ Nε, l(x0

N, u⇤0 N) < ε

Predefined footsteps, Capturability constraint

Waseda University

• Must always be able to stop within 2 steps.

P mi(¨ cz

i + gz)cx,y i

− mi cz

i ¨

cx,y

i

P mi(¨ cz

i + gz)

− → zx,y

ref

Waseda University

TUM Johnny/Lola

cx,y(t + ∆T) = cx,y

ref

P mi(¨ cz

i + gz)cx,y i

− mi cz

i ¨

cx,y

i

P mi(¨ cz

i + gz)

≈ zx,y

ref

TUM Johnny/Lola

Honda Asimo

P mi(¨ cz

i + gz)cx,y i

− mi cz

i ¨

cx,y

i

P mi(¨ cz

i + gz)

≈ zx,y

ref

ξx,y(t + ∆t) = ξx,y

ref

ToDaï H7 & Toyota

cx,y − m cz ¨ cx,y − ˙ Lx,y m(¨ cz + gz) − → px,y

i

cx,y(t + ∆T) = cx,y

ref

Sony QRIO

min X

• cx,y − cz

gz ¨ cx,y − px,y

i

• 2

cx,y(t + ∆T) = cx,y

ref , ˙

cx,y(t + ∆T) = 0

Predefined footsteps, NO capturability constraint

Kawada HRP-2

min X k... c x,yk2 + β

• cx,y cz

gz ¨ cx,y + S ˙ Lx,y mgz px,y

i

• 2

A korean variant

min X k... c x,yk2 + β

• cx,y cz

gz ¨ cx,y + S ˙ Lx,y mgz px,y

i

• 2

+ γkLx,yk2

Nao omniwalk

cx,y − cz gz ¨ cx,y ∈ conv {px,y

i

} min X k... c x,yk2

EuroGraphics

min X ⇣

• cx,y,z

k

− px,y,z

i

• − lref

⌘2 +

• cx,y − cz¨

cx,y ¨ cz + gz − px,y

i

• 2

Adaptive footsteps

Nao’s future algorithm

cx,y − cz gz ¨ cx,y ∈ conv {px,y

i

} min X

• ˙

cx,y − ˙ cx,y

ref

• 2

Walking without thinking about it

Vision feedback

Todaï

ξx,y(t + ∆t) = ξx,y

ref

Todaï

Todaï

Key ingredients ?

• Viability & Capturability
• Artificial synergy synthesis
• Model Predictive Control

Boston Dynamics ?

Don’t want/have computing resources ?

Combining simple rules (MIT LegLab)

• Control vertical oscillations
• Control upper body attitude
• Adaptive step placement, «neutral position»

Combining simple rules (biomimetic)

• Central Pattern Generators (oscillators)
• Control upper body attitude
• Adaptive step placement

Motion and Force Control

Whole body motion

• Inverse Kinematics + joint control
• Virtual Model Control
• Task Function Approach
• Operational Space Control

CoM motion control

cx,y zx,y ξx,y 1 ω ˙ cx,y 1 ω ˙ ξ

x,y

1 ω2 ¨ cx,y

CoM motion control

˙ c = ω(ξ − c) ˙ ξx,y = ω(ξx,y − zx,y) zx,y = cx,y

ref + k(ξx,y − cx,y ref )

˙ ξx,y = ω(k − 1)(cx,y

ref − ξx,y)

Contact force control

• Damping oscillations

˙ z = ωz(zd − z) zx,y

d

= cx,y

ref + k(ξx,y − cx,y ref ) + k0(zx,y − cx,y ref )

Contact force control

Numerical implementation

People who are really serious about software should make their own hardware.

Alan Kay

(invented Object Oriented Programming at Xerox PARC)

People who are really serious about control algorithms should make their own numerical solver.

Pierre-Brice Wieber

(invented not much yet at INRIA Grenoble)

Discretized trajectories

• Nishiwaki 2002 IROS: 52–104 samples over

3 steps, 2.6–5.2 s / 50 ms

• Buschmann 2007 ICHR: 20–30 pieces of

cubic spline over 3 steps ≈ 100 ms period

• Morisawa 2006 ICHR: 7 pieces of quartic
• r quintic + exponential over 2 steps ≈

300 ms

Discretized trajectories

• Takenaka 2009 IROS: 14 pieces of line +

exponential over 2 steps ≈ 70–130 ms

• Pratt 2006 ICHR: 2 pieces of exponentials
• ver 1 step

Discretized trajectories

• Van de Panne 1997 EG: 2 pieces of cubic

spline over 2 steps

• Kajita 2003 ICRA: 320–640 pieces with

constant jerk over 2–4 steps, 1.6–3.2 s / 5 ms

• Herdt 2010 RSJAR: 16 pieces with constant

jerk over 2 steps, 1.6 s / 100 ms

Discretized trajectories

• Like Kajita and Buschmann, let’s use cubic

splines (piecewise constant jerk) for the CoM

• We can bound the overshoot of the CoP:
• 100 ms appears fair enough in this linear

zx,y zx,y

max  1

8¨ cx,y

maxδt2 ⇡ gzkzx,y cx,ykmax

8cz δt2 ⇡ 1 2δt2

Detailed formulas

Herdt 2010 RSJAR

... Ck =    ... c k . . . ... c k+N−1    Zx

k+1 =

   zx

k+1

. . . zx

k+N

   = Sz   cx

k

˙ cx

k

¨ cx

k

  + Uz ... C

x k

˙ Ck+1 =    ˙ ck+1 . . . ˙ ck+N    = Sv   ck ˙ ck ¨ ck   + Uv ... Ck

Detailed formulas

Sz =    1 T T 2/2 − h/g . . . . . . . . . 1 NT N 2T 2/2 − h/g    Uz =    T 3/6 − Th/g . . . ... (1+3N +3N 2) T 3/6 − Th/g . . . T 3/6 − Th/g   

Sv =    1 T . . . . . . . . . 1 NT    Uv =    T 2/2 . . . ... (1+2N) T 2/2 . . . T 2/2   

Cost function

Fk+1 = Vk+1fk + ¯ Vk+1 ¯ Fk+1

Vk+1 =                  1 . . . 1 . . . . . .                  ¯ Vk+1 =                   . . . . . . 1 . . . . . . 1 1 . . . . . . 1 ...                  

min α 2

• ...

C

x,y k

• 2 + β

2

• ˙

Cx,y

k+1 − ˙

Cx,y

ref

• 2

+ γ 2

• Zx,y

k+1 − F x,y k+1

• 2

Cost function

min

xk

1 2xT

k Qkxk + pT k xk

Qk = Q0

k

Q0

k

• Q0

k =

αI + βU T

v Uv + γU T z Uz

−γU T

z ¯

Vk+1 −γ ¯ V T

k+1Uz

γ ¯ V T

k+1 ¯

Vk+1

• xk =

    ... C

x k

¯ F x

k+1

... C

y k

¯ F y

k+1

    min α 2

• ...

C

x,y k

• 2 + β

2

• ˙

Cx,y

k+1 − ˙

Cx,y

ref

• 2

+ γ 2

• Zx,y

k+1 − F x,y k+1

• 2

Constraints on the CoP

xk =     ... C

x k

¯ F x

k+1

... C

y k

¯ F y

k+1

    l ≤ R(F θ

k+1)

Zx

k+1 − F x k+1

Zy

k+1 − F y k+1

• ≤ u

l ≤ R(F θ

k+1)

✓Uz − ¯ Vk+1 Uz − ¯ Vk+1

• xk +

Szˆ cx

k − Vk+1f x k

Szˆ cy

k − Vk+1f y k

◆ ≤ u

✓Z0x

k+1

Z0y

k+1

◆ = R(F θ

k+1)

Zx

k+1 − F x k+1

Zy

k+1 − F y k+1

• Trivial constraints

x0

k =

    Z0x

k

¯ F x

k+1

Z0y

k

¯ F y

k+1

    l ≤ R(F θ

k+1)

Zx

k+1 − F x k+1

Zy

k+1 − F y k+1

• ≤ u

How to solve a Quadratic Program

Unconstrained

min

x

1 2xT Qx + pT x Qx + p = 0

Equality constrained

min

x

1 2xT Qx + pT x Qx + p = AT λ ✓ Q AT A ◆ ✓ x −λ ◆ + ✓p b ◆ = 0 s.t. Ax + b = 0

Trivially constrained

min

x

1 2xT Qx + pT x ¯ Q¯ x + ¯ p = 0 s.t. x + b = 0 Qx + p = ET λ (x = Ex)

83

Active constraints

Computation time

0.002 0.004 0.006 0.008 0.01 0.012 0.014 0.016 2 4 6 8 10 12 Computation time (s) Time (s) Computation time for QL, PLDP, with Warm Start (WS), with Limited Time (LT) on HRP-2 QL PLDP PLDP WS-LT