Space robots, Walking robots, Optimal Control

1

Pierre-Brice WieberINRIA Grenoble

Efficient computation of the dynamics

2

Lagrangian dynamics

M(q) =X

k

JTtk mk Jtk + JT

Rk Ik JRk

N(q, q) =X

k

JTtk mk Jtk + JT

Rk Ik JRk � JTRk (Ik JRk q)⇥ JRk

F =X

k

JTtk fk + JT

Rk ⌧k

M(q) q +N(q, q) q = F

3

Recursive Newton-Euler Algorithm

• Forward recursion: differential kinematics

• Backward recursion: inertial effects (Newton and Euler equations)

xk, !k, xk, !k

4

Let’s consider a simple example

5

SCARA robots

6

P2

P0

P1

x

y

q1

q2

1 m

1m

Kinematic model

7

Direct Kinematics Cartesian position

x1

y1

�=

0

0

�+

cos(q1) � sin(q1)

sin(q1) cos(q1)

� 1

0

�=

cos(q1)

sin(q1)

�

x2

y2

�=

0

0

�+

cos(q1) � sin(q1)

sin(q1) cos(q1)

�

✓1

0

�+

cos(q2 � q1) � sin(q2 � q1)

sin(q2 � q1) cos(q2 � q1)

� 1

0

�◆

=

cos(q1) + cos(q2)

sin(q1) + sin(q2)

�

8

Cartesian speed Jacobian matrix

x2

y2

�=

� sin(q1)q1 � sin(q2)q2

cos(q1)q1 + cos(q2)q2

�

=

� sin(q1) � sin(q2)

cos(q1) cos(q2)

� q1

q2

�

9

x1

y1

�=

� sin(q1) 0

cos(q1) 0

� q1

q2

�

Kinetic Energy

K =m1

2

x1

y1

�T x1

y1

�+

m2

2

x2

y2

�T x2

y2

�

10

Kinetic Energy

K =

m1

2

x1

y1

�T x1

y1

�+

m2

2

x2

y2

�T x2

y2

�

=

m1

2

q1

q2

�T 1 0

0 0

� q1

q2

�+

m2

2

q1

q2

�T 1 cos(q1 � q2)

cos(q1 � q2) 1

� q1

q2

�

11

Space robots, Walking robots, Optimal Control

12

Pierre-Brice WieberINRIA Grenoble

The dynamics of space robots

13

And yet it moves

< 1 degree/step14

Structure of the minimal coordinates

• Joint positions

• Position and orientation with respect to the environment

q =

2

4qx0

�0

3

5

15

Structure of the kinematics

xk = Jtk(q) q

!k = JRk(q) q

xk = x0 + R0(xk � x0) +R0 Jtk˙q

Rk !k = R0 !0 +R0 JRk˙q

Jtk =hR0 Jtk I3⇥3 �(xk � x0)⇥R0 JR0

i

JRk =h

JRk 03⇥3 R

Tk R0 JR0

i

16

Structure of the Lagrangian dynamics

M(q) =X

k

JTtk mk Jtk + JT

Rk Ik JRk

N(q, q) =X

k

JTtk mk Jtk + JT

Rk Ik JRk � JTRk (Ik JRk q)⇥ JRk

F =X

k

JTtk fk + JT

Rk ⌧k

17

M(q) q +N(q, q) q = F

Newton & Euler equations (again)

X

k

mk

⇣Jtk q + Jtk q

⌘=

X

k

mkxk =X

k

fk

18

X

k

(xk � x0)⇥mkxk +Rk Ik !k +Rk!k ⇥ (Ik !k) =

X

k

(xk � x0)⇥ fk +Rk ⌧k

Holonomy and nonholonomy

m c = 0

d

dt

hX(xk � c)⇥mkxk +RkIk!k

i= 0

19

c =1

m

X

k

mkxk

Skylab Gym

20

Nonholonomy for dogs

Etienne-Jules Marey (1830-1904)

21

And yet it moves

< 1 degree/step22

The dynamics of walking

23

Kerry Skarbakka

24

Avoiding to fallViability considerations 58

fall

fall

fall

fall

25

Lyapunov stability

8 " > 0, 9 � > 0 s.t. kx(t0)� x0k � ) 8 t � t0, kx(t)� x0k "

• Continuity of distance between trajectories

• Local concept

• Since 1892

26

• Stability implies local positive invariance

Positive invariance

27

Positive invariance

x(t0) 2 S ) 8 t � t0, x(t) 2 S

28

