PLANNING OPTIMAL MOTIONS FOR ANTHROPOMORPHIC SYSTEMS Antonio El - - PowerPoint PPT Presentation

1

PLANNING OPTIMAL MOTIONS FOR ANTHROPOMORPHIC SYSTEMS

Antonio El Khoury Under the supervision of Florent Lamiraux and Michel Taïx

PhD Defense Committee Brigitte d’Andréa-Novel Maren Bennewitz Timothy Bretl Patrick Danès Rodolphe Gelin Abderrahmane Kheddar Florent Lamiraux Michel Taïx

June 3rd 2013

2

THE MOTION PLANNING PROBLEM

3

A DECOUPLED APPROACH FOR MOTION PLANNING

[Kuffner et al. (ICRA 2000)] [Lozano-Perez (TRO 1983)]

4

OUTLINE

1 2 3

5

PATH OPTIMIZATION FOR THE BOUNDING BOX APPROACH

1

 Simplification of planning: 3-DoF bounding box of the robot

6

PROBLEM SIMPLIFICATION: THE BOUNDING BOX APPROACH

[Yoshida et al. (TRO 2008)]

7

CART-TABLE MODEL

c x y c

z x x p g p z y y g                     

 Dynamic balance criterion for walking robots on a flat surface: the Zero-Moment Point (ZMP)

[Kajita et al. (ICRA 2003)] [Vukobratovic et al. (TBE 1969)]

8

PREVIEW-CONTROL-BASED PATTERN GENERATOR

 The Center of Mass (CoM) trajectory is generated from a desired ZMP trajectory for the cart-table model

[Kajita et al. (ICRA 2003)]

9

SO WHAT’S WRONG?

 What is wrong with the current scheme?

• Random nature of RRT ⇒ Random path
• Even after shortcut optimization, robot orientation is still random

 Need for frontal walking

• Shorter trajectories (in time)
• Camera facing the walking direction

10

CONTRIBUTION: REGULAR SAMPLING OPTIMIZATION (RSO)

11

REGULAR SAMPLING OPTIMIZATION (RSO)

12

A* ALGORITHM

13

COST FUNCTION

2 2 2 2

( ) ( ) 1 if ( ) ( ) 1 if

f la t f f la t m a x m a x f la t f f la t m in m a x

v v v v v C v v v v v               

1 ( , ) ( )

L i j

c o s t d s v s   q q (Walking time)

 Heuristic function: cost of walking frontally from to while staying on P

i

q

g

q

14

APARTMENT SCENARIO

RRT RRT Shortcut tcut Optimizati zation

• n

RSO Total

• tal

Chairs 4.0 1.9 2.1 8.0 Boxes 0.092 2.5 0.24 2.8 Apartm tmen ent 1.2 2.4 2.4 6.0

15

PERFORMANCE OF REGULAR SAMPLING OPTIMIZATION

 Computation time (s)  Walking time (s)

Shortcut tcut Optimization zation Shortcut tcut Optimization zation + RS RSO Chairs 40 35 Boxes 66 57 Apartm tmen ent 200 120

 Summary of RSO

• Regular sampling of path
• Four orientations states for each sample configuration
• A* search with time as cost function

 Discussion of results

• Optimized trajectories are shorter with respect to walk time
• Very low computational overhead to the planning scheme when

compared to walking time gain

16

SUMMARY

17

BUT…

18

WHOLE-BODY OPTIMAL MOTION PLANNING

2

19

RRT EXTENSION

[Kuffner et al. (ICRA 2000)]

20

 Contact and static balance constraints: plan on a zero- measure manifold

( )  f q

PLANNING ON A CONSTRAINED MANIFOLD

[Berenson et al. (IJRR 2011), Dalibard et al. (IJRR 2013)]

Planning manifold: Fixed right foot 6D position Fixed left foot 6D position Center of mass projection at support polygon center

21

STATICALLY BALANCED PATH PLANNING

 Properties

• Generation of quasi-static collision-free paths.
• Probabilistic completeness.
• Geometric local minima avoidance.

 Drawbacks

• Random and long paths.
• No time parametrization.
• Additional processing needed to obtain a feasible trajectory.

22

CONSTRAINED RRT: PROPERTIES AND DRAWBACKS

 , : state and control vectors  : differential equation of the model  , , : constraint vector functions

23

NUMERICAL OPTIMIZATION FOR OPTIMAL CONTROL PROBLEMS (NOC)

(· ), (· ),

m in ( ( ), ( ), ) ( ( ), ( )) ( ( ))

T T

J t T T L t t d t T   



x u

x u x u x

( ) ( , ( ), ( )), [0 , ], ( , ( ), ( )) , [0 , ], ( , ( ), ( )) , [0 , ], ( (0 ), ( )) . t t t t t T t t t t T t t t t T T        x f x u g x u h x u r x x

x

u

f

g

h

r

 Properties

