Equality Constrained Diffusion for Direct Trajectory Optimization

The recent success of diffusion-based generative models in image and natural language processing has ignited interest in diffusion-based trajectory optimization for nonlinear control systems. Existing methods cannot, however, handle the nonlinear equality constraints necessary for direct trajectory optimization. As a result, diffusion-based trajectory optimizers are currently limited to shooting methods, where the nonlinear dynamics are enforced by forward rollouts. This precludes many of the benefits enjoyed by direct methods, including flexible state constraints, reduced numerical sensitivity, and easy initial guess specification. In this paper, we present a method for diffusion-based optimization with equality constraints. This allows us to perform direct trajectory optimization, enforcing dynamic feasibility with constraints rather than rollouts. To the best of our knowledge, this is the first diffusion-based optimization algorithm that supports the general nonlinear equality constraints required for direct trajectory optimization.

DROP: Dexterous Reorientation via Online Planning

Achieving human-like dexterity is a longstanding challenge in robotics, in part due to the complexity of planning and control for contact-rich systems. In reinforcement learning (RL), one popular approach has been to use massively-parallelized, domain-randomized simulations to learn a policy offline over a vast array of contact conditions, allowing robust sim-to-real transfer. Inspired by recent advances in real-time parallel simulation, this work considers instead the viability of online planning methods for contact-rich manipulation by studying the well-known in-hand cube reorientation task. We propose a simple architecture that employs a sampling-based predictive controller and vision-based pose estimator to search for contact-rich control actions online. We conduct thorough experiments to assess the real-world performance of our method, architectural design choices, and key factors for robustness, demonstrating that our simple sampled-based approach achieves performance comparable to prior RL-based works. Supplemental material: https://caltech-amber.github.io/drop.

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.