Time-Optimal Path Param.: a tool for Humanoid Motion Planning and Predictive Control

Stéphane Caron
Talk at Humanoids 2016, W3
November 15, 2016

In this talk...

Time-Optimal Retiming

Kino-dynamic process

  1. Interpolate a path sq(s)
  2. Find a path retiming ts(t)


Not retimed



It is not for time optimality that we use time-optimal retiming.


(for starters)

Full dynamics

Equations of motion for a fixed-base manipulator:


subject to torque constraints |τ|τmax.

Path-tracking dynamics

The robot tracks the path sq(s) retimed as s(t):


Torque constraints can then be written as  a(s)¨s+b(s)˙s2+c(s)0.

Time-optimal solutions

TOPP applies to all systems for which path-tracking constraints can be written:


Pontryagin's maximum principle

At each s(t) of the retimed trajectory:

  • either ¨s is maximal: ¨s=β(s,˙s)
  • or ¨s is minimal: ¨s=α(s,˙s)

We know how to enumerate switch points where the optimal solution alternates between α and β [Pham, TRO 2014].

Phase Space

Waiter motion

Source: CRI group @ NTU.

Whole-body Optimization

Whole-body dynamics

The equations of motion for a humanoid are:


with separate constraints:

  • torque limits:  |τ|τmax
  • linearized friction cones:  Aifi0


How to write them as a(s)¨s+b(s)˙s2+c(s)0?

Tour de force

Solution found by [Hauser, IJRR 2014]:

  • Eliminate torque variables:
  • τ(s,˙s,¨s)=m(s)¨s+c(s)˙s2+g(s)Ji(s)fi
  • Constraints are now a polytope  B[¨s ˙s2 F]d, where F stacks contact forces.
  • Project all force variables [Bretl and Lall, TRO 2008] to get a polygon: a¨s+b˙s2+c0

Figure adapted from [Hauser, IJRR 2014].

(¨s,˙s2)P    F, (i, Aifi0) and B[¨s ˙s2 F]d

Certificate of feasibility

Forces are not explicitely computed by the optimizer.

Yet, we have the guarantee that the controller can find feasible forces at run time.


[Pham & Stasse, TMECH 2015] improved the algorithm for numerical TOPP.

Source: CRI group @ NTU.

Time to run TOPP on N=100 discretization steps: 5 ms
versus 2.5 s with non-linear optimization (SLP).

How can a “time optimal” motion
be so slow?

Answer: Robustness

All dynamic constraints are scaled:

  • Torques: |τ|  γττmax
  • Friction: |fti|  γfμfzi
  • Area of surface contacts scaled by γs

where γτ,γf,γs(0,1), typically 50%.


Question 1

Most computation time (1 s) is now spent computing a(s),b(s),c(s).

Question 2

What to update in a joint-angle path q(s) when there is no solution?

Reduced-model Optim.

Contact Wrench Cone


In multi-contact, frictional constraints are way more critical than torque limits.


Friction cone of resultant contact wrench.

Reduced model

Center of mass + Angular momentum.


We don't know how to interpolate the angular momentum, therefore:


  • Whent it fails: update the COM path.
  • Can we avoid the IK step for the angular momentum?
    (e.g. using the Linear Pendulum Mode where ˙LG=0)

Reduced-model Support Polytopes

Multi-contact ZMP support areas
[Caron et al., TRO 2016]

COM acceleration support volume
[Caron et al., Humanoids 2016]

Predictive Control

Step timings

Fixed step timings do not work any more in multi-contact, where proper timings depend on terrain topology.

Multi-contact MPC on 3D COM


Ongoing work with Quang-Cuong Pham.


Use TOPP as dynamics filter in a predictive controller.

Main benefit

Step timings are computed automatically.

Contact switch in preview window

Parameterize foot trajectory, feed back step timing to COM optimization.


Continuous interpolation/retiming loop running at 20-30 Hz.


Concluding remarks

Time-optimal retiming

A fast dynamics filter.

Whole-body TOPP

Core idea of polytope projection: force/torques are not explicitly computed.

Reduced-model TOPP

Low-dimensional model for replanning and faster computations.

Predictive control

Nonlinear optimization running at 20-30 Hz in the control loop.