Contact constraints arise naturally in many robot planning problems. In recent years, a variety of contact-implicit trajectory optimization algorithms have been developed that avoid the pitfalls of mode pre-specification by simultaneously optimizing state, input, and contact force trajectories. However, their reliance on first-order integrators leads to a linear tradeoff between optimization problem size and plan accuracy. To address this limitation, we propose a new family of trajectory optimization algorithms that leverage ideas from discrete variational mechanics to derive higher-order generalizations of the classic time-stepping method of Stewart and Trinkle. By using these dynamics formulations as constraints in direct trajectory optimization algorithms, it is possible to perform contact-implicit trajectory optimization with significantly higher accuracy. For concreteness, we derive a second-order method and evaluate it using several simulated rigid-body systems, including an underactuated biped and a quadruped. In addition, we use this second-order method to plan locomotion trajectories for a complex quadrupedal microrobot. The planned trajectories are evaluated on the physical platform and result in a number of performance improvements.
This content will become publicly available on November 30, 2025
This paper reports a novel result: with proper robot models based on geometric mechanics, one can formulate the kinodynamic motion planning problems for rigid body systems as exact polynomial optimization problems. Due to the nonlinear rigid body dynamics, the motion planning problem for rigid body systems is nonconvex. Existing global optimization-based methods do not parameterize 3D rigid body motion efficiently; thus, they do not scale well to long-horizon planning problems. We use Lie groups as the configuration space and apply the variational integrator to formulate the forced rigid body dynamics as quadratic polynomials. Then, we leverage Lasserre’s hierarchy of moment relaxation to obtain the globally optimal solution via semidefinite programming. By leveraging the sparsity of the motion planning problem, the proposed algorithm has linear complexity with respect to the planning horizon. This paper demonstrates that the proposed method can provide globally optimal solutions or certificates of infeasibility at the second-order relaxation for 3D drone landing using full dynamics and inverse kinematics for serial manipulators. Moreover, we extend the algorithms to multi-body systems via the constrained variational integrators. The testing cases on cart-pole and drone with cable-suspended load suggest that the proposed algorithms can provide rank-one optimal solutions or nontrivial initial guesses. Finally, we propose strategies to speed up the computation, including an alternative formulation using quaternion, which provides empirically tight relaxations for the drone landing problem at the first-order relaxation.
more » « less- Award ID(s):
- 2118818
- PAR ID:
- 10565185
- Publisher / Repository:
- Sage Journals, The International Journal of Robotics Research
- Date Published:
- Journal Name:
- The International Journal of Robotics Research
- ISSN:
- 0278-3649
- Format(s):
- Medium: X
- Sponsoring Org:
- National Science Foundation
More Like this
-
-
The complex dynamics of agile robotic legged locomotion requires motion planning to intelligently adjust footstep locations. Often, bipedal footstep and motion planning use mathematically simple models such as the linear inverted pendulum, instead of dynamically-rich models that do not have closed-form solutions. We propose a real-time optimization method to plan for dynamical models that do not have closed form solutions and experience irrecoverable failure. Our method uses a data-driven approximation of the step-to-step dynamics and of a failure margin function. This failure margin function is an oriented distance function in state-action space where it describes the signed distance to success or failure. The motion planning problem is formed as a nonlinear program with constraints that enforce the approximated forward dynamics and the validity of state-action pairs. For illustration, this method is applied to create a planner for an actuated spring-loaded inverted pendulum model. In an ablation study, the failure margin constraints decreased the number of invalid solutions by between 24 and 47 percentage points across different objectives and horizon lengths. While we demonstrate the method on a canonical model of locomotion, we also discuss how this can be applied to data-driven models and full-order robot models.more » « less
-
A key problem in robotic locomotion is in finding optimal shape changes to effectively displace systems through the world. Variational techniques for gait optimization require estimates of body displacement per gait cycle; however, these estimates introduce error due to unincluded high order terms. In this paper, we formulate existing estimates for displacement, and describe the contribution of low order terms to these estimates. We additionally describe the magnitude of higher (third) order effects, and identify that choice of body coordinate, gait diameter, and starting phase influence these effects. We demonstrate that variation of such parameters on two example systems (the differential drive car and Purcell swimmer) effectively manages third order contributions.more » « less
-
Newly, there has been significant research interest in the exact solution of the AC optimal power flow (AC-OPF) problem. A semideflnite relaxation solves many OPF problems globally. However, the real problem exists in which the semidefinite relaxation fails to yield the global solution. The appropriation of relaxation for AC-OPF depends on the success or unfulflllment of the SDP relaxation. This paper demonstrates a quadratic AC-OPF problem with a single negative eigenvalue in objective function subject to linear and conic constraints. The proposed solution method for AC-OPF model covers the classical AC economic dispatch problem that is known to be NP-hard. In this paper, by combining successive linear conic optimization (SLCO), convex relaxation and line search technique, we present a global algorithm for AC-OPF which can locate a globally optimal solution to the underlying AC-OPF within given tolerance of global optimum solution via solving linear conic optimization problems. The proposed algorithm is examined on modified IEEE 6-bus test system. The promising numerical results are described.more » « less
-
Trajectory optimization o↵ers mature tools for motion planning in high-dimensional spaces under dynamic constraints. However, when facing complex configuration spaces, cluttered with obstacles, roboticists typically fall back to sampling-based planners that struggle in very high dimensions and with continuous di↵erential constraints. Indeed, obstacles are the source of many textbook examples of problematic nonconvexities in the trajectory-optimization prob- lem. Here we show that convex optimization can, in fact, be used to reliably plan trajectories around obstacles. Specifically, we consider planning problems with collision-avoidance constraints, as well as cost penalties and hard constraints on the shape, the duration, and the velocity of the trajectory. Combining the properties of B ́ezier curves with a recently-proposed framework for finding shortest paths in Graphs of Convex Sets (GCS), we formulate the planning problem as a compact mixed-integer optimization. In stark contrast with existing mixed-integer planners, the convex relaxation of our programs is very tight, and a cheap round- ing of its solution is typically sufficient to design globally-optimal trajectories. This reduces the mixed-integer program back to a simple convex optimization, and automatically provides optimality bounds for the planned trajectories. We name the proposed planner GCS, after its underlying optimization framework. We demonstrate GCS in simulation on a variety of robotic platforms, including a quadrotor flying through buildings and a dual-arm manipulator (with fourteen degrees of freedom) moving in a confined space. Using numerical experiments on a seven-degree-of-freedom manipulator, we show that GCS can outperform widely-used sampling-based planners by finding higher-quality trajectories in less time.more » « less