PaperlayerView exact revision on arXiv

Source-linked AI summary

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

arXiv:2505.01589v1cs.RO

TL;DR

High-dimensional robotic trajectory optimization must handle dynamics, constraints, and task-specific objectives, while existing AGHF methods are limited to squared-control costs and often require constraint-satisfying initial guesses. BLAZE generalizes AGHF to arbitrary costs and introduces a Phase 1–Phase 2 approach for constraint-violating starts. In six realistic Kinova Gen3 scenarios, it completed 100% of trials, compared with 33.3% for Aligator and 0% for RAPTOR.

  • Problem

    Existing AGHF methods focus on squared-control costs and often require initial guesses that satisfy constraints, limiting the trajectory objectives and starts they accommodate.

    Source: §I · p. 2

  • Method

    BLAZE generalizes the AGHF formulation to arbitrary cost functions and uses a Phase 1–Phase 2 framework to bring constraint-violating initial guesses into the feasible set before objective optimization.

    Source: §I · p. 2, §VII · p. 10

  • Results

    In six realistic scenarios, BLAZE completed 100% of trials, compared with 33.3% for Aligator and 0% for RAPTOR.

    Source: E. Kinova Gen3 Hard Task-Based Scenarios · p. 8

  • Takeaways & Limitations

    BLAZE generates dynamically feasible trajectories while optimizing a specified objective, including from initial guesses that violate constraints.

    Source: §VII · pp. 9–10

    Because dynamic constraints enter through a penalty term, BLAZE may produce higher-cost trajectories than traditional optimal control methods, and balancing its constraint and dynamic-feasibility penalties can require tuning.

    Source: §VIII · p. 10

Abstract

from arXiv · show

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/

I. INTRODUCTION

High-dimensional constrained trajectory optimization must balance efficiency, nonlinear dynamics, constraints, scalability, and reliable convergence. BLAZE generalizes AGHF to arbitrary costs and constraint-violating initial guesses, and the paper evaluates it through comparisons and a Kinova Gen3 hardware demonstration.

I. INTRODUCTION

  • AGHF methods can generate dynamically feasible trajectories rapidly over a two-dimensional domain, but are limited to squared-control costs and often require constraint-satisfying initial guesses.The paper positions these restrictions as barriers to broader trajectory synthesis.Source: §I · p. 2
  • BLAZE generalizes the AGHF formulation to arbitrary cost functions and uses a Phase 1–Phase 2 algorithm to support constraint-violating initial guesses.The paper also introduces a method for enforcing input constraints within AGHF.Source: §I · p. 2
  • The paper compares BLAZE with state-of-the-art trajectory optimization techniques and demonstrates it on a Kinova Gen3 robot.The stated evaluation includes simulation comparisons and a hardware platform validation.Source: §I · p. 2

II. PRELIMINARIES

  • The objective is to generate a trajectory between specified initial and terminal states that satisfies dynamics and state and input constraints while minimizing an arbitrary cost.The assumed feasible trajectory need only exist; it is not assumed to be given to the user.Source: §II · pp. 2–3
  • The paper targets rapid, constraint-aware trajectory generation for high-dimensional systems using the AGHF PDE.Its stated assumptions include smooth, Lipschitz-continuous dynamics and existence of a constraint-satisfying trajectory.Source: §II · p. 3

III. THE AFFINE GEOMETRIC HEAT FLOW PARTIAL DIFFERENTIAL EQUATION

AGHF evolves a trajectory through a homotopy while decreasing an action functional. This section extends the framework to arbitrary cost functions by allowing a more general matrix in the formulation.

III. THE AFFINE GEOMETRIC HEAT FLOW PARTIAL DIFFERENTIAL EQUATION

  • AGHF is a parabolic PDE framework that deforms an arbitrary initial trajectory into a dynamically feasible one while minimizing squared control input.The section reviews the foundational formulation and introduces generalizations for arbitrary cost functions; inequality constraints are treated separately.Source: §III · p. 3

