FACTO: Function-space Adaptive Constrained Trajectory Optimization for Robotic Manipulators

TL;DR

Comparisons with optimization-based planners (CHOMP, TrajOpt, GPMP2, GPMP2) and sampling-based planners show the improved solution quality and feasibility, especially in constrained single- and multi-arm scenarios.

Abstract

This paper introduces Function-space Adaptive Constrained Trajectory Optimization (FACTO), a new trajectory optimization algorithm for both single- and multi-arm manipulators. Trajectory representations are parameterized as linear combinations of orthogonal basis functions, and optimization is performed directly in the coefficient space. The constrained problem formulation consists of both an objective functional and a finite-dimensional objective defined over truncated coefficients. To address nonlinearity, FACTO uses a Gauss-Newton approximation with exponential moving averaging, yielding a smoothed quadratic subproblem. Trajectory-wide constraints are addressed using coefficient-space mappings, and an adaptive constrained update using the Levenberg-Marquardt algorithm is performed in the null space of active constraints. Comparisons with optimization-based planners (CHOMP, TrajOpt, GPMP2) and sampling-based planners (RRT-Connect, RRT*, PRM) show the improved solution quality and feasibility, especially in constrained single- and multi-arm scenarios. The experimental evaluation of FACTO on Franka robots verifies the feasibility of deployment.

Figures (11)

• Figure 1: Overview of the FACTO framework. (Top left) Joint trajectories are represented in a coefficient space using an offset function and a set of orthogonal basis functions. (Top right) Equality and inequality constraints define a feasible subspace in coefficient space, within which the update step is selected. (Bottom right) Active constraints are enforced through null-space projection, enabling adaptive optimization while preserving feasibility. (Bottom left) In multi-arm settings, robot instances exchange only the information required for self-collision checking and coordination/collaboration tasks. Overall, these components form a complete FACTO pipeline for generating continuous-time, constraint-satisfying trajectories in both single- and multi-robot systems.
• Figure 2: Environment- and self-collision check for the dual-arm system. (a) Each robot $r$ is represented by its sphere tree $\bm{\mathcal{B}}_\mathrm{r}$, all rooted in a shared frame. Environment distances are obtained by evaluating each sphere $\mathcal{B}_{\mathrm{r}, i}$ in $\bm{\mathcal{B}}_\mathrm{r}$ against the workspace SDF. (b) For the self-collision check, all collision-check bodies are vectorized into the unified list $\bm{\mathfrak B} = \bigoplus_{\mathrm{r}=1}^{2} \bm{\mathcal{B}}_\mathrm{r}$. A multi-robot allowed-collision matrix $A_{cm}$ determines which pairs in $\bm{\mathfrak B}$ must be tested. Green cells denote required distance queries, while orange cells indicate pairs excluded from checking. The structure captures the collision relationships of both intra-robot and cross-robot (shown in the off-diagonal blocks outlined in bold green) within a single framework.
• Figure 3: Illustration of the constraint structure used in trajectory optimization. Task constraints $h_i(\psi)$ (top left) restrict the solution to trajectories that satisfy task-specific requirements. Boundary constraints (top right) enforce initial and goal conditions on the trajectory coefficients. Activated constraints (bottom right), such as joint limits or collision constraints, become active only when the current iterate approaches the corresponding boundary. All active constraints are collected into the linearized system $A_{\mathrm{ct}} \Delta\psi = b_{\mathrm{ct}}$ (bottom left), which defines a feasible update subspace intersecting with the descent direction of the objective $\nabla \mathcal{F}$. The optimization then proceeds within this subspace to ensure constraint satisfaction while reducing the objective cost.
• Figure 4: Overview of the FACTO optimization framework used for multi-robot trajectory generation. Each robot instance performs collision checking (environment, self, and cross-robot) and enforces individual or collaborative task constraints together with boundary and joint-limit conditions. These components contribute to the objective $\mathcal{F}$ and the associated smoothness term $\varrho$. Active constraints are handled through a null-space projection, after which a quadratic program is solved in the remaining feasible subspace. The lower panel illustrates the resulting motion update: the unconstrained search direction (blue dash) is projected onto the null space to satisfy active constraints, yielding the constrained update (blue) between the initial configuration $\bm q_0$ and the goal $\bm q_g$.
• Figure 5: Dual-arm motion planning pipeline. Robot configurations $q_L$ and $q_R$ are mapped to end-effector poses $T_L(q_L)$ and $T_R(q_R)$. Candidate grasps, $T_{g_L}$ and $T_{g_R}$, are validated using self- and environment-collision checks.