Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Phasing Through the Flames: Rapid Motion Planning with the AGHF PDE for Arbitrary Objective Functions and Constraints

Challen Enninful Adu, César E. Ramos Chuquiure, Yutong Zhou, Pearl Lin, Ruikai Yang, Bohao Zhang, Shubham Singh, Ram Vasudevan

TL;DR

BLAZE reformulates trajectory optimization for high-dimensional robots by generalizing the Affine Geometric Heat Flow (AGHF) PDE to arbitrary cost functions and coupling it with a Phase 1–Phase 2 framework to handle constraint violations during online planning. It introduces a Constrained Lagrangian and an input-constraint treatment that leverage efficient inverse-dynamics-based derivatives, enabling rapid generation of dynamically feasible trajectories that respect obstacles and actuation limits. The approach is validated against state-of-the-art trajectory optimizers and demonstrated on Kinova Gen3 hardware, achieving sub-second solutions (often within $2$–$3$ seconds) even in challenging, obstacle-rich tasks, while maintaining feasibility. The demonstrated speed, scalability, and hardware applicability suggest significant potential for online planning and real-time control in complex robotic systems.

Abstract

The generation of optimal trajectories for high-dimensional robotic systems under constraints remains computationally challenging due to the need to simultaneously satisfy dynamic feasibility, input limits, and task-specific objectives while searching over high-dimensional spaces. Recent approaches using the Affine Geometric Heat Flow (AGHF) Partial Differential Equation (PDE) have demonstrated promising results, generating dynamically feasible trajectories for complex systems like the Digit V3 humanoid within seconds. These methods efficiently solve trajectory optimization problems over a two-dimensional domain by evolving an initial trajectory to minimize control effort. However, these AGHF approaches are limited to a single type of optimal control problem (i.e., minimizing the integral of squared control norms) and typically require initial guesses that satisfy constraints to ensure satisfactory convergence. These limitations restrict the potential utility of the AGHF PDE especially when trying to synthesize trajectories for robotic systems. This paper generalizes the AGHF formulation to accommodate arbitrary cost functions, significantly expanding the classes of trajectories that can be generated. This work also introduces a Phase1 - Phase 2 Algorithm that enables the use of constraint-violating initial guesses while guaranteeing satisfactory convergence. The effectiveness of the proposed method is demonstrated through comparative evaluations against state-of-the-art techniques across various dynamical systems and challenging trajectory generation problems. Project Page: https://roahmlab.github.io/BLAZE/

Phasing Through the Flames: Rapid Motion Planning with the AGHF PDE for Arbitrary Objective Functions and Constraints

TL;DR

BLAZE reformulates trajectory optimization for high-dimensional robots by generalizing the Affine Geometric Heat Flow (AGHF) PDE to arbitrary cost functions and coupling it with a Phase 1–Phase 2 framework to handle constraint violations during online planning. It introduces a Constrained Lagrangian and an input-constraint treatment that leverage efficient inverse-dynamics-based derivatives, enabling rapid generation of dynamically feasible trajectories that respect obstacles and actuation limits. The approach is validated against state-of-the-art trajectory optimizers and demonstrated on Kinova Gen3 hardware, achieving sub-second solutions (often within – seconds) even in challenging, obstacle-rich tasks, while maintaining feasibility. The demonstrated speed, scalability, and hardware applicability suggest significant potential for online planning and real-time control in complex robotic systems.

Abstract

The generation of optimal trajectories for high-dimensional robotic systems under constraints remains computationally challenging due to the need to simultaneously satisfy dynamic feasibility, input limits, and task-specific objectives while searching over high-dimensional spaces. Recent approaches using the Affine Geometric Heat Flow (AGHF) Partial Differential Equation (PDE) have demonstrated promising results, generating dynamically feasible trajectories for complex systems like the Digit V3 humanoid within seconds. These methods efficiently solve trajectory optimization problems over a two-dimensional domain by evolving an initial trajectory to minimize control effort. However, these AGHF approaches are limited to a single type of optimal control problem (i.e., minimizing the integral of squared control norms) and typically require initial guesses that satisfy constraints to ensure satisfactory convergence. These limitations restrict the potential utility of the AGHF PDE especially when trying to synthesize trajectories for robotic systems. This paper generalizes the AGHF formulation to accommodate arbitrary cost functions, significantly expanding the classes of trajectories that can be generated. This work also introduces a Phase1 - Phase 2 Algorithm that enables the use of constraint-violating initial guesses while guaranteeing satisfactory convergence. The effectiveness of the proposed method is demonstrated through comparative evaluations against state-of-the-art techniques across various dynamical systems and challenging trajectory generation problems. Project Page: https://roahmlab.github.io/BLAZE/
Paper Structure (27 sections, 5 theorems, 50 equations, 5 figures, 11 tables)

