Provably Feasible and Stable White-Box Trajectory Optimization

TL;DR

This work proposes a white-box TO solver, where the SQP solver is informed with characteristics of the objective function and the dynamic system and uses these characteristics to derive approximate dynamic systems and customize the discretization schemes.

Abstract

We study the problem of Trajectory Optimization (TO) for a general class of stiff and constrained dynamic systems. We establish a set of mild assumptions, under which we show that TO converges numerically stably to a locally optimal and feasible solution up to arbitrary user-specified error tolerance. Our key observation is that all prior works use SQP as a black-box solver, where a TO problem is formulated as a Nonlinear Program (NLP) and the underlying SQP solver is not allowed to modify the NLP. Instead, we propose a white-box TO solver, where the SQP solver is informed with characteristics of the objective function and the dynamic system. It then uses these characteristics to derive approximate dynamic systems and customize the discretization schemes.

Figures (5)

• Figure 1: The sketch of convergence proof for our white-box TO solver, built on top of the SQP solver framework. Key to the convergence of SQP to a feasible solution lies in the satisfaction of LICQ (a). We use two techniques to ensure the satisfaction of LICQ. First, we approximate the potential energy (b) and introduce penalty functions for hard constraints, leading to an approximate TO problem. Second, we introduce an adaptive timestep subdivision scheme (c). The main goal of our approximation is to regularize the unbounded curvature (d) and derive modified potential energies and penalty functions with bounded curvature (e, red curve). A typical strategy to remove such unbounded curvature is to introduce piecewise functions that keep the energy unmodified in a bounded domain (f) and replace the energy with a smoother model in the unbounded domain. After SQP returns a feasible solution satisfying the $\epsilon$-perturbed KKT condition to the approximate TO (g), we show that the energy level of the generated feasible solution falls inside the bounded, unmodified domain of our approximate energy (f). Therefore, it satisfies the $\epsilon$-perturbed KKT condition of the original TO problem.
• Figure 2: Our penalty function for the equality constraint ($\bar{h}_j^e$ on the left) and the inequality constraint ($\bar{h}_j^i$ on the right) ensures their sufficient satisfaction.
• Figure 3: Consider a robot arm dragging a deformable cushion through an elastic string. To model such a scenario, our dynamic system needs to model the $\mathcal{SE}(3)$ constraint for each robot link (a) (sec:rotation) and hinge joint (b) (sec:limit). The elastic string can be discreted as a mass-spring system (c), using one spring between each pair of red vertices (sec:elastic). And the deforamble cushion can be be modeled using volumetric hyper-elastic energy, such the Saint Venant–Kirchhoff elastic energy (d), using one energy term for each red triangle (sec:elastic). Finally, we need to handle the collisions and contacts (e) between the cushion and the ground (sec:Contact). In our extended paper, we show that all these energy models have curvature-bounded relaxation.
• Figure 4: Illustration of the collision penalty using a simple 2D example with a circle and a point. Our penalty smooths the non-differentiable collision constraint. Left: the non-differentiable collision constraint $h^i_j$ formulated as the distance (red dashed line). Middle: the corresponding collision penalty value $\bar{h}^i_j$. Right: Our theoretical framework can scale to arbitrarily complex geometries via sphere sampling (blue circles).
• Figure 5: Illustration of the friction model with an example of a point contacting a flat surface in 2D (top left). The exact friction force magnitude (middle) changes abruptly with velocity. Our modified friction force (right) is sufficiently smooth. We further introduce the function $P_6$ that attenuates the force magnitude when velocity is sufficiently large, which is essential for our analysis.

Theorems & Definitions (44)

• remark 1
• Theorem 1: Informal
• remark 2
• remark 3
• lemma 1
• lemma 2
• corollary 1
• lemma 3
• lemma 4
• lemma 5: Constraint-Convergence