Autonomous planning and execution of motions on a humanoid robot.


How to attack the problem

Kinematic Motion Planning

(Adapted from: Eric O. Scott)

Historically, kinematic systems were considered first. (Latombe, 1991)

Kinematic constraints
Apply to configurations q
Joint limits |q|qmax
Collision avoidance dobs(q)>ϵ
Maintain contacts f(q)=0

Kinematic planning is done in the configuration space C of the robot.

The main challenge is collision avoidance

Roadmap Planners


Build a roadmap (graph) G=(V,E) over the configuration space C of the robots. Nodes are configurations V={q1,q2,} and edges represent paths.


Stochastic Roadmap Planners

(Source: Eric O. Scott)

A roadmap planner turns a local planner into a global one, enforcing additional constraints such as collision avoidance.

Probabilistic Roadmaps (PRM)

Multiple-query model: grow the roadmap uniformly at random, connect pairs of configurations using graph search and the local planner. (Kavraki et al., 1996)

Rapidly-exploring Random Trees (RRT)

Single-query model: try to connect the initial configuration to the goal as fast as possible. (LaValle et al., 1998)



The planner can tell in finite time whether there are solutions, and if so find one.

Probabilistic completeness

If there are solution, the planner will find one given enough computation time:

P(solution found after N extensions)N−−−1.

Unless the planner finds a solution, it is impossible to distinguish the cases where (1) more extensions are required, or (2) the problem has no solution.

A correctness guarantee

Completeness proofs formalize the requirements of a motion planner, e.g. full actuation, minimum obstacle clearance, etc.

Kinodynamic Planning

Dynamic systems are constrained in two ways: (Donald et al., 1993)

Kinematic constraints Dynamic constraints
Apply to configurations q Include time-derivatives ˙q and ¨q
Joint limits |q|qmax Velocity limits: |˙q|˙qmax
Collision avoidance (self, obstacles) Torque limits: |τ(q,˙q,¨q)|τmax
Maintain contacts (position) Maintain contacts (friction)

To take all into account, kinodynamic planning is usually done in the state space X, which is by nature more complex than C.

Completeness of Kinodyn. Planners

Yes No
(LaValle and Kuffner, 2001): RRT is probabilistically complete for kinodynamic planning. (Kunz and Stilman, 2015): RRT with fixed time step and best-input extension is not prob. complete.
(Hsu et al., 2002): PRM is prob. complete in (α,β)-expansive state spaces when α>0. (Hsu et al., 1997): checking if α>0 is as hard as solving the planning problem itself.
(Papadopoulos et al., 2014): RRT with random piecewise-constant controls is probabilistically complete. (Caron et al., 2014): RRT with fixed time step and Bezier curve interpolation is is not prob. complete.
(Caron et al., 2014): RRT with acceleration-compliant interpolation is probabilistically complete. Source of the confusion: need to specify both planner and extension functions.

Comparison in simulations


Pendulum with low torques |τ|5 Nm.
Requires multiple swings in order to lift up.


Acceleration-compliant interpolation
Bezier curves with fixed time step

Initial limits of TOPP

Switching contact dynamics

How to go through contact switches dynamically, i.e. without stopping at quasi-static configurations.

Integration with motion planning

Retiming after planning is highly inefficient. For example, if the path is dynamically unfeasible 5% of the start, the remaining 95% of path planning computations were unnecessary.

Structure of the CWC

Friction Cone (4 inequalities)

The resultant force f lies in a friction cone:  |fx|,|fy|μfz  fz>0.

Center of pressure (4 inequalities)

Keeps the COP in the contact polygon:  |τx|Yfz  |τy|Xfz.

Yaw constraints (8 inequalities)

An original discovery of our study:


Corroborates works (Cisneros et al., 2014) on foot yaw regulation for humanoids.

Bits of Convex Polyhedra Theory


Theorem (Minkowski-Weyl)

For a subset P of Rd, the following statements are equivalent:

  1. P is a polyhedron:  P={x:Axb} for ARm×d and bRm
  2. There are finite real vectors v1,v2,,vn and r1,r2,,rs in Rd such that P=conv(v1,v2,,vn)+nonneg(r1,r2,,rs).

Thus, every polyhedron has two representations of type (1) and (2), known as (halfspace) H-representation and (vertex) V-representation, respectively.

Adapted from the Polyhedral Computation FAQ.

Friction Cone Duality

Double-description method (Fukuda, 1996)

Libraries such as cdd (C++) and pycddlib (Python) allow for efficient conversion between H- and V-representations. In particular, for cones:

Derivation of the Gravito-inertial Wrench Cone (GIWC) (Qiu et al., 2011)

App. to Statics and Dynamics (RSS 2015)

On top of this derivation, we applied the GIWC to two problems:

Statics: robust equilibrium

Statically stable postures that can resist bounded disturbance forces.

Dynamics: trajectory retiming

Combine the GIW with Time-Optimal Path Parameterization (TOPP library) to generate dynamic motions.

ZMP of a Wrench


ZMP of a wrench (existing definition)

The ZMP is actually defined (Sardain & Bessonnet, 2004) for any wrench w=(f,τ) in any virtual plane Π(O,n). The ZMP ZΠ(O,n) is the point such that n×τZ=0:


Full support area (new definition)

The full support area S of the ZMP in the plane Π(O,n) is the image of the complementary wrench cone by the equation above.

Vertices of the Full Support Area

Previous intuitions

Previous works (Sugihara et al., 2002) (Popovic et al., 2005) (Harada et al., 2006) tried to extend the definition as convex hull of contact point, thus looking for convex polyhedra.

Our approach

Vertices are located at the intersection of the plane with friction cones (= contact points on horizontal floor, but not in general).


We will sort these vertices into two polygons:


The steering idea of putting motion planning into the fast feedback loop took us in a journey through:

Overall system

The components we described integrate with each other as follows:

Always a great deal of open questions. For instance, what about contact planning?
The road ahead opens to many more journeys!