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

TL;DR

FACTO is a novel trajectory optimization algorithm for robotic manipulators that operates in function space, improving solution quality and feasibility in constrained scenarios through adaptive constrained updates.

Contribution

The paper introduces FACTO, a new function-space trajectory optimization method that uses orthogonal basis functions and adaptive constrained updates for robotic manipulators.

Findings

Outperforms existing planners in solution quality and feasibility.

Effective in constrained single- and multi-arm manipulation tasks.

Validated on Franka robots for real-world deployment.

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.