CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Whole-body optimization based control
Abderrahmane KHEDDAR
CNRS-AIST Joint Robotics Laboratory (JRL), UMI3218/CRT, Tsukuba, Japan
CNRS-UM2 LIRMM, Interactive Digital Humans group, Montpellier, France
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Summary

Motivation


Dynamic motion generation between
contact stances



Multi-contact planning
Whole-body dynamic optimization
Experiments on HRP-2
Problems and open issues
2
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Cumbersome environments
3
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Escande, Kheddar, Miossec IEEE/RSJ IROS 2009 (video session)
4
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
PG: implementation
5
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Tasks in posture generation
can be used in a more general way
to express tasks not related to planning:
• Orientation of a body
• looking at a target (including a new contact)
• keeping visual features in the field of view
•…
It amounts to restraint
to smaller sub-manifolds
9
6
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Example of tasks

Carrying a glass
Idea: having n collinear to
k with the same direction
Escande, Kheddar, Miossec, Garsault, ISER, 2008
Escande, Kheddar, IEEE/RSJ IROS 2009
Escande, Kheddar, Chapter 6 in Humanoid Motion Planning, K. Harada, E. Yoshida and K.
Yokoi (Eds), Springer, STAR series, pp. 161–180, 2010
7
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Generalized PG

Unifies manipulation and locomotion


Unifies objects, robots, agents


No distinctions
Only goals are specified
Functional extensions


Bilateral contacts (e.g. grasps)
Deformable bodies
Bouyarmane, Kheddar, IEEE/RSJ Humanoids, 2010
Bouyarmane, Kheddar, Multi-contact stances planning for multiple agents,
IEEE ICRA, 2011
8
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Generalized PG in planning
Bouyarmane, Kheddar, Multi-contact stances planning for multiple agents,
IEEE ICRA, 2011
9
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Motion generation

Main ideas

MPC on simplified models


Operational task-based prioritized control




Computer Graphics communities (all variants)
Bouyarmane et al. IROS 2011; Salini et al. ICRA 2011
Whole-body dynamic optimization


E.g. Sentis, Park, Khatib, IEEE TRO 2010
E.g. Mansard, Saab et al. or L. Righetti et al. ICRA 2011
Closed-loop QP-based control


All variants of Kajita et al.’s PG
This talk
Possibly others

E.g. learning techniques
10
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Why motion optimization

Benefits





Minimization of a criteria
Same method whatever the motion
Easy inclusion of all constraints (actuator limitations, joint
limits, stability, collision)
Necessary for high performance motions, highly constrained
motions
Drawbacks


Off-line (solution: motions database)
Does not solve control problem (possibility : stochastic
optimization)
11
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Motion optimization problem

System model
u  A(q)q  H (q, q)  J (q)T Fc

General problem




Look for a motion q(t) or control u(t) t in [0…tf]
Criteria to minimize f(q(t),u(t))
Constraints to satisfy c(q(t),u(t))</=0
t in [0…tf]
Problem to solve
min f  q(t ), u(t ) 
q ( t ) or u ( t )
c  q(t ), u (t )   0

12
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
How to solve optimization pb?

Solving method (first implementations)

Discretization




Of parameters q(t) = q(p,t) (ex.: B-Splines)
Of constraints at times ti: c(q(ti)) </=0 i  [0…N]
System control u(t) computed with inverse
dynamic model
Problem to solve
min f  q( p, t ), u  q( p, t )  
p
c  q( p, ti ), u  q( p, ti )    0 i  1...N 


Resolution with a nonlinear optimization
algorithm
13
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Implementation on HRP-2

General architecture of the program
Definition of motion
and constraints
Motion class
Optimization algorithm (IPOPT)
k=0k=0
B-Splines, dynamic
criteriamodel
Criteria
Criteria
gradient
Iteration
k k
k=k+1 Iteration
k=k+1
Criteria gradient
Constraints
Constraints
Convergence ?
No
NoConvergence ?
Constraints gradient
Constraints
gradient
Yes
Yes
End
14
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Optimal Motion Generation

Optimal motion problematic

Minimization of any criteria



Energy consumption
Time, jerk, etc.
Constraints
Actuators’ torque, max speeds,
Joint limits…
 Collision and Auto-collision
 Unilateral contact, stability


Output
High performance desired
motion with constraint satisfaction


Tool


Development of a software framework
A unified constraint definition
Miossec, Yokoi, Kheddar, IEEE ROBIO, 2006
Escande, Miossec, Kheddar, IEEE/RSJ Humanoids, 2007
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Extreme tasks


Dynamic transition from one
feasible posture to another
under joint torque limitation
Combining two different
motions


accelerating an object upward
sliding the body into under the
object
Arisumi, Chardonnet, Kheddar, Yokoi, IEEE ICRA 07
Arisumi, Miossec, Chardonnet, Yokoi, IEEE/RSJ IROS 08
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Problem

Theoretical



Difficult to find a compromise between the
number of trajectory control points (optimization
variables) and sampling time
Difficult to keep a uniformed sampling when the
final time is an optimization variable
No guarantee of constraints satisfaction
between time samples

Optimization using interval analysis (Lengagne et al.)


Guarantee of constraint satisfaction
Computationally heavy
17
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Multi-contact motion optimization



Whole-body model (incl. dynamics)
Motion local planning
Play trajectories in pseudo-closed-loop
Contact sets

Optimization
Dynamic motion
… if the solutions fits within critical time

Use in a closed-loop scheme
18
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Problem (case 1)
19
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Problem (case 2)
20
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Solution (adopted)
dk+1 (Ci , Cj)
0
t
t0
t1
t2
t3
t4
t5
Tf
21
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
More about subdivisions
Lengagne, Mathieu, Kheddar, Yoshida, HUMANOIDS, 2010
Lengagne, Mathieu, Kheddar, Yoshida, IROS, 2010
22
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Video: HRP-2
Reported by the NEW SCIENTIST and
REUTERS Press Agency
23
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Other videos: HRP-2
24
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
HRP-2 emulating impaired leg
Lengagne, Kheddar, Druon, Yoshida, IEEE ROBIO, 2011
25
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Comparing motions
26
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Floating contact
27
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Avoiding collisions
q(t)
P
Motion
Parametrization
Optimization
Solver
Polynomial
Approximation
(Taylor)
T (t),R (t),v(t),ω(t)
mind (Ci , C j )
Global Minima Distance Computation
28
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
CD (no)
29
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
CD (yes)
30
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Conclusion


Optimization is a powerful tool
Yet, we need adding:






Impact
Multi-contact stabilizer
Collision avoidance
Faster solvers
Flexibility of the ankle
Current software status

Draft
31
CNRS-AIST Joint Robotics Laboratory, UMI3218/CRT (JRL)
Collaborators (on this topic)

Past members

New members (RoboHow.Cog)
32