A. Homotopies and the Action Functional

  • The homotopy x(t,s) represents a family of trajectories, with t as the first argument and s as the evolution parameter.The family is twice differentiable in t and differentiable in s.Source: A. Homotopies and the Action Functional · p. 3
  • Allowing M(x(t,s)) to be an arbitrary positive semi-definite and invertible matrix supports general cost functions, unlike standard AGHF formulations tailored to reduce control effort.The paper states that M should be defined differently for general cost functions.Source: A. Homotopies and the Action Functional · p. 3
  • Along an AGHF homotopy, the action functional is nonincreasing; a curve where the PDE right-hand side is zero is a steady-state solution.The action functional is defined by integrating the Lagrangian along the trajectory.Source: A. Homotopies and the Action Functional · pp. 3–4

B. Solving the AGHF Rapidly

The AGHF is solved rapidly by exploiting its two-dimensional domain with the Method of Lines, while pseudospectral discretization and spatial vector algebra reduce computation. The generalized action functional aligns with arbitrary optimal-control costs, and under stated conditions the resulting trajectories can be made arbitrarily close to dynamically feasible ones.

B. Solving the AGHF Rapidly

  • The AGHF PDE has a two-dimensional domain and scales polynomially with state dimension, enabling its solution by the Method of Lines.The Method of Lines discretizes one domain dimension into nodes and solves the resulting coupled ODEs.Source: B. Solving the AGHF Rapidly · p. 4
  • Pseudospectral discretization reduces the required nodes while maintaining accuracy, and spatial vector algebra accelerates AGHF right-hand-side evaluation.Fewer nodes reduce repeated evaluations; the pseudospectral formulation also enables precise computation of time derivatives.Source: B. Solving the AGHF Rapidly · p. 4

C. Ensuring A Coincides with the (OCP) Cost Function

  • For trajectories satisfying system dynamics (2), the error term vanishes, reducing the Action Functional to integral c(x(t), ẋ(t), u(t)); as AGHF evolves, it decreases and coincides with the OCP objective.The standard formulation minimizes squared control input.Source: C. Ensuring A Coincides with the (OCP)… · pp. 4–5
  • For sufficiently large kd and smax, extracted controls generate trajectories arbitrarily close to dynamically feasible ones, with an explicit error bound.The guarantee applies when the initial curve lies in an open set around the boundary-conditioned curves and the stated assumptions hold.Source: C. Ensuring A Coincides with the (OCP)… · pp. 4–5

IV. INCORPORATING ARBITRARY CONSTRAINTS INTO THE AGHF

The AGHF incorporates inequality constraints through penalty terms in the Lagrangian, while a separate input-constraint approach avoids augmenting the state. The penalty framework does not itself guarantee satisfaction of inequality constraints, especially for constraint-violating initial trajectories.

A. Constrained Lagrangian

  • The constrained Lagrangian adds a penalty activated only for inequality violations, but does not guarantee eventual constraint satisfaction, particularly from infeasible initial trajectories.The AGHF convergence properties apply, and the feasible-trajectory result applies when the penalty function is coercive; the Phase 1 approach addresses the initial-violation challenge.Source: A. Constrained Lagrangian · p. 5

B. Incorporating Input Constraints in the Lagrangian

  • Instead of augmenting the state to enforce input constraints, the method appends input-penalty terms to the constrained Lagrangian.State augmentation increases the system and AGHF PDE dimensions for high-dimensional robots; evaluating the added penalty derivatives requires derivatives of extracted controls.Source: B. Incorporating Input Constraints in… · p. 5
  • When B = I, extracted controls can be computed through inverse dynamics, and an extended Recursive Newton-Euler Algorithm evaluates their derivatives.The extended algorithm recursively applies the chain rule to compute control-input derivatives.Source: B. Incorporating Input Constraints in… · pp. 5–6

V. DESIGNING A PHASE 1 -PHASE 2 ALGORITHM USING THE AGHF

The Phase 1–Phase 2 algorithm first moves an initial trajectory toward feasibility, then optimizes the objective while maintaining feasibility. The paper describes evaluations covering scalability, obstacle scenarios, and a Kinova Gen3 hardware platform.

V. DESIGNING A PHASE 1 -PHASE 2 ALGORITHM USING THE AGHF

  • Constraint-violating initial guesses can increase penalties, shrink ODE solver steps, and lengthen AGHF convergence; feasible initial trajectories are challenging to design for complex constraints.The two phases use distinct action functionals, with Phase 2 initialized from the Phase 1 solution.Source: §V · p. 6
  • Phase 1 drives the initial trajectory toward the feasible set while promoting dynamic feasibility; Phase 2 optimizes the objective while maintaining feasibility.For sufficiently large kcons and smax, with kcons > kd, Phase 2 remains within the feasible set when initialized there.Source: §V · p. 6

