Inverse Dynamics Trajectory Optimization for Contact-Implicit Model Predictive Control

Robots must make and break contact to interact with the world and perform useful tasks. However, planning and control through contact remains a formidable challenge. In this work, we achieve real-time contact-implicit model predictive control with a surprisingly simple method: inverse dynamics trajectory optimization. While trajectory optimization with inverse dynamics is not new, we introduce a series of incremental innovations that collectively enable fast model predictive control on a variety of challenging manipulation and locomotion tasks. We implement these innovations in an open-source solver, and present a variety of simulation examples to support the effectiveness of the proposed approach. Additionally, we demonstrate contact-implicit model predictive control on hardware at over 100 Hz for a 20 degree-of-freedom bi-manual manipulation task.

Temporal Logic Motion Planning with Convex Optimization via Graphs of Convex Sets

Temporal logic is a concise way of specifying complex tasks. But motion planning to achieve temporal logic specifications is difficult, and existing methods struggle to scale to complex specifications and high-dimensional system dynamics. In this paper, we cast Linear Temporal Logic (LTL) motion planning as a shortest path problem in a Graph of Convex Sets (GCS) and solve it with convex optimization. This approach brings together the best of modern optimization-based temporal logic planners and older automata-theoretic methods, addressing the limitations of each: paths are represented with continuous Bezier curves, avoiding clipping and pass-through; computational complexity is polynomial (not exponential) in the number of sample points; global optimality can be certified; soundness and completeness are guaranteed under mild assumptions; and most importantly, the method scales to complex specifications and high-dimensional systems, including a 30-DoF humanoid. Open-source code is available at https://github.com/vincekurtz/ltl_gcs.

Mixed-Integer Programming for Signal Temporal Logic with Fewer Binary Variables

Signal Temporal Logic (STL) provides a convenient way of encoding complex control objectives for robotic and cyber-physical systems. The state-of-the-art in trajectory synthesis for STL is based on Mixed-Integer Convex Programming (MICP). The MICP approach is sound and complete, but has limited scalability due to exponential complexity in the number of binary variables. In this letter, we propose a more efficient MICP encoding for STL. Our new encoding is based on the insight that disjunction can be encoded using a logarithmic number of binary variables and conjunction can be encoded without binary variables. We demonstrate in simulation examples that our proposed approach significantly outperforms the state-of-the-art for long and complex specifications. Open-source software is available at https://stlpy.readthedocs.io .

Robust Approximate Simulation for Hierarchical Control of Piecewise Affine Systems under Bounded Disturbances

Piecewise affine (PWA) systems are widely applied in many practical cases such as the control of nonlinear systems and hybrid dynamics. However, most of the existing PWA control methods have poor scalability with respect to the number of modes and system dimensions and may not be robust to the disturbances in performance. In this paper, we present a robust approximate simulation based control method for PWA systems under bounded external disturbances. First, a lower-dimensional linear system (abstraction) and an associated interface are designed to enable the output of the PWA system (concrete system) to track the output of the abstraction. Then, a Lyapunov-like simulation function is designed to show the boundedness of the output errors between the two systems. Furthermore, the results obtained for linear abstraction are extended to the case that a simpler PWA system is the abstraction. To illustrate the effectiveness of the proposed approach, simulation results are provided for two design examples.

Contact-Implicit Trajectory Optimization with Hydroelastic Contact and iLQR

Contact-implicit trajectory optimization offers an appealing method of automatically generating complex and contact-rich behaviors for robot manipulation and locomotion. The scalability of such techniques has been limited, however, by the challenge of ensuring both numerical reliability and physical realism. In this paper, we present preliminary results suggesting that the Iterative Linear Quadratic Regulator (iLQR) algorithm together with the recently proposed pressure-field-based hydroelastic contact model enables reliable and physically realistic trajectory optimization through contact. We use this approach synthesize contact-rich behaviors like quadruped locomotion and whole-arm manipulation. Furthermore, open-loop playback on a Kinova Gen3 robot arm demonstrates the physical accuracy of the whole-arm manipulation trajectories. Code is available at https://bit.ly/ilqr_hc and videos can be found at https://youtu.be/IqxJKbM8_ms .