• Generation of locally optimal trajectories.
• Enforcement of equality and inequality constraints.

 Drawbacks

• Possible failure if stuck in local minima.
• Success depends of the “initial guess”.
• Prior processing needed to guarantee optimization success.

24

NOC: PROPERTIES AND DRAWBACKS

 Optimal motion planning two-stage scheme: first plan draft path, then optimize  Locally optimal collision-free trajectory generation  Application to a humanoid robot with fixed coplanar contact points

25

A DECOUPLED APPROACH FOR OPTIMAL MOTION PLANNING

 Capsule: Set of points lying at a distance r from a segment  Simple to implement  Fast distance and penetration computation

26

(SELF-)COLLISION AVOIDANCE: CAPSULE BOUNDING VOLUMES

e1 e2 r

27

MINIMUM BOUNDING CAPSULE OVER A POLYHEDRON

2 3 , ,

4 m in 3

r

r r    

1 2

e e 2 1

e e

( , ) 0 , fo r a ll r d   

1 2

v e e v

28

OPTIMAL CONTROL PROBLEM FORMULATION

( ) ( )

T T

J t t d t   q q ( ) [ ( ), ( ), ( )] ( ) [ ( )]

T T

t t t t t t   x q q q u q ( ) ( ) ( ) t t t       q q q q q q τ τ τ

( ( )) ( (0 )) ( ( )) ( (0 )) ( ( ), ( ), ( ))

lf lf rf rf zm p su p

t t t t t    p q p q p q p q p q q q

( )

m in t

 d

 MUSCOD-II: specially tailored SQP solver.  Trajectory search space: (or jerk) piecewise linear.  Discretized constraints: 20 nodes over trajectory.

29

OPTIMAL CONTROL PROBLEM SOLVER

( ) t q

30

MARTIAL ARTS MOTION

31

SHELVES SCENARIO

 Decoupled approach: first plan, then optimize.  Draft path provided by constrained planner.  Path used as initial guess by numerical optimal control solver  Generated trajectories are locally optimal, feasible, and collision-free.

32

SUMMARY

 Optimal control solver: black box

• Even with a proper initial guess and duration, solver fails sometimes
• Difficult to tune

 Problems are sensitive to scaling  Very long computation time  Difficult to extend to walking motions

33

BUT…

34

A WHOLE-BODY MOTION PLANNER FOR DYNAMIC WALKING

3

35

PLANNING ON A CONSTRAINED MANIFOLD

36

SMALL-SPACE CONTROLLABILITY

 For small-space controllable systems any collision-free path can be approximated by a sequence of collision-free feasible trajectories

SMALL-SPACE CONTROLLABILITY

37

[Laumond et al. (TRO 1994)]

SMALL-SPACE CONTROLLABILITY OF A WALKING HUMANOID ROBOT

 A quasi-statically walking humanoid robot is not small-space controllable

38

 Cart-table model:   Moving the CoM fast enough in an arbitrarily small neighborhood generates dynamically balanced walk

39

SMALL-SPACE CONTROLLABILITY OF A WALKING HUMANOID ROBOT

2 2 2

1 , 1

x y c

x x p g p z y y                         

( ) sin ( ) y t t  

2

( ) (1 ) s in ( )

y

p t t           

 

          

SMALL-SPACE CONTROLLABILITY OF A WALKING HUMANOID ROBOT

 A dynamically walking humanoid robot is is small-space controllable!

40

41

A TWO-STEP WHOLE-BODY MOTION PLANNING ALGORITHM

42

WHOLE-BODY MOTION PLANNING IN A CLUTTERED ENVIRONMENT

43

APPLICATION ON THE HRP-2

 A dynamically-walking humanoid robot is small-space controllable  A two-step well-grounded algorithm for whole-body motion planning on a flat surface

• Plan a draft sliding quasi-static path
• Use the small-space controllability property to approximate it with a

sequence of collision-free steps

 Combine navigation and manipulation seamlessly  Simpler, more reliable and faster than whole-body optimal control

44

SUMMARY

45

CONCLUSION

 Efficient path optimization method for humanoid walk planning when using a bounding box approach [ICINCO 2011]  Combining constrained path planning and optimal control methods for the generation of locally optimal collision-free trajectories [ICRA 2013]  Generalization of constrained path planning to walk planning [Humanoids 2011, IJRR 2013]  All contributions used to generate motions on the HRP-2 humanoid robot

46

SUMMARY OF CONTRIBUTIONS

 Decoupled approach for motion planning

• Easy
• Fast
• Sound

 Instead of generating complex motions for complex dynamics

• Focus on simpler systems
• Find equivalence properties
• Solve efficiently and reliably a particular class of motion planning

problems

47

CONCLUSION

 How can we execute trajectories reliably in uncertain environments?  Can we extend the small-space controllability property to multi-contact motion?

48

OPEN QUESTIONS

THANK YOU FOR YOUR ATTENTION

49