VI. EXPERIMENTS AND RESULTS

  • The experiments are described as covering scalability comparisons, obstacle scenarios, and translation of results to a Kinova Gen3 hardware platform.The section outline says it concludes by summarizing the evaluations but supplies no comparative outcomes here.Source: §VI · p. 6

A. Implementation of BLAZE

BLAZE’s implementation and evaluations compare constrained trajectory optimization methods across increasing system dimensions and sphere-obstacle scenarios. The experiments report faster scaling and solve times for BLAZE, with obstacle-avoidance trajectories generated for all tested scenarios.

A. Implementation of BLAZE

  • The implementation uses an activation function for state and input constraints, with ccons controlling how quickly it transitions from 0 to 1 after a violation.The same activation function is applied to input constraints using S(gj(u)).Source: A. Implementation of BLAZE · pp. 6–7

B. Experimental Setup

  • BLAZE was compared with Crocoddyl, Aligator, and RAPTOR on fixed-time problems enforcing input and state constraints, and obstacle avoidance where applicable.The experiments used an Ubuntu 22.04 machine with an AMD EPYC 7742 CPU.Source: B. Experimental Setup · p. 7

1) Selection of Parameters

  • For qualitatively new constraints or obstacles, parameters were varied until a feasible solution was found, followed by a small grid search if needed.Most experiments used parameters reported in.Source: 1) Selection of Parameters · p. 7

2) Evaluating Success or Failure

  • Success was assessed by forward-integrating the open-loop control with feedback and checking final-state error, constraint violation, and—when relevant—obstacle clearance.Obstacle trials checked sampled joint positions at 0.01-second intervals.Source: 2) Evaluating Success or Failure · p. 7

3) Aligning Initial Guesses

  • Crocoddyl and Aligator received control guesses derived by inverse dynamics from the shared initial trajectory, while RAPTOR received fitted Bezier coefficients.The acceleration trajectory for the DDP methods was computed using a fitted Chebyshev polynomial and differentiation matrix.Source: 3) Aligning Initial Guesses · p. 7

C. BLAZE Scalability with Increasing System Dimension

  • BLAZE scaled more favorably and solved faster than the other methods for most evaluated robot systems with N > 1.The systems ranged from 1–5-link pendulums to single, bimanual, and trimanual Kinova Gen3 arms; Fig. 2 reports mean solve times over ten runs.Source: C. BLAZE Scalability with Increasing… · p. 7, Figure 2 · p. 8

D. Kinova Gen3 Sphere Obstacle Avoidance

  • BLAZE generated collision-free trajectories in all tested obstacle scenarios and fully converged faster than Aligator, with lower objective cost than its third iteration.The comparison used 20 scenarios with 5 or 10 obstacles and a 60-second time budget.Source: D. Kinova Gen3 Sphere Obstacle Avoidance · p. 8

E. Kinova Gen3 Hard Task-Based Scenarios

In Kinova Gen3 task-based scenarios, BLAZE solved all trials while Aligator and RAPTOR solved fewer; hardware demonstrations also show rapid collision-free trajectory generation. The conclusion presents BLAZE’s generalized costs and Phase1-Phase2 approach, while noting objective-minimization and tuning trade-offs.

E. Kinova Gen3 Hard Task-Based Scenarios

  • Each hard-scenario Action Functional decreased and converged to a steady-state value as the AGHF evolution parameter increased.Figure 3 plots the Action Functional against the AGHF evolution parameter s for all hard task-based scenarios.Source: E. Kinova Gen3 Hard Task-Based Scenarios · p. 8, Figure 3 · p. 8
  • BLAZE completed 100% of six realistic task-based scenario trials, versus 33.3% for Aligator and 0% for RAPTOR.The scenarios include cuboid obstacles and require 2-second trajectories under state, input, and obstacle constraints; Table III summarizes the comparison.Source: E. Kinova Gen3 Hard Task-Based Scenarios · p. 8, Table III · p. 9

