Riemannian Direct Trajectory Optimization of Rigid Bodies on Matrix Lie Groups

TL;DR

The paper addresses dynamical trajectory optimization for rigid bodies by formulating the problem on matrix Lie groups and solving it with constrained Riemannian optimization. It combines a Lie Group Variational Integrator (LGVI) for discrete dynamics on $\mathrm{SO}(3)\times \mathbb{R}^3$ with a line-search Riemannian Interior Point Method (RIPM) to handle nonlinear equality/inequality constraints, preserving topology and avoiding singularities. Key contributions include exact closed-form first- and second-order Riemannian derivatives of the dynamics, a linear-cost differentiation framework with respect to horizon and DOF, and an open-source implementation showing order-of-magnitude speedups over traditional ambient-space solvers on challenging drone and manipulator tasks. The work significantly advances fast, topology-aware motion planning for full rigid-body dynamics and scales to multi-body systems while maintaining dynamical feasibility. The approach has practical impact for real-time or near-real-time planning in robotics, where topology preservation and efficient optimization are critical.

Abstract

Designing dynamically feasible trajectories for rigid bodies is a fundamental problem in robotics. Although direct trajectory optimization is widely applied to solve this problem, inappropriate parameterizations of rigid body dynamics often result in slow convergence and violations of the intrinsic topological structure of the rotation group. This paper introduces a Riemannian optimization framework for direct trajectory optimization of rigid bodies. We first use the Lie Group Variational Integrator to formulate the discrete rigid body dynamics on matrix Lie groups. We then derive the closed-form first- and second-order Riemannian derivatives of the dynamics. Finally, this work applies a line-search Riemannian Interior Point Method (RIPM) to perform trajectory optimization with general nonlinear constraints. As the optimization is performed on matrix Lie groups, it is correct-by-construction to respect the topological structure of the rotation group and be free of singularities. The paper demonstrates that both the derivative evaluations and Newton steps required to solve the RIPM exhibit linear complexity with respect to the planning horizon and system degrees of freedom. Simulation results illustrate that the proposed method is faster than conventional methods by an order of magnitude in challenging robotics tasks.

Figures (7)

• Figure 1: Constrained Riemannian direct trajectory optimization of rigid bodies formulated on matrix Lie groups. The rigid body dynamics can be formulated in generalized coordinates ($q \in \mathbb{R}^n$) or on maximal coordinates (product space of $\mathrm{SO}(3)\times \mathbb{R}^3$). We compare the landscape of a quadratic cost $\|Rz - z\|^2$ defined on $\mathcal{M}=\mathrm{SO}(3)$ projected on $\mathbb{S}^2\cong\mathrm{SO}(3)/\mathrm{SO}(2)$ with different coordinates. With generalized coordinates represented by Euler angles, the landscape is highly complicated with saddle points. With the matrix Lie group coordinates, the decision variable is in a symmetric homogeneous space. In this work, we, for the first time, introduce constrained Riemannian optimization to solve motion planning of rigid bodies modeled on matrix Lie groups.
• Figure 2: Example of a rigid body system in maximal coordinates and its Jacobian. The blue block indicates the Jacobians with nonzero elements. The lighter gray box indicates zero block, while the darker gray box indicates the Jacobians of the autonomous single rigid body dynamics.
• Figure 3: Testing cases for drone docking from randomly generated initial poses in $\mathrm{SO}(3)\times \mathbb{R}^3$.
• Figure 4: Convergence of RIPM on drone on $\mathrm{SO}(3)\times \mathbb{R}^3$ with full dynamics with $100$ different initial conditions. The tolerance for convergence is set to $E_0 = 1e^{-14}$. The convergence rate of unconstrained cases in (a) is much faster than that of constrained cases in (b). The dashed lines indicate the cases that do not converge, while the others are converged cases. Accelerated convergence or super-linear convergence is observed to indicate the success of the second-order method. In (c), we compared the iterations to converge. We can see that it takes fewer iterations for both cases to stop with the default tolerance of IPOPT $\epsilon_{\mathrm{tol}} = 1e^{-6}$. The convergence rate and speed outperform the quaternion-based method IPOPT computed in the ambient space.
• Figure 5: Trajectories optimization for drone traversing complex environments considering general nonlinear constraints.

Theorems & Definitions (8)

• Definition 1: Riemannian Gradient
• Definition 2: Connection
• Definition 3: Covariant Derivative
• Definition 4: Riemannian Hessian
• Definition 5: Retraction
• Definition 6: Second-order Retraction
• Remark 1
• Definition 7: First-Order Optimality Conditions