# Motivation

## Scope

Autonomous planning and execution of motions on a humanoid robot.

## Challenges

• Perception has low rate update loops
• Planning in space and time
• Control under kinodynamic constraints:
• etc.

# Kinematic Motion Planning

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

## Idea

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.

## Approach

• Initialization:
• G=({qinit},)
• Extension:
• Generate a new node qC
• Try to connect q to some qV
• Termination (optional):
• Try to connect a target qgoal

(Source: Eric O. Scott)

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

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)

# Completeness

## Completeness

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)

˙x=f(x,u)M(q)¨q+˙qC(q)˙q+g(q)=τ+Jf
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

## System

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

# 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:

τz[τmin,τmax]τmin:=μ(X+Y)fz+|Yfxμτx|+|Xfyμτy|,τmax:=+μ(X+Y)fz|Yfx+μτx||Xfy+μτy|.

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

# Bits of Convex Polyhedra Theory

## Definitions

• Minkowski sum of two sets:  P+Q={p+q:pP and qQ}.
• Convex hull:  conv(v1,,vn)=iαivi where i,αi>0 and iαi=1.
• Positive span:  nonneg(r1,,rs)=iλiri where i,λi>0.

## 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:

• BS: span matrix of the cone:  Bx0x=BSλ,λ>0
• AF: face matrix of the cone:  x=Aλ,λ>0AFx0

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

• Dynamic equilibrium wgiO=AgiOf and frictional constraints Bf0
• Conversion to span form:  f=BSλ,λ>0
• V-representation of the GIWC:  wgiO=AgiOBSλ,λ>0
• Finally, H-representation of the GIWC:  (AgiOBS)FwgiO0

# 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

## Limitations

• The ground ZMP can only be used for locomotion on horizontal floors
• It accounts for pressure (pi>0) but not for friction limits (ftiμipi)

## 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:

xZ=n×τOnf+xO.

## 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).

xZi:=n×τO,infi+xO=n×(OCi×fi)nfi+xO,

We will sort these vertices into two polygons:

• positive (virtual) pressure P+={xZi | (nfi)>0}, and
• negative (virtual) pressure P={xZi | (nfi)<0}.

# Journey

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?