Control Barrier Functions for Singularity Avoidance in Passivity-Based Manipulator Control

Task-space Passivity-Based Control (PBC) for manipulation has numerous appealing properties, including robustness to modeling error and safety for human-robot interaction. Existing methods perform poorly in singular configurations, however, such as when all the robot's joints are fully extended. Additionally, standard methods for constrained task-space PBC guarantee passivity only when constraints are not active. We propose a convex-optimization-based control scheme that provides guarantees of singularity avoidance, passivity, and feasibility. This work paves the way for PBC with passivity guarantees under other types of constraints as well, including joint limits and contact/friction constraints. The proposed methods are validated in simulation experiments on a 7 degree-of-freedom manipulator.

Mini Cheetah, the Falling Cat: A Case Study in Machine Learning and Trajectory Optimization for Robot Acrobatics

Seemingly in defiance of basic physics, cats consistently land on their feet after falling. In this paper, we design a controller that lands the Mini Cheetah quadruped robot on its feet as well. Specifically, we explore how trajectory optimization and machine learning can work together to enable highly dynamic bioinspired behaviors. We find that a reflex approach, in which a neural network learns entire state trajectories, outperforms a policy approach, in which a neural network learns a mapping from states to control inputs. We validate our proposed controller in both simulation and hardware experiments, and are able to land the robot on its feet from falls with initial pitch angles between -90 and 90 degrees.

Trajectory Optimization for High-Dimensional Nonlinear Systems under STL Specifications

Signal Temporal Logic (STL) has gained popularity in recent years as a specification language for cyber-physical systems, especially in robotics. Beyond being expressive and easy to understand, STL is appealing because the synthesis problem---generating a trajectory that satisfies a given specification---can be formulated as a trajectory optimization problem. Unfortunately, the associated cost function is nonsmooth and non-convex. As a result, existing synthesis methods scale poorly to high-dimensional nonlinear systems. In this letter, we present a new trajectory optimization approach for STL synthesis based on Differential Dynamic Programming (DDP). It is well known that DDP scales well to extremely high-dimensional nonlinear systems like robotic quadrupeds and humanoids: we show that these advantages can be harnessed for STL synthesis. We prove the soundness of our proposed approach, demonstrate order-of-magnitude speed improvements over the state-of-the-art on several benchmark problems, and demonstrate the scalability of our approach to the full nonlinear dynamics of a 7 degree-of-freedom robot arm.

Automatic Trajectory Synthesis for Real-Time Temporal Logic

Many safety-critical systems must achieve high-level task specifications with guaranteed safety and correctness. Much recent progress towards this goal has been made through controller synthesis from temporal logic specifications. Existing approaches, however, have been limited to relatively short and simple specifications. Furthermore, existing methods either consider some prior discretization of the state-space, deal only with a convex fragment of temporal logic, or are not provably complete. We propose a scalable, provably complete algorithm that synthesizes continuous trajectories to satisfy non-convex \gls*{rtl} specifications. We separate discrete task planning and continuous motion planning on-the-fly and harness highly efficient boolean satisfiability (SAT) and \gls*{lp} solvers to find dynamically feasible trajectories that satisfy non-convex \gls*{rtl} specifications for high dimensional systems. The proposed design algorithms are proven sound and complete, and simulation results demonstrate our approach's scalability.

Approximate Simulation for Template-Based Whole-Body Control

Reduced-order template models are widely used to control high degree-of-freedom legged robots, but existing methods for template-based whole-body control rely heavily on heuristics and often suffer from robustness issues. In this letter, we propose a template-based whole-body control method grounded in the formal framework of approximate simulation. Our central contribution is to demonstrate how the Hamiltonian structure of rigid-body dynamics can be exploited to establish approximate simulation for a high-dimensional nonlinear system. The resulting controller is passive, more robust to push disturbances, uneven terrain, and modeling errors than standard QP-based methods, and naturally enables high center of mass walking. Our theoretical results are supported by simulation experiments with a 30 degree-of-freedom Valkyrie humanoid model.