**Stéphane Caron**

LIRMM, CNRS-UM2

Talk at Humanoids 2016, W3

November 15, 2016

https://scaron.info/slides/humanoids-2016/

In this talk...

- Interpolate a path s↦q(s)
- Find a path retiming t↦s(t)

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

Equations of motion for a fixed-base manipulator:

M(q)¨q+˙q⊤C(q)˙q+g(q)=τsubject to torque constraints |τ|≤τmax.

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

˙q=qs˙s¨q=qs¨s+qss˙s2τ=m(s)¨s+c(s)˙s2+g(s)Torque constraints can then be written as a(s)¨s+b(s)˙s2+c(s)≤0.

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

a(s)¨s+b(s)˙s2+c(s)≤0At 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].

Source: CRI group @ NTU.

The equations of motion for a humanoid are:

M(q)¨q+˙q⊤C(q)˙q+g(q)=S⊤τ+∑J⊤i(q)fiwith separate constraints:

- torque limits: |τ|≤τmax
- linearized friction cones: Aifi≤0

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

Solution found by [Hauser, IJRR 2014]:

- Eliminate torque variables: τ(s,˙s,¨s)=m(s)¨s+c(s)˙s2+g(s)−∑J⊤i(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+c≤0

Figure adapted from [Hauser, IJRR 2014].

(¨s,˙s2)∈P ⇔ ∃F, (∀i, Aifi≤0) and B[¨s ˙s2 F]⊤≤d
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?

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%.

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

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

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

Friction cone of resultant contact wrench.

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)

Multi-contact ZMP support areas

[Caron et al., TRO 2016]

COM acceleration support volume

[Caron et al., Humanoids 2016]

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

Ongoing work with Quang-Cuong Pham.

Use TOPP as dynamics filter in a predictive controller.

Step timings are **computed automatically**.

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

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

A **fast** dynamics filter.

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

Low-dimensional model for **replanning** and faster computations.

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