VII. CONCLUSION

  • BLAZE generalizes AGHF to arbitrary cost functions and uses Phase1 to reach feasibility before Phase2 optimizes while maintaining feasibility.The framework also introduces input-constraint enforcement without increasing the state dimension.Source: §VII · pp. 9–10

VIII. LIMITATIONS

  • Penalty-based dynamic constraints can limit objective minimization relative to traditional optimal-control methods, and balancing the constraint and dynamic-feasibility penalties may require tuning.The authors note that this trade-off may be acceptable when rapid generation of dynamically feasible trajectories is the goal.Source: §VIII · p. 10

APPENDIX A PROOF OF LEMMA 4

The proof expands the Action Functional integrand in the variation parameter, integrates by parts using the boundary conditions, and applies the AGHF PDE to establish the result.

APPENDIX A PROOF OF LEMMA 4

  • The proof starts from the variation of the Action Functional and Taylor-expands its integrand around the trajectory parameterized by s.The expansion is stated for L(x(t, s + δ), ˙x(t, s + δ)).Source: Appendix A · p. 12
  • Substituting the Taylor expansion into the variation and integrating by parts applies the boundary conditions (7).Source: Appendix A · p. 12
  • The proof rearranges the resulting terms and substitutes the AGHF PDE definition to obtain the desired result.Source: Appendix A · p. 12

APPENDIX B PROOF OF THEOREM 7

The proof uses an invariant sublevel set of the Action Functional and bounds the error between an AGHF trajectory and its integrated trajectory. Coercivity and Bellman–Gronwall arguments support the resulting bounds.

APPENDIX B PROOF OF THEOREM 7

  • Coercivity of the cost function is used to bound trajectories and their derivatives within the relevant Action Functional sublevel set.The proof argues that unbounded state or derivative values would contradict coercivity.Source: Appendix B · p. 12
  • The proof first establishes an invariant sublevel set of the Action Functional, then bounds the error between a trajectory in that set and its integrated trajectory.It applies the Bellman–Gronwall inequality and a bound established in Lemma 13.Source: Appendix B · p. 12
  • A feasible path provides a nonempty Action Functional sublevel set, and AGHF solutions initialized there converge to an invariant set as s increases.For sufficiently large s_max, the resulting trajectory is continuous and its Action Functional is at most C1.Source: Appendix B · pp. 12–13
  • The error analysis defines the trajectory error and bounds its dynamics using continuity, global Lipschitz conditions, and Bellman–Gronwall.The proof also derives a bound on the extracted control input from continuity and trajectory boundedness.Source: Appendix B · p. 13

APPENDIX C PROOF OF LEMMA 11

The proof substitutes the stated definitions into the control expression, simplifies it, and identifies the result with the robot dynamics when B = I.

APPENDIX C PROOF OF LEMMA 11

APPENDIX D EXPERIMENT HYPERPARAMETERS

The appendix documents the experiment parameter choices for Aligator, Crocoddyl, RAPTOR, and BLAZE. It describes each method’s tunable parameters and points to tables listing settings by experiment.

APPENDIX D EXPERIMENT HYPERPARAMETERS

  • Aligator and Crocoddyl share time discretization and running-cost weights, while other tunable weights and convergence parameters vary by method.The running cost combines weighted input and state 2-norms; Crocoddyl also permits state-constraint penalties, and Aligator exposes convergence parameters.Source: Appendix D · p. 14
  • Tables V–VIII list Aligator settings for scalability and obstacle scenarios and Crocoddyl settings for scalability.Source: Appendix D · p. 14, Table V · p. 14, Table VI · p. 14
  • RAPTOR’s principal parameters are Bezier degree and constraint-evaluation points, alongside IPOPT solver and convergence settings.The same parameter set was used across the swing-up and obstacle-avoidance scenarios.Source: Appendix D · p. 14, Table IV · p. 14
  • BLAZE extends constraint parameters k_cons and c_cons to be specified per constraint, with settings reported for its experiments.The text gives cuboid and sphere-obstacle examples; parameter tables cover scalability, sphere-obstacle avoidance, and realistic scenarios.Source: Appendix D · p. 14, Table IX · p. 15, Table X · p. 15
Loading 2505.01589v1…