This paper contains 27 sections, 5 theorems, 50 equations, 5 figures, 11 tables.

Table of Contents

  1. Introduction
  2. Preliminaries
  3. The Affine Geometric Heat Flow Partial Differential Equation
  4. Homotopies and the Action Functional
  5. Solving the AGHF Rapidly
  6. Ensuring $\mathcal{A}$ Coincides with the (OCP) Cost Function
  7. Incorporating Arbitrary Constraints into the AGHF
  8. Constrained Lagrangian
  9. Incorporating Input Constraints in the Lagrangian
  10. Designing a Phase 1 -Phase 2 Algorithm using the AGHF
  11. Experiments and Results
  12. Implementation of BLAZE
  13. Experimental Setup
  14. Selection of Parameters
  15. Evaluating Success or Failure
  16. ...and 12 more sections

Key Result

Lemma 4

Let $x_s$ satisfy the AGHF PDE eqn:AGHF. Then, $\frac{d \mathcal{A}(x_s)}{ds} \leq 0$ for all $s$. In addition, if the right hand side of the AGHF PDE when evaluated at $x_{s^*}$ is equal to $0$ for some $s^* \in [0,s_{max})$, then $\frac{d\mathcal{A}(x_{s^*})}{ds} = 0$.

Figures (5)

  • Figure 1: This paper introduces BLAZE, a Phase 1 - Phase 2 Affine Geometric Heat Flow (AGHF) framework, to rapidly solve optimal control problems while respecting robot constraints and avoiding obstacles. It begins with an initial trajectory (shown in orange with the color gradient illustrating the evolution in time starting from darkest and going to lightest) that may violate constraints (e.g., the second and fourth pose of the arm are in collision with the boxes and outlined in red). If the initial trajectory is infeasible, BLAZE enters Phase 1, where it evolves the trajectory into a trajectory that satisfies all constraints (e.g., in the blue trajectory, the Kinova arm has been moved out of collision with the boxes). Once the trajectory satisfies all constraints, Phase 2 begins, optimizing the motion to minimize a user-specified cost function while maintaining feasibility (optimized trajectory shown green). BLAZE optimizes the trajectory to reach a target configuration while avoiding the obstacles while considering the full dynamical model of the arm. Note that optimal control (including Phase 1 and Phase 2) for this 14 dimensional state space model is completed within $3s$ while satisfying input, state, and collision avoidance constraints.
  • Figure 2: A bar plot comparing the mean solve times for four different trajectory optimization algorithms: BLAZE, RAPTOR, Crocoddyl and Aligator. For $N \leq 5$ the results correspond to the 1-5 link pendulum, $N=7$ corresponds to the Kinova Gen3 Arm, $N=14$ corresponds to a bimanual system of two Kinova Gen3 Arms, $N=21$ corresponds to a trimanual system of three Kinova Gen3 Arms. Each experiment was run ten times. Overall, we see that BLAZE shows better scalability and solve times than the other methods as the system dimension increases.
  • Figure 3: A plot showing the evolution of the Action Functional versus the AGHF evolution parameter $s$ for all the hard task–based scenario experiments.
  • Figure 4: This figure shows a visualization of one of the task-based scenarios (scenario 4). In each subfigure, the color gradient illustrates the evolution in time where the start configuration is in the darkest shade and the end configuration in the lightest. The initial trajectory for the scenario is shown in the top image in yellow and collides with the obstacles along the path. The proposed algorithm is able to push this initial guess out of collision and generate the optimal collision-free solution shown in green in 2.19s for this scenario, whereas the other comparison methods cannot find a solution within the allotted time for this scenario.
  • Figure 5: This figure shows a series of still frames of the Kinova arm following the trajectory generated by BLAZE for Scenario 2 in Table \ref{['table: detailed experiment 3']}. Each subfigure depicts the arm at a different point in time. The initial guess collides with the obstacle. The proposed algorithm is able to push this initial guess out of collision and generate the optimal collision-free solution in 2.45s, whereas Aligator does it in 57.21s.

Theorems & Definitions (16)

  • Definition 1: Coercive Functions evans2022partial
  • Definition 3: Affine Geometric Heat Flow
  • Lemma 4: Action Functional Along the AGHF Homotopy
  • Definition 5: Control Extraction
  • Definition 6: Lagrangian and Action Functional for (OCP)
  • Theorem 7: AGHF Generates Feasible Trajectories
  • Remark 8: Relationship to Previous Lagrangians
  • Definition 9: Constrained Lagrangian
  • Lemma 10: AGHF for a Constrained Lagrangian
  • Lemma 11
  • ...and 6 more