Control of Full Body Humanoid Push Recovery Using Simple Models Benjamin Stephens Thesis Proposal Carnegie Mellon, Robotics Institute November 23, 2009

Control of Full Body Humanoid Push Recovery Using Simple Models

Benjamin StephensThesis Proposal

Carnegie Mellon, Robotics Institute

November 23, 2009

Committee:Chris Atkeson (chair)

Jessica HodginsHartmut Geyer

Jerry Pratt (IHMC)

Benjamin Stephens | Carnegie Mellon University | Control of Full-Body Humanoid Push Recovery Using Simple Models

2


3

Thesis Proposal Overview

Simple models can be used to simplify control of full-body push recovery for complex robots

RxF

x

Lx

y

RyF

LzF

RzF

LyF LxF

xy

z

LyRx

Ry

refp

fp

0fp

Strategy decisions and optimization over future

actions

Simple approximate

dynamics model with COM and two

feet

Reactive full-body force

control


4

Motivations

• Improve the performance and usefulness of complex robots, simplifying controller design by focusing on simpler models that capture important features of the desired behavior

•Enabling dynamic robots to interact safely with people in everyday uncertain environments

•Modeling human balance sensing, planning and motor control to help people with balance disabilities


5

Approaches to Humanoid Balance

Controls Complex

Robot

Utilizes Simple

Model(s)

Reactive to

Pushes

Optimizes Over the Future

ZMP Preview Control S. Kajita, et.al., ‘03

Reflexive ControlPratt, ‘98Yin, et. al., ’07Geyer ‘09

Passive Dynamic WalkingMcGeer ’90

Inverse-Dynamics-Based ControlHyon, et. al., ’07Sentis, ‘07

Proposed Work

Exa

mp

les


6

Expected Contributions

•Analytically-derived bounds on balance stability defining unique recovery strategies

•Optimal control framework for planning step recovery and other behaviors involving balance

•Transfer of dynamic balance behaviors designed for simple models to complex humanoid through force control


7

Outline

•Simple Models of Biped Balance

•Push Recovery Strategies

•Optimal Control Framework

•Humanoid Robot Control

•Proposed Work and Timeline

refp

fp

0fp


8

Outline






refp

fp

0fp


9

Outline






refp

fp

0fp


10

Outline






refp

fp

0fp


11

Outline






refp

fp

0fp


12

Outline






refp

fp

0fp


13

Simple ModelsVery simple dynamic models approximate full body motion

refp

fp

0fp


•The sum of forces on the COM results in an acceleration of the COM

Simple Biped Dynamics

14

gF

RF LF

PF

gF

RF

LF

P~P Center of mass (COM)

PF

RP LP~, LR PP Foot locations

gPi FPmFF


•The COP is the origin point on the ground of the force that is equivalent to the contact forces


15

gF

PF

gF

P

eqF

CP

eqF

~CP Center of pressure (COP)

PF

RP LP

gPi FPmFF


•Ground torques can be used to move the COP or apply moments to the COM


16

gF

PF

gF

PF

PCP

eqF

HMFPP iii

~H Angular momentum

eqF

RP LP

gPi FPmFF


•The base of support defines the limits of the COP and, consequently, the maximumforce on the COM


17

gF

PF

gF

P

eqF

CP

gPi FPmFF PF

eqF

RP LP

HMFPP iii


•Instantaneous 3D biped dynamics form a linear system in contact forces.


18

gF

PF

PCP

eqF

RP LP

H

FPm

M

F

M

F

IPPIPP

II g

L

L

R

R

LR00


Simple Biped Inverse Dynamics

•The contact forces can be solved for generally using constrained quadratic programming

WFFbAFbAFF TT

F minarg

dCF

Least squares problem

(quadratic programming)Linear Inequality Constraints•COP under the

feet•Friction

H

FPm

M

F

M

F

IPPIPP

II g

L

L

R

R

LR00

bAF

19


20

3D Linear Biped Model

•The Linear Biped Model is a special case derived by making a few additional assumptions:▫Zero vertical acceleration▫Sum of moments about COM is zero▫Forces/moments are distributed linearly

P

P

L

L

R

R

L

L

R

R

M

F

M

F

M

F

0

0

0

0

1 LR

0

0 g

P

P

LLRR

FPm

M

F

IPPPP

I

REFERENCE:Stephens, “3D Linear Biped Model for Dynamic Humanoid Balance,” Submitted to ICRA 2010


21

Linear Double Support Region

•Using a fixed double support-phase transition policy, the weights can be defined by linear functions

Ry

Rx

Ly

Lx

y

x

yx

11

y

x

Ry

x

Rotated Coordinate Frame

D

yDL 2

D

yDR 2