Obstacle avoidance

• Largest set that doesn’t intersect obstacles

• Largest invariant set that doesn’t intersect obstacles

• Largest set that doesn’t intersect obstacles and CAN be made invariant : the Viability Kernel

29

Michael Jackson

30

Contact forces

(m(c+ g) =

Pfi

d

dt

hX(xk � c)⇥mkxk +RkIk�k

i=

P(pi � c)⇥ fi

mc⇥ (c+ g) + L

m(cz + gz)=

Ppi ⇥ fiPfzi

31

On a flat ground, the Center of Pressure

• Let’s focus on the horizontal momenta

mc⇥ (c+ g) + L

m(cz + gz)=

Ppi ⇥ fiPfzi

cx,y � mcz cx,y � ˙Lx,y

m(cz + gz)=

Ppx,yi

fz

iPfz

i

⇥ conv {px,yi

}

32

Flat walking

• Let’s consider walking “flat”:

appears to be a linear Differential Inclusion

33

cx,y � cz

gzcx,y 2 conv {px,y

i

}

The dynamics of fallinga

CoM

Fig. 6. An example of convex hulls Z(t) in dotted line and Z(t) in plainline for five steps of biped walking. When the CoM lies on the edge of theset Z(t), it lies on a line (in red) defined by an orthogonal vector a (in blue)

such that the whole set Z(t) lies on one side of this line.

leading to the unavoidable conclusion that the CoM is diverg-

ing infinitely away from the set Z(t) in the direction of thevector a,

limt→+∞

aT xG(t) = +∞.

Every locomotion can be considered to start with a CoM at

rest within the set Z(t), and as soon as this CoM leaves this

set, it diverges infinitely away from it. We can observe that

such a divergence to infinity of this Cart-Table approximate

model corresponds in fact to a fall for the original legged robot

(Figure 5): avoiding the legged robot to fall means therefore

avoiding the Cart-Table model to diverge to infinity. From the

point of view of the Viability analysis discussed in section II-

B, the undesirable states for this Cart-Table model appear

therefore to be the states “at infinity”.

This viability analysis could be led further, and approxima-

tions of the corresponding viability kernel may be obtained,

but it is time now for Model Predictive Control to come into

play.

C. Minimizing the derivatives of the position of the Center of

Mass

One point of interest in the previous analysis is that when

diverging to infinity, the CoM diverges at a pace greater than

exponential, what implies that all its derivatives, velocity,

acceleration, jerk, etc... diverge at the same pace, greater than

exponential. Integrals of norms of these derivatives such as∫ +∞

tk

∥

∥

∥x

(n)G

(t)∥

∥

∥

2dt (10)

appear therefore to have infinite values for all diverging

motions, and finite values for all motions ending up in finite

time at an equilibrium state, the kind of locomotion we’re

generally interested in. Minimizing such integrals implicitly

selects therefore a non-diverging motion whenever possible,

what solves exactly the problem discussed in section II-B of

selecting viable states out of non-viable states. This is quite

a striking discovery that minimizing any derivative of the

position of the CoM always generates a safe legged locomotion

whenever possible!

On top of that, the exponential rates in the divergence of

the CoM also imply that the values of the same integrals but

over finite time intervals,

∫ tk+T

tk

∥

∥

∥x

(n)G

(t)∥

∥

∥

2dt, (11)

raise exponentially as well, allowing to discard most of the

non-viable states with the same minimization process even on

finite time intervals. Of course, the longer the time interval T ,the more non-viable states can be discarded.

Now, the minimization of these integrals can obviously be

put in the form of a Model Predictive Control scheme as

discussed in section II-C, leading directly to an exact viable

feedback in the case of the infinite integrals (10) (exact with

respect to the Cart-Table model) or to an approximate viable

feedback in the case of the finite integrals (11). The specificity

of this MPC scheme is that the viability property need not

be expressed and checked explicitly, it is satisfied implicitly

when minimizing the cost functions. And interestingly enough,

it appears that with such an MPC scheme, considering short

prediction horizons, no more than 1 s, what implies lookingonly one step ahead, is already enough in preserving viability

and generating safe locomotion even in the presence of strong

perturbations [15].

The fact that minimizing the jerk of the CoM generates sta-

ble walking motions has already been acknowledged in [12],

but through an optimal Linear Quadratic Regulator scheme

only related to Lyapunov stability and therefore inducing

viability only locally. This scheme would be unable therefore

to face properly strong perturbations such as in Figure 3. This

minimization of the jerk of the CoM has been generalized then

in [15], following the MPC scheme presented here, minimizing

the cost function (11) with respect to the dynamics (9).

The strong connection between this scheme and the viability

property that we presented here wasn’t discussed there, but

numerical simulations were proposed, demonstrating its ability

to face strong perturbations properly.

In these previous works, the foot steps and therefore the

convex hulls Z(t) were considered to be fixed in advance andimpossible to change. This limits of course the possibilities for

the legged system to maintain viability and avoid falling (this

is exactly the situation of Figure 2 with respect to Figure 3).

We can observe in the analysis presented here that the set-

valued function Z(t) can be included in the control variableswithout any other modification of the feedback scheme: modi-

fying foot placement in order to minimize the jerk of the CoM

will continue to generate stable walking motions whenever

possible.

IV. DISCUSSION

We have seen in the previous section how an exact viable

feedback control law can be designed for an approximate

model of legged locomotion through a very simple Model

Predictive Control scheme. We have seen that accurate and

perfectly tractable approximations can also be obtained very

easily by varying the length of the corresponding horizon of

prediction.

ha, cx,y(t)� cx,y(t0)i � ha, cx,y(t0)ir

cz

gsinh

✓rg

cz(t� t0)

◆

34

The Viability kernelViability considerations 58

fall

fall

fall

fall

35

Viability

• Avoiding to fall is a Viability objective

• Anticipation is the key

36

The dynamics of fallinga

CoM

Fig. 6. An example of convex hulls Z(t) in dotted line and Z(t) in plainline for five steps of biped walking. When the CoM lies on the edge of theset Z(t), it lies on a line (in red) defined by an orthogonal vector a (in blue)

such that the whole set Z(t) lies on one side of this line.

leading to the unavoidable conclusion that the CoM is diverg-

ing infinitely away from the set Z(t) in the direction of thevector a,

limt→+∞

aT xG(t) = +∞.

Every locomotion can be considered to start with a CoM at

rest within the set Z(t), and as soon as this CoM leaves this

set, it diverges infinitely away from it. We can observe that

such a divergence to infinity of this Cart-Table approximate

model corresponds in fact to a fall for the original legged robot

(Figure 5): avoiding the legged robot to fall means therefore

avoiding the Cart-Table model to diverge to infinity. From the

point of view of the Viability analysis discussed in section II-

B, the undesirable states for this Cart-Table model appear

therefore to be the states “at infinity”.

This viability analysis could be led further, and approxima-

tions of the corresponding viability kernel may be obtained,

but it is time now for Model Predictive Control to come into

play.

C. Minimizing the derivatives of the position of the Center of

Mass

One point of interest in the previous analysis is that when

diverging to infinity, the CoM diverges at a pace greater than

exponential, what implies that all its derivatives, velocity,

acceleration, jerk, etc... diverge at the same pace, greater than

exponential. Integrals of norms of these derivatives such as∫ +∞

tk

∥

∥

∥x

(n)G

(t)∥

∥

∥

2dt (10)

appear therefore to have infinite values for all diverging

motions, and finite values for all motions ending up in finite

time at an equilibrium state, the kind of locomotion we’re

generally interested in. Minimizing such integrals implicitly

selects therefore a non-diverging motion whenever possible,

what solves exactly the problem discussed in section II-B of

selecting viable states out of non-viable states. This is quite

a striking discovery that minimizing any derivative of the

position of the CoM always generates a safe legged locomotion

whenever possible!

On top of that, the exponential rates in the divergence of

the CoM also imply that the values of the same integrals but

over finite time intervals,

∫ tk+T

tk

∥

∥

∥x

(n)G

(t)∥

∥

∥

2dt, (11)

raise exponentially as well, allowing to discard most of the

non-viable states with the same minimization process even on

finite time intervals. Of course, the longer the time interval T ,the more non-viable states can be discarded.

Now, the minimization of these integrals can obviously be

put in the form of a Model Predictive Control scheme as

discussed in section II-C, leading directly to an exact viable

feedback in the case of the infinite integrals (10) (exact with

respect to the Cart-Table model) or to an approximate viable

feedback in the case of the finite integrals (11). The specificity

of this MPC scheme is that the viability property need not

be expressed and checked explicitly, it is satisfied implicitly

when minimizing the cost functions. And interestingly enough,

it appears that with such an MPC scheme, considering short

prediction horizons, no more than 1 s, what implies lookingonly one step ahead, is already enough in preserving viability

and generating safe locomotion even in the presence of strong

perturbations [15].

The fact that minimizing the jerk of the CoM generates sta-

ble walking motions has already been acknowledged in [12],

but through an optimal Linear Quadratic Regulator scheme

only related to Lyapunov stability and therefore inducing

viability only locally. This scheme would be unable therefore

to face properly strong perturbations such as in Figure 3. This

minimization of the jerk of the CoM has been generalized then

in [15], following the MPC scheme presented here, minimizing

the cost function (11) with respect to the dynamics (9).

The strong connection between this scheme and the viability

property that we presented here wasn’t discussed there, but

numerical simulations were proposed, demonstrating its ability

to face strong perturbations properly.

In these previous works, the foot steps and therefore the

convex hulls Z(t) were considered to be fixed in advance andimpossible to change. This limits of course the possibilities for

the legged system to maintain viability and avoid falling (this

is exactly the situation of Figure 2 with respect to Figure 3).

We can observe in the analysis presented here that the set-

valued function Z(t) can be included in the control variableswithout any other modification of the feedback scheme: modi-

fying foot placement in order to minimize the jerk of the CoM

will continue to generate stable walking motions whenever

possible.

IV. DISCUSSION

We have seen in the previous section how an exact viable

feedback control law can be designed for an approximate

model of legged locomotion through a very simple Model

Predictive Control scheme. We have seen that accurate and

perfectly tractable approximations can also be obtained very

easily by varying the length of the corresponding horizon of

prediction.

ha, cx,y(t)� cx,y(t0)i � ha, cx,y(t0)ir

cz

gsinh

✓rg

cz(t� t0)

◆

37

Regulate the Center of Mass

• Any derivative of the motion of the Center of Mass allows discriminating falling motions

• Optimal Control may be a solution to the Viability objective

38

Z 1

t

���c(n)���2dt

Optimal Control

39

Dynamics of manipulator robots

M(q) q +N(q, q) q +G(q) = ⌧

Gravity and joint torques

40

State space representation

x = f(x, u)

M(q) q +N(q, q) q +G(q) = ⌧

41

State space representation

x =

q

q

�, u = ⌧

x =

q

q

�=

q

M(q)�1(u�N(q, q)q �G(q))

�= f(x, u)

42

Continuous or discrete time

x = f(x, u)

xk+1 = f(xk, uk)

43

Optimal control

• is asymptotically stabilizing if the system is controllable

xk+1 = f(xk, uk)

V

⇤(x0) = minu0,...

1X

0

l(xk, uk)

u

⇤0(x0)

44

Optimal control

• is asymptotically stabilizing if the system is controllable

• as Lyapunov function

xk+1 = f(xk, uk)

V

⇤(x0) = minu0,...

1X

0

l(xk, uk)

u

⇤0(x0)

V

⇤(x0)

45

Linear Quadratic Regulator (LQR)

xk+1 = Axk +B uk

V (x0) = minu0,...

1X

k=0

x

TkQxk + u

TkRuk

46

Hamilton-Jacobi-Bellman Dynamic Programming

V (x0) = minu0,...

1X

k=0

x

TkQxk + u

TkRuk

= minu0

x

T0 Qx0 + u

T0 Ru0 + V (x1)

47

LQR? PD!

V (x) = x

TP x

x

T0 P x0 = min

u0

x

T0 Qx0 + u

T0 Ru0 + x

T1 P x1

= minu0

x

T0 Qx0 + u

T0 Ru0 + (Ax0 +Bu0)

TP (Ax0 +Bu0)

Ru0 +B

TPB u0 +B

TPAx0 = 0

u0 = �(R+B

TPB)�1

B

TPAx0 = K x0

48

Algebraic Ricatti equation

x

T0 P x0 = x

T0 (Q+A

TPA�A

TPB(R+B

TPB)�1

B

TPA)x0

49

Biped walking

50

Flat walking

• Let’s consider walking “flat”:

appears to be a linear Differential Inclusion

51

cx,y � cz

gzcx,y 2 conv {px,y

i

}

Kajita 2003 ICRA

2

4ck+1

ck+1

ck+1

3

5 =

2

41 T T 2/20 1 T0 0 1

3

5

2

4ckckck

3

5+

2

4T 3/6T 2/2T

3

5 ...c k

V (x0) = minu0,...

1X

k=0

x

Tk

2

410

�c

z/g

z

3

5 ⇥1 0 �c

z/g

z⇤xk + u

Tk uk

52

Kajita 2003 ICRA

53