Perspectives on Motion Planning and Control for Humanoid Robots in Multi-contact Scenarios
Stéphane Caron
Nakamura-Takano Laboratory
Seminar @ NTU
March 7^{th}, 2016
Stéphane Caron
Nakamura-Takano Laboratory
Seminar @ NTU
March 7^{th}, 2016
Autonomous planning and execution of motions on a humanoid robot.
Robots implement the perception-planning-action loop:
Current implementations are closer to the following:
Assuming good trajectory tracking is a holonomic approximation, but humanoids are subject to a nonholonomic constraint. Nonholonomy can be addressed with a continuous replanner updating the reference trajectory in real time.
Problem: find a trajectory from the robot's current state to some goal state.
Trajectories t↦q(t) in the configuration space C are paths in the state-space X.
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
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.
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) }
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.
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.
Completeness proofs formalize the requirements of a motion planner, e.g. full actuation, minimum obstacle clearance, etc.
Both PRM and RRT are probabilistically complete for kinematic path planning.
Let γ:[0,L]→F be a path of (Euclidean) length L, with γ(0)=a, γ(L)=b and let R=inf0≤t≤Lr(γ(t)) be the distance of the path of the obstacles. Then the probability that [PRM] will fail to connect the points a and b is at most 2LR(1−αR2)N, where α=π/(4|F|).
The RRT-Connect algorithm is probabilistically complete and vertices [of the roadmap] converge to a uniform distribution over Cfree.
Dynamic systems are constrained in two ways: ^{(Donald et al., 1993)}
˙x=f(x,u)⇔M(q)¨q+˙q⊤C(q)˙q+g(q)=τ+J⊤fKinematic 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.
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. |
The steering function is the local planner that connects two configurations x1 and x2 by a trajectory x:[0,T]→C with x(0)=x1 and x(T)=x2.
A perfect solution can be found mathematically, as e.g. for Reeds and Shepp curves used in non-holonomic planners. ^{ (Laumond et al., 1998) }
Control functions u(t) are sampled and their response simulated by forward dynamics. ^{ (LaValle, 2001) } Applies to a wide range of systems, but not humanoids.
A state trajectory q:[0,T]→C is interpolated, then tracked by inverse dynamics. Commonly used in humanoid motion planning.
Consider a time-invariant differential system with Lipschitz-smooth dynamics f and full actuation. Suppose that the kinodynamic planning problem between two states xinit and xgoal admits a smooth solution γ:[0,T]→C with δ-clearance in control space. A randomized motion planner using a acceleration-compliant interpolation function is probabilistically complete.
When x′→x, the interpolation from x to x′ stays within a neighborhood of x and its accelerations converges to ∥˙q∥∥Δq∥∥Δ˙q∥, the discrete acceleration encoded in Δx.
Pendulum with low torques |τ|≤5 Nm.
Requires multiple swings in order to lift up.
X:={x=(q,˙q),q∈C,˙q∈Rn}.
The state space X has:
Also: never been used for humanoid motion planning.
Acceleration-compliant interpolation functions are not straightforward
to find for multi-DOF systems.
Followed e.g. by (Kuffner et al., 2002), yet with limitation to quasi-static trajectories (no dynamic motion).
Converts a path q(s) into a feasible trajectory q(s(t)) with minimum duration T.
Direct-integration method proposed in (Bobrow et al., 1985).
How to go through contact switches dynamically, i.e. without stopping at quasi-static configurations.
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.
Limiting curves in the phase-space (s,˙s) of a path q(s).
The AVP-RRT planner separates path and velocity planning thanks to AVP.
We extend existing TOPP methods to find the interval of reachable velocities along a path.
(q(s),[vmin,vmax]init)AVP−−−→[vmin,vmax]endIntegration with a configuration-space RRT where nodes are augmented with velocity intervals (q,[vmin,vmax]).
AVP-RRT is (almost) probabilistically complete.
Double-pendulum with low torques.
Requires multiple swings in order to lift up.
The contact force fi at the contact (Ci,ni) has two components:
Each surface-to-surface contact (Ci,ni) yields friction, represented by a friction coefficient μi and its corresponding friction cone.
Contact forces fi should lie inside their friction cones to avoid transitions towards other contact modes (e.g. sliding).
Friction can be equivalently expressed by a Contact Wrench Cone (CWC):
We derived the analytical formula of the CWC for rectangular contact surfaces (foot shape of current humanoids). There is therefore no additional cost in using this minimal, non-redundant representation.
The resultant force f lies in a friction cone: |fx|,|fy|≤μfz ∧ fz>0.
Keeps the COP in the contact polygon: |τx|≤Yfz ∧ |τy|≤Xfz.
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.
The CWC enables the use of TOPP. Below are two retimings of the same input:
Contact forces and the centroidal momentumof the system are bound by the relation:
[m¨xG˙LG]=[mg0]+∑contacti[fi−−→GCi×fi]
See e.g.
(Wieber, 2008)
for a proof of this result.
It derives from the assumption that
internal
forces are conservative (d'Alembert's principle).
The previous equation of motion rewrites to
[fgiτgiO]:=−∑contacti[fi−−→GCi×fi]That is to say, wgiO=AgiOf, where wgi is the gravito-inertial wrench. Meanwhile, four-sided linearized friction cones can be written:
⎡⎢ ⎢ ⎢ ⎢ ⎢⎣+√20−μ−√20−μ0+√2−μ0−√2−μ⎤⎥ ⎥ ⎥ ⎥ ⎥⎦R⊤ifi≤0Question: what is the image of inequalities Bf≤0 by the linear mapping w=Af?
For a subset P of Rd, the following statements are equivalent:
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.
Libraries such as cdd (C++) and pycddlib (Python) allow for efficient conversion between H- and V-representations. In particular, for cones:
On top of this derivation, we applied the GIWC to two problems:
Statically stable postures that can resist bounded disturbance forces.
Combine the GIW with Time-Optimal Path Parameterization (TOPP library) to generate dynamic motions.
Previous definition: the point on the floor where the moment of the gravito-inertial wrench is vertical. ^{ (Sardain & Bessonnet, 2004) }
If the motion is dynamically balanced by valid contact forces, the ZMP lies in the convex hull of ground contact points.
The resultant moment is not zero at the ZMP (it is aligned with the ground normal).
Regulating ˙LG=0, ¨xG=ω2(xZ−xG), where ω:=√g+¨zGzG−zZ.
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×τOn⋅f+xO.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.
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.
Vertices are located at the intersection of the plane with friction cones (= contact points on horizontal floor, but not in general).
xZi:=n×τO,in⋅fi+xO=n×(−−→OCi×fi)n⋅fi+xO,We will sort these vertices into two polygons:
If one of the two polygons P+ or P− is empty, S is equal to the other.
Let D:=P+−P−=conv({r1,…,rk}) denote the vertices of the Minkowski difference. The support area S is the reunion of two polygonal cones: C+=P++∑iR+ri and C−=P−+∑iR+(−ri). In particular, when P+∩P− has non-empty interior, S covers the whole plane Π(O,n).
Now, we know how to compute the ZMP support area in any virtual plane, including those above the robot. With the latter, we get a:
The control law equation becomes ¨xG=ω2(xG−xZ), where this time ω:=√g+¨zGzG−zZ.
Even on a horizontal floor, the support area is smaller than the convex hull of ground contact points.
The linear pendulum (LP or LIP) model assumes that ˙L=0, i.e., the angular momentum is regulated to a constant value.
Previous works also assume ¨zG=0, i.e., the COM is regulated to lie in a plane.
Although it was previously unnoticed, these regulations constraint the GIWC, and therefore shrink the ZMP support area.
A novel algorithm based on the double-description method to compute the pendular support area (after shrinking due to ˙L=0 and ¨zG=0).
Integration within a COM-ZMP trajectory generator and validation in simulations.
Contact-feasible whole-body multi-contact locomotion across uneven terrain.
The steering idea of putting motion planning into the fast feedback loop took us in a journey through:
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!
Stéphane Caron – March 7^{th}, 2016