Linear Weighting Functions

D2

REFERENCE:Stephens, “Modeling and Control of Periodic Humanoid

Balance using the Linear Biped Model,” Humanoids 2009


22

Using Linear Biped Model

•Analytic solution of contact forces and phase transition allows for explicit modeling of balance control.


23

Push Recovery Strategies For Simple ModelsSimple model dynamics define unique human-like recovery strategies

refp

fp

0fp


Three Basic Strategies

•From simple models, we can describe three basic push recovery strategies that are also observed in humans

1. 2. 3.

24


25

Ankle Strategy

Assumptions:▫Zero vertical acceleration▫No torque about COM

Constraints:▫COP within the base

of support

gF

PF

PCP

eqF

RP LP

REFERENCE:Kajita, S.; Tani, K., "Study of dynamic biped locomotion on rugged terrain-derivation and application of the linear inverted pendulum mode," ICRA 1991

CP PPL

mgF


26

Ankle Strategy

COM Position

CO

M

Velo

city

minmaxCC PP

L

gPPP

L

g

max2

minCC PP

PP

Linear constraints on the COP define a linear stability region for which the ankle strategy is stable

REFERENCE:Stephens, “Humanoid Push Recovery,” Humanoids 2007


27

Hip Strategy

Assumptions:▫Zero vertical acceleration▫Treat COM as a flywheel

Constraints:▫Flywheel “angle” has limits

gF

PF

PCP

eqF

RP LP

L

PPL

mgF CP

Im,

REFERENCE:•Pratt J, Carff J., Drakunov S., Goswami A., “Capture Point: A Step toward Humanoid Push Recovery” Humanoids, 2006


28

Hip Strategy

COM Position

CO

M

Velo

city

Linear bounds for the hip strategy are defined by assuming bang-bang control of the flywheel to maximum angle

Stephens, “Humanoid Push Recovery,” Humanoids 2007


Stepping

• Stepping can move the base of support to recover from much larger pushes. Simple models can predict step time, step location and the number of steps required to recover balance.

1. 2. 3. 4.

11, xx

CxSxCOM Position

CO

M V

elo

city

0

29

22 , xx

33 , xx



Stepping

• Stepping can move the base of support to recover from much larger pushes. Simple models can predict step time, step location and the number of steps required to recover balance.

1. 2. 3. 4.

11, xx

CxSxCOM Position

CO

M V

elo

city

0

30

22 , xx

33 , xx



Stepping

• Stepping can move the base of support to recover from much larger pushes.

1. 2. 3. 4.

11, xx

CxSxCOM Position

CO

M V

elo

city

0

31

22 , xx

33 , xx



32

Stepping

• Analytic models can predict step time, step location and the number of steps required to recover balance.

-0.7 -0.6 -0.5 -0.4 -0.3 -0.2 -0.1 0 0.1 0.2 0.3

-0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

0.7

x position

y p

osi

tion

Reaction RegionLocation of COP during capture swing phase

Capture RegionLocation of capture step that results in stable recovery



33

Strategy State Machine

•Analytic push recovery strategies can be incorporated into a finite state machine framework that then generates appropriate responses.

? Ankle Strategy

HipStrategy

SteppingSimple Model Look-

up


34

Optimal Control For Simple Model Push RecoveryEfficient optimal control performed on simple models approximates desired behavior of the full system. refp

fp

0fp


Optimal Control of Simple Model

• The dynamics of the simple model can be used to efficiently perform optimal control over an N-step horizon.

ttt

t

t

t

t

t

t

BuAxp

T

T

T

p

p

p

T

TT

p

p

p

2

32

1

1

1

5.0

166.0

100

10

5.01

t

t

t

t

t Cx

p

p

p

g

Lz

01

t

Nt

t

t

t Ux

z

z

z

Z DC

0

1

t

Nt

t

t

Nt

t

t

t Ux

u

u

u

x

x

x

x

X BABA

0

1

10

2

1

1

N-step LIPM Dynamics

N-step COP Output

LIPM Dynamics

COP Output

35

REFERENCE: •Kajita, S., et. al., "Biped walking pattern generation by using preview control of zero-moment point," ICRA 2003


36

Optimal Control of Simple Model• Given footstep location, optimal

control can solve for the optimal trajectory of the COM

Objective Function

222

1

2

2

1tt

reftt

reft

N

treft duppcppbzzaJ

02

1JUfHUUJ t

Tt

Tt

t

Tt

Tt

Ut UfHUUU

t 2

1minarg

reftz

tp

REFERENCE: •Wieber, P.-B., "Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations," Humanoid Robots 2006


Optimal Control for Stepping

• Footstep location can be added to the optimization to determine optimal step location and COM trajectory.

