Trajectory Optimization
4.4 Contact Formulation and Complementarity ConstraintsConstraints
Following the method introduced by Posa et al. (2014), contacts were incorporated as variables in the optimization problem. In extending the method to 3D, a friction cone formulation was initially used, but this made it awkward to constrain frictional forces to oppose motion in sliding. It also introduced severe nonlinearities, which often made the solver fail to converge. Using a friction pyramid formulation, as described by Stewart & Trinkle (1996), the solver behaved much more robustly. A four-sided pyramid was used here. A value of 1 was used for the coefficient of friction.
This is somewhat higher than a realistic value, and so may make it easier to avoid slipping and allow exploitation of larger tangential friction forces, but this value was chosen for consistency with other NaturalMotion simulations, and it would be straightforward to explore other values in future.
To avoid specifying many redundant constraints, only points which were expected to make contact with the ground were included in the problem formulation. For all the examples considered here, this included four points on each foot. Other points could be added if necessary to deal with motions where other body parts such as hands or knees might contact the ground. No mechanism was included to deal with self-contacts.
The choice of contact formulation introduces complementarity constraints, i.e. con-straints of the type
xi ≥ 0 gi(x) ≥ 0
xigi(x) = 0 (4.2)
This set of constraints may also be written using the more compact notation
0 ≤ xi⊥gi(x) ≥ 0 (4.3)
For a basic model of contact forces, the first inequality may specify that the separa-tion between bodies is never negative (i.e. penetrasepara-tion does not occur), the second may specify that reaction forces may only be repulsive, and the final equality may specify that reaction forces can only apply when bodies are in contact.
Friction can also be incorporated into this framework as follows
0 ≤ y⊥cn≥ 0 0 ≤ λe + v⊥β ≥ 0
0 ≤ µcn− eTβ⊥λ ≥ 0 (4.4)
where y is the contact separation, cn the normal force, e are vectors defining the friction directions (the friction polygon corresponds to the convex hull of e), v are the components of the relative velocity of the bodies parallel to the friction directions, λ is a term related to the relative velocity, and µ is the coefficient of friction. The first complementarity constraint is similar to the basic model described above. The second and third constraints enforce that friction acts in a direction to prevent sliding and that the tangential forces satisfy the Coulomb friction model. See Stewart &
Trinkle (1996) for full details.
Complementarity constraints are difficult for solvers as the feasible region is poorly connected, with discontinuous jumps between points with non-zero xi and points with non-zero gi(x). Two common ways to deal with this issue are constraint relaxation, as used by Posa et al. (2014), and penalty methods.
In the constraint relaxation approach, the constraint is replaced with
xi ≥ 0 gi(x) ≥ 0
xigi(x) ≤ α (4.5)
for a small value of α, which is reduced towards zero as the solution progresses. Even with constraint relaxation, difficulties can still be encountered with convergence when α gets close to zero. During experiments, convergence issues were found at α values comparable to the desirable complementarity tolerance, and so potentially interfering with reaching a solution.
Other methods are based on penalty functions. In these approaches, a modified problem is considered, in which the constraint is removed altogether and replaced with an additional weighted term in the objective function.
Fukushima et al. (1998) consider a sequential quadratic programming (SQP)
ap-proach using a differentiable penalty term with a smoothing parameter to handle complementarity constraints. Rather than solve a series of problems, they describe a method to update the penalty weight and smoothing parameter after each SQP iteration.
Many solvers include an elastic mode, in which constraints are relaxed, and a penalty term is added to the objective function. Anitescu (2005) shows that the elastic mode can be used effectively for problems with complementarity constraints.
It can be shown that, for certain formulations of penalty function and sufficiently large values of the penalty weight, solutions of the original problem are also solu-tions of the modified problem (Benson et al. 2006, Wright & Nocedal 1999, Ch. 15, Ch. 17.3). A formulation with this property is referred to as an exact penalty func-tion. Choosing a suitable weight value in advance can be difficult, but it is possible to start with a moderate value and increase it in successive steps until the penalty term becomes sufficiently small.
Benson et al. (2006) describe two formulations of exact penalty functions for com-plementarity problems. In the `1 or summation formulation, a term of the form ρP xigi(x) is added to the objective function, where ρ is the penalty weight. In the `∞ or pairwise formulation, the penalty term is of the form ρ maxi{xigi(x)}.
The summation formulation described by Benson et al. (2006) was originally used for this implementation, but this led to a problem. Within the solver tolerance, it was possible for each xigi(x) in the sum to have a small negative value. The solver would succeed in driving the summed penalty term to zero, but since there might be thousands of complementarity constraints in the problem, a large number of small negative values could allow a few of the individual terms in the sum to have large positive values and hence significantly violate their constraints. Changing to a pairwise formulation eliminated this problem, and with this amendment the exact penalty method was found to be more robust than the constraint relaxation implementation for solving complementarity problems. The pairwise formulation replaces the objective function with
f (x) + ρζ
xigi(x) ≤ ζ (4.6)
where f is the original objective function, and ρ is the penalty weight. The inequality applies for each complementarity constraint, so ζ gives the maximum
complemen-tarity violation.