Bring the Heat: Rapid Trajectory Optimization with Pseudospectral Techniques and the Affine Geometric Heat Flow Equation

TL;DR

PHLAME is introduced, which applies pseudospectral collocation and spatial vector algebra to efficiently solve the Affine Geometric Heat Flow (AGHF) Partial Differential Equation (PDE) for trajectory optimization and reduces the number of function evaluations required to yield a high accuracy solution thereby allowing it to scale efficiently to high-dimensional robotic systems.

Abstract

Generating optimal trajectories for high-dimensional robotic systems in a time-efficient manner while adhering to constraints is a challenging task. This paper introduces PHLAME, which applies pseudospectral collocation and spatial vector algebra to efficiently solve the Affine Geometric Heat Flow (AGHF) Partial Differential Equation (PDE) for trajectory optimization. Unlike traditional PDE approaches like the Hamilton-Jacobi-Bellman (HJB) PDE, which solve for a function over the entire state space, computing a solution to the AGHF PDE scales more efficiently because its solution is defined over a two-dimensional domain, thereby avoiding the intractability of state-space scaling. To solve the AGHF one usually applies the Method of Lines (MOL), which discretizes one variable of the AGHF PDE, and converts the PDE into a system of ordinary differential equations (ODEs) that are solved using standard time-integration methods. Though powerful, this method requires a fine discretization to generate accurate solutions and requires evaluating the AGHF PDE which is computationally expensive for high-dimensional systems. PHLAME overcomes this deficiency by using a pseudospectral method, which reduces the number of function evaluations required to yield a high accuracy solution thereby allowing it to scale efficiently to high-dimensional robotic systems. To further increase computational speed, this paper presents analytical expressions for the AGHF and its Jacobian, both of which can be computed efficiently using rigid body dynamics algorithms. PHLAME is tested across various dynamical systems, with and without obstacles and compared to a number of state-of-the-art techniques. PHLAME generates trajectories for a 44-dimensional state-space system in $\sim5$ seconds, much faster than current state-of-the-art techniques. A project page is available at https://roahmlab.github.io/PHLAME/

Figures (8)

• Figure 1: PHLAME works by first taking in some initial guess of a trajectory (trajectory of $i$th state shown in dark blue) which does not have to be dynamically feasible and evolves it into some dynamically feasible final trajectory dark green). Both trajectories start and end at $\mathtt{x}(0) = \mathtt{x}_0$ and $\mathtt{x}(T) = \mathtt{x}_f$ respectively. Notice that at the initial trajectory Digit (a high dimensional humanoid robot) has a dynamically infeasible set of configurations during it's stepping trajectory and that at the end of the PHLAME solve that trajectory is made into a dynamically feasible one where Digit is able to step over the box.
• Figure 2: Scaling trend of the mean evaluation times (in $\mu$s) of the right-hand side of the AGHF using Algorithm \ref{['alg: pino aghf']} as the number of bodies (N) increases from 2 to 22. The systems with $N$ between 2 and 6 are penduli systems (i.e., 2-link pendulum, 3-link pendulum, etc. ). The $N = 7$ system is the Kinova Gen3 and the $N = 22$ system is the pinned Digit V3 biped. Each robot's AGHF RHS was evaluated at a 1000 random robot configurations. Notice the polynomial line of best fit lacks a significant $N^3$ term, suggesting that the right-hand side computation time scales approximately quadratically with $N$ in practice.
• Figure 3: Scaling trend of the mean evaluation times (in $\mu$s) of the AGHF Jacobian using Algorithm \ref{['alg: pino aghf jacobian']} as the number of bodies (N) increases from 2 to 22. The systems with $N$ between 2 and 6 are penduli systems (i.e., 2-link pendulum, 3-link pendulum, etc. ). The $N = 7$ system is the Kinova Gen3 and the $N = 22$ system is the pinned Digit V3 biped. Each robot's AGHF Jacobian was evaluated at a 1000 random robot configurations. Notice the polynomial line of best fit lacks a significant $N^4$ term, suggesting that the Jacobian computation time scales approximately cubically with $N$ in practice.
• Figure 4: A bar plot comparing the mean solve times for three different trajectory optimization algorithms: PHLAME, AGHF AGHF_OG and Crocoddyl crocoddyl2020. For $N \leq 5$ the results correspond to the 1-5 link pendulum, for $N=7$ to the Kinova Gen3 Arm and for $N=22$ for the Pinned Digit biped robot. Each experiment was run ten times using the best solver parameter set. For $N>=5$ we do not show results for the Original AGHF implementation as no results were obtained in a reasonable amount of time. Overall, we see that PHLAME shows better empirical scalability and solve time than the other methods. The values of $\mathtt{x}_0$ and $\mathtt{x}_f$ are provided in https://github.com/roahmlab/PHLAME_comparisons.
• Figure 5: Side-by-side figures showing the convergence of action functional and $\int_0^T || u(t)||^2 dt$ for one of the trajectories found in the kinova no obtacle trajectory optimization problems. The lines transitions from dark blue to dark green as the trajectory converges to an extremal solution of \ref{['eqn:AGHF']}

Theorems & Definitions (9)

• Definition 2
• Lemma 3
• Lemma 4
• Theorem 5
• Theorem 6
• Definition 7
• Definition 8
• proof
• proof