reftz

tp

refp

fp

0fp

222

1

2

2

1tt

reftt

reft

N

treft duppcppbzzaJ

ffreft ppp 02

1pU

reftf

reft ppz 1UU 00

1

1

1

pU

0

1

1

0 U

1

0

0

1 U

02

1J

p

Uf

p

UH

p

UJ

f

tT

f

t

T

f

t

0reftp

37

REFERENCE:•Diedam, H., et. al., "Online walking gait generation with adaptive foot positioning through Linear Model Predictive control," IROS 2008


-0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0.2

COM

ZMP

Optimal Step Recovery (Example)

refp

fp

0fp

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-0.05

0

0.05

0.1

0.15

posi

tion

x

y

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-0.2

0

0.2

0.4

0.6

velo

city

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1-0.1

0

0.1

0.2

0.3zm

p


39

Optimization of Swing Trajectory•The optimization can be

augmented to generate natural swing foot trajectories.

xF

bp

fp

p

fF

fF

-0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 0.3

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0.2

x

y

COM

ZMPSwing Foot

0 5 10 15 20 250

0.05

0.1

0.15

0.2

posi

tion

Swing Foot Trajectory

x

y

0 5 10 15 20 250

0.5

1

velo

city

0 5 10 15 20 25-10

-5

0

5

10

forc

e

timesteps


40

Optimization of Torso Lean

• Similarly, a third mass corresponding to the torso can be added. This can be used to model small rotations of the torso and hip strategies.

xF

p

bff

ffff pp

L

gmFpm

btt

tttt pp

L

gmFpm

tb

tf

b

f

bb

tt

ff

bb

pm

mp

m

mp

m

mp

pm

mp

m

mp

m

mp


41

Angular Momentum Regulation

•Large angular momentum about the COM must be dissipated quickly to regain balance

•There are two simple possibilities for dissipating angular momentum:

HKH Hdes

Asymptotically decrease angular momentum using a

fixed controller

TNTt

Tt

Tt HHH ,,, 1H

Include change of angular momentum in the optimization

REFERENCE:M. Popovic, A. Hofmann, and H. Herr, "Angular momentum regulation during human walking: biomechanics and control,“ ICRA 2004


42

Minimum Variance Control

• As opposed to minimizing jerk trajectories, it has been suggested that a more human-like objective function minimizes the variance at the target.

REFERENCE:•Harris, Wolpert, “Signal-dependent noise determines motor planning” Nature 1998

1

0

11t

i

T

iit

iit

t BuABuAXCov refp

fp

0fp

N

it uwXCovJ 21


43

Humanoid Robot Control Using Simple ModelsDynamics, strategies and optimal control of simple models can be combined to control full-body push recovery refp

fp

0fp


44

Controlling a Complex Robot with a Simple Model

•Full body balance is achieved by controlling the COM using the policyfrom the simple model.

•The inverse dynamics chooses from the set of valid contact forces the forcesthat result in the desired COM motion.

RxF

x

Lx

y

RyF

LzF

RzF

LyF LxF

xy

z

LyRx

Ry

Variable Fixed

Contact Force Selection

?


45

General Humanoid Robot Control

FJ

J

IN

N

q

x

MM

MMT

Tb

2

1

2

1

2221

1211 0

021

q

xJJ b

Dynamics

Contact constraints

Desired COM Motion

des

gdes

L

L

R

R

LR H

FPm

M

F

M

F

IPPIPP

II00

Control Objectives

Pose Bias qqKqqK desd

desp

Lx

RyF

LzF

RzF

LyF LxF

LyRx

Ry

Variable Fixed



46

General Humanoid Robot Control

Lx

RyF

LzF

RzF

LyF LxF

LyRx

Ry

Variable Fixed


qqKqqK

H

P

qJxJ

qJxJ

CG

CG

F

F

q

x

I

DD

DD

JJ

JJ

JJIMM

JJMM

desd

desp

des

des

b

b

R

L

b

RL

RL

RR

LL

TR

TL

TR

TL

21

21

22

11

22

11

21

21

222221

111211

0000

000

000

000

000

0


WxxbAxbAxx TT

x minarg

dCx

General Solution To Inverse Dynamics

•Fully general solution•Many “weights” to tune•May choose undesirable forces

Weighted least- squares solution

Linear Inequality Constraints:•COP under the feet•Friction

Variable Fixed



48

Feed-forward Force Inverse Dynamics

• Pre-compute contact forces using simple model and substitute into the dynamics

qJxJ

FJN

FJN

q

x

JJ

IMM

MM

b

T

Tb

21

22

11

21

2221

1211

0

0

bxA

Linear System

•Easier to solve•Less “weights” to tune•More model/task-specific•Pre-computing forces may be difficult

