Exploiting structures of trajectory optimization for efficient optimal motion planning

Trajectory optimization is an important tool for optimal motion planning due to its flexibility in cost design, capability to handle complex constraints, and optimality certification. It has been widely used in robotic applications such as autonomous vehicles, unmanned aerial vehicles, humanoid robots, and highly agile robots. However, practical robotic applications often possess nonlinear dynamics and non-convex constraints and cost functions, which makes the trajectory optimization problem usually difficult to be efficiently solved to global optimum. The long computation time, possibility of non-convergence, and existence of local optima impose significant challenges to applying trajectory optimization in reactive tasks with requirements of real-time replanning. In this thesis, two structures of optimization problems are exploited to significantly improve the efficiency, i.e. computation time, reliability, i.e. success rate, and optimality, i.e. quality of the solution. The first structure is the existence of a convex sub-problem, i.e. the problem becomes convex if a subset of optimization variables is fixed and removed from the optimization. This structure exists in a wide variety of problems, especially where decomposition of spatial and temporal variables may result in convex sub-problem. A bilevel optimization framework is proposed that optimizes the subset and its complement hierarchically where the upper level optimizes the subset with convex constraints and the lower level uses convex optimization to solve its complement. The key is to use the solution of the lower level problem to compute analytic gradients for the upper-level problem. The bilevel framework is reliable due to its convex lower problem, efficient due to its simple upper problem, and yields better solutions than alternatives, although the existing requirement of convex sub-problem is generally too strict for many applications. The second structure is the local continuity of the argmin function for parametric optimization problems which map from problem parameters to the corresponding optimal solutions. The argmin function can be approximated from data which is collected offline by sampling the problem parameters and solving them to optima. Three approaches are proposed to learn the argmin from data each suited best for distinct applications. The nearest-neighbor optimal control searches problems with similar parameters and uses their solution to initialize nonlinear optimization. For problems with globally continuous argmin, neural networks can be used to learn from data and a few steps of convex optimization can further improve their predictions. As for problems with discontinuous argmin, mixture of experts (MoE) models are used. The MoE contains several experts and a classifier and is trained by splitting the data first according to discontinuity of argmin and then training each expert independently. Both empirical $k$-Means and theoretical topological data analysis approaches are explored for discontinuity identification and finding suitable data splits. Both methods result in data splits that help train MoE models that outperform the discontinuity-agnostic learning pipeline using standard neural networks. The trajectory learning approach is efficient since it only requires model evaluation to compute a trajectory, reliable since the MoE model is accurate after correctly handling discontinuity, and optimal since the data are collected offline and solved to optimal. Moreover, this local continuity structure is less restrictive and exists for a wide range of non-degenerate problems. The exploitation of these two structures helps build an efficient optimal motion planner with high reliability

FRoGGeR: Fast Robust Grasp Generation via the Min-Weight Metric

Many approaches to grasp synthesis optimize analytic quality metrics that measure grasp robustness based on finger placements and local surface geometry. However, generating feasible dexterous grasps by optimizing these metrics is slow, often taking minutes. To address this issue, this paper presents FRoGGeR: a method that quickly generates robust precision grasps using the min-weight metric, a novel, almost-everywhere differentiable approximation of the classical epsilon grasp metric. The min-weight metric is simple and interpretable, provides a reasonable measure of grasp robustness, and admits numerically efficient gradients for smooth optimization. We leverage these properties to rapidly synthesize collision-free robust grasps - typically in less than a second. FRoGGeR can refine the candidate grasps generated by other methods (heuristic, data-driven, etc.) and is compatible with many object representations (SDFs, meshes, etc.). We study FRoGGeR's performance on over 40 objects drawn from the YCB dataset, outperforming a competitive baseline in computation time, feasibility rate of grasp synthesis, and picking success in simulation. We conclude that FRoGGeR is fast: it has a median synthesis time of 0.834s over hundreds of experiments.Comment: Accepted at IROS 2023. The arXiv version contains the appendix, which does not appear in the conference versio

Optimal Control for Kinematic Bicycle Model with Continuous-time Safety Guarantees: A Sequential Second-order Cone Programming Approach

The optimal control problem for the kinematic bicycle model is considered where the trajectories are required to satisfy the safety constraints in the continuous-time sense. Based on the differential flatness property of the model, necessary and sufficient conditions in the flat space are provided to guarantee safety in the state space. The optimal control problem is relaxed to the problem of solving three second-order cone programs (SOCPs) sequentially, which find the safe path, the trajectory duration, and the speed profile, respectively. Solutions of the three SOCPs together provide a sub-optimal solution to the original optimal control problem. Simulation examples and comparisons with state-of-the-art optimal control solvers are presented to demonstrate the effectiveness of the proposed approach.Comment: 8 pages, 5 figure

SL1M: Sparse L1-norm Minimization for contact planning on uneven terrain

International audienceOne of the main challenges of planning legged locomotion in complex environments is the combinatorial contact selection problem. Recent contributions propose to use integer variables to represent which contact surface is selected, and then to rely on modern mixed-integer (MI) optimization solvers to handle this combinatorial issue. To reduce the computational cost of MI, we exploit the sparsity properties of L1 norm minimization techniques to relax the contact planning problem into a feasibility linear program. Our approach accounts for kinematic reachability of the center of mass (COM) and of the contact effectors. We ensure the existence of a quasi-static COM trajectory by restricting our plan to quasi-flat contacts. For planning 10 steps with less than 10 potential contact surfaces for each phase, our approach is 50 to 100 times faster that its MI counterpart, which suggests potential applications for online contact re-planning. The method is demonstrated in simulation with the humanoid robots HRP-2 and Talos over various scenarios