Variable Fixed



49

Simple Model Policy-Weighted Inverse Dynamics•Automatically generate weights according

to the optimal controller.▫2nd order model of the value function

determines cost function for applying non-optimal controls.

tttttt uxfVuxLuxQ ,,,

uuQuuuu optuu

Toptoptu

2

1Q cost

Variable Fixed



50

des

gdes

L

L

R

R

LR H

FPm

M

F

M

F

IPPIPP

II00

Simple Model Policy-Weighted Inverse Dynamics•Using the simple model, the cost function

can be converted into weights on inverse dynamics.

Variable Fixed


optuu

uuQuuuu optuu

Toptoptu

2

1Q cost


Task Control During Balance

•Modeled as a virtual external force/torque on the system

task

taskg

LR M

FFPmF

IPPIPP

II

0

00

task

task

T

task

T

taskT

Tb

M

F

J

JF

J

J

IN

N

q

x

MM

MM

2

1

2

1

2

1

2221

1211 0

Virtual COM Dynamics

Virtual Humanoid Dynamics

51

taskF

REFERENCE:•Pratt J., et.al., “Virtual Actuator Control," IROS 1996


52

Simulation of Full Body Push Recovery


53

Robot Push Recovery Experiments


54

Proposed Work

refp

fp

0fp


55

Proposed Work

•Implementation of human-like push recovery strategies on the Sarcos humanoid robot


56

Proposed Work

refp

fp

0fp

• Simple model dynamics• Simple model inverse dynamics

• Standing balance strategies• Stepping strategies• Strategy switching state machine

• Optimal control of stepping• Extensions to model (swing leg dynamics, hip strategy, etc.)• Sequential quadratic programming to determine optimal step time• 2nd order optimization generating local value function approximation• Full-body inverse kinematics tracking of optimal plan• Force feed-forward inverse dynamics for standing balance• Force feed-forward inverse dynamics for stepping• Policy-weighted inverse dynamics• Integral control for robustness

CompletedIn ProgressTo be completed


57

Receding Horizon Control of Simple Model

•The full body will not exactly agree with the simple model , but by re-optimizing over a receding horizon, control can be robust to small errors.


58

2nd Order Optimization of Simple Model

•A 2nd order optimization method produces a local approximation of the value function along the trajectory

Goal

Initial State

Local 2nd order model of value function

Optimal Trajectory

The 2nd order model describes the relative cost of applying an action other than the optimal action

Simple Model Policy-Weighted Inverse Dynamics

tttttt uxfVuxLuxQ ,,,

uuQuuuu optuu

Toptoptu

2

1Q cost


59

Sequential Quadratic Programming•SQP used to solve non-linear problems:

▫Step Time Optimization Existing optimal control framework is only

linear if a fixed step time is assumed.▫Double Support Constraints Because the step location is variable, the true

double support constraints are nonlinear.

•Analytic models can be used to estimate fixed values or provide good initial guesses.


60

Integral Balance Control

•Integral Balance Control, related to 2nd-order sliding mode control, was previously applied to control of humanoid balance.

•Can this method be used to transfer robust control of simple system to the full body?

REFERENCE:•Stephens, “Integral Control of Humanoid Balance," IROS 2007•Levant, “Sliding order and sliding accuracy in sliding mode control”, Journal of Control, 1993

uxhy , u

u

huxf

x

hy

,

uxfx ,

uxfx

hy

u

hu des ,

1


61

Timeline

•November ‘09 – Thesis Proposal▫6 months – Controller theory/refinement 1 month – Open loop planning 2 months – Receding horizon planning 3 months – Policy-weighted inverse dynamics

▫4 months – Experiments 1 month - Step recovery robot experiments 2 month - Multiple strategy robot

experiments 1 month – Comparison to human experiments

▫2 months – Thesis writing•December ‘10 - Defense


62

Conclusion

refp

fp

0fp


63

Thesis Proposal Overview

Simple models can be used to simplify control of full-body push recovery for complex robots

RxF

x

Lx

y

RyF

LzF

RzF

LyF LxF

xy

z

LyRx

Ry

refp

fp

0fp

Strategy decisions and planning over future actions

Simple approximate

dynamics model with COM and two

feet

Reactive full-body force

control


64

Acknowledgements•Committee:

▫Chris Atkeson (Advisor/Chair)▫Jessica Hodgins▫Hartmut Geyer▫Jerry Pratt (IHMC/External)

•Stuart Anderson•People who helped with practice talk

Questions?

Documents

Control of Full Body Humanoid Push Recovery Using Simple Models Benjamin Stephens Thesis Proposal Carnegie Mellon, Robotics Institute November 23, 2009