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

Yichang Feng; Xiao Liang; Minghui Zheng

cs.ROFeb 23, 2026

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

Authors: Yichang Feng, Xiao Liang, Minghui Zheng

Organizations: J. Mike Walker ’66 Department of Mechanical Engineering, Texas A&M University, College Station, TX 77843, USA · Zachry Department of Civil and Environmental Engineering, Texas A&M University, College Station, TX 77843 USA

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 & tables

Fig. 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.

Panel of Figure 2 — Fig. 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.

Fig. 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.

Fig. 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}

Fig. 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.

Scenario	$\epsilon$	$N$	$\varrho$	$(\beta_{1},\,\beta_{2})$
bookshelf_small	0.065	6	5e-2	(0.250, 0.125)
bookshelf_tall	0.065	6	5e-2	(0.250, 0.125)
bookshelf_thin	0.065	6	5e-2	(0.250, 0.125)
box	0.075	6	5e-2	(0.250, 0.125)
kitchen	0.075	7	5e-2	(0.250, 0.125)
cage	0.035	8	5e-3	(0.500, 0.250)

TABLE I : Parameter settings of FACTO for benchmark scenarios.

Scene	Metric	FACTO-Cheby	FACTO-Sin	FACTO-Cos	CHOMP	TrajOpt	GPMP2	RRT-C	RRT*	PRM
bookshelf_small	Success (%)	93	97	98	98	67	97	96	47	78
	Time (s)	0.0376	0.2114	0.0387	0.1010	0.1297	0.0624	0.6753	1.5649	0.8481
	Time max (s)	0.1819	0.5763	0.1235	0.2410	0.4280	0.1594	9.5129	9.9939	8.4673
	Roughness	52.41	17.32	52.02	36.59	96.17	22.52	75.15	61.16	178.60
	Roughness max	100.31	66.30	85.62	54.06	579.64	37.42	555.34	198.49	408.42
bookshelf_tall	Success (%)	98	98	98	98	84	97	93	28	83

TABLE II : Single-arm benchmark results of the unconstrained tasks in bookshelf scenarios.

Scene	Metric	FACTO-Cheby	FACTO-Sin	FACTO-Cos	CHOMP	TrajOpt	GPMP2	RRT-C	RRT*	PRM
box	Success (%)	90	92	98	98	22	93	99	4	85
	Time (s)	0.0838	0.1430	0.0546	0.0961	0.2428	0.0590	0.2927	4.7499	1.9173
	Time max (s)	0.3000	0.4993	0.2863	0.2390	0.4431	0.3306	4.4057	9.4519	9.5398
	Roughness	40.91	18.43	53.14	33.94	225.98	22.48	120.63	68.01	178.07
	Roughness max	78.90	92.20	103.93	46.99	472.49	40.59	466.65	139.62	443.24
cage	Success (%)	95	93	95	19	0	82	92	0	17

TABLE III : Single-arm benchmark results of the unconstrained tasks in box and cage scenarios.

Scene	Metric	FACTO-Cheby	FACTO-Sin	FACTO-Cos	CHOMP	TrajOpt	GPMP2	RRT-C	RRT*	PRM
kitchen	Success (%)	87	90	94	85	43	74	99	55	78
	Time (s)	0.1044	0.1710	0.0841	0.1592	0.2247	0.1126	0.5383	1.6377	1.2332
	Time max (s)	0.3939	0.5831	0.9290	0.4375	0.5261	0.1998	6.0240	8.7961	8.2058
	Roughness	81.45	41.94	79.26	53.69	299.30	33.01	189.48	149.74	274.39
	Roughness max	133.54	106.45	154.38	115.46	212.52	46.36	824.69	506.71	630.49
table_pick	Success (%)	91	98	99	87	56	97	98	33	85

TABLE IV : Single-arm benchmark results of the unconstrained tasks in kitchen and table_pick scenarios.

Panel of Figure 10 — Fig. 6 : Result of cage scenario, single arm for unconstrained tasks

Panel of Figure 11 — Fig. 7 : Single-arm result of the unconstrained tasks in kitchen and table_under_pick scenarios

Scene	Metric	FACTO-Cheby	FACTO-Sin	FACTO-Cos	CHOMP	IMACS-RRT-C
kitchen_constr	Success (%)	79	88	90	10	86
	Time (s)	0.1800	0.5602	0.2077	1.5915	4.0364
	Time max (s)	0.8345	1.6304	1.0268	1.8074	8.7817
	Roughness	125.12	93.97	141.20	335.49	2117.17
	Roughness max	482.55	266.14	424.33	1852.22	7570.24
table_under_pick_constr	Success (%)	77	88	96	9	72

TABLE V : Single-arm benchmark results for task-constrained scenarios.

Panel of Figure 13 — Fig. 8 : Single-arm results of the constrained tasks in kitchen and table scenarios

Scene	Metric	FACTO-Cheby	FACTO-Sin	FACTO-Cos	CHOMP	TrajOpt	GPMP2	RRT-C	RRT*	PRM
table_under_pick	Success (%)	70	90	92.5	27.5	27.5	47.5	90	12.5	12.5
	Time (s)	0.2056	0.4776	0.2421	0.4379	0.4767	0.3116	2.0272	7.0996	5.3386
	Time max (s)	0.3828	1.0061	0.8534	0.5005	0.5297	0.8220	8.3906	8.1623	10.0060
	Roughness	134.59	80.76	250.56	88.85	88.85	60.00	512.03	337.10	498.88
	Roughness max	331.68	176.61	466.41	108.28	108.28	88.56	1013.21	628.82	847.33

TABLE VI : Benchmark results for unconstrained dual-arm coordination scenario.

Scene	Metric	FACTO-Cheby	FACTO-Sin	FACTO-Cos	CHOMP	IMACS-RRT-C
table_under_pick_constr	Success (%)	62.5	77.5	87.5	7.5	27.5
	Time (s)	0.2453	1.5373	0.3703	3.5326	4.2324
	Time max (s)	0.3948	2.6837	0.6560	4.2109	8.6093
	Roughness	197.39	129.74	322.18	95.55	1122.33
	Roughness max	647.02	270.11	488.10	140.23	2674.44

TABLE VII : Benchmark results for constrained dual-arm coordination scenario.

Panel of Figure 16 — Fig. 9 : Results of dual-arm coordination in a table scenario

Panel of Figure 17 — Fig. 10 : Physical validation results of a single arm executing 2 tasks in the table scenario

Panel of Figure 18 — Fig. 11 : Physical validation results of the dual arms executing 3 tasks in 2 scenarios

Robot	Scenario	$N$	Scaled time (s)
Single arm	bookshelf	6	2.754
	bookshelf_half	6	3.517
	box	6	4.626
	table_pick	6	5.853
	table_under_pick	6	5.138
	table_under_pick_constr	7	5.291

TABLE VIII : Physical experiment suite and time-scaling settings.

Trajectory Optimization

Trajectory optimization computes motion or control sequences that minimize costs while satisfying system dynamics, safety limits, and task constraints.

See all 199 papers

Mar 10, 2026

DRAFTO: Decoupled Reduced-space and Adaptive Feasibility-repair Trajectory Optimization for Robotic Manipulators

This paper introduces a new algorithm for trajectory optimization, Decoupled Reduced-space and Adaptive Feasibility-repair Trajectory Optimization (DRAFTO). It first constructs a constrained objective that accounts for smoothness, safety, joint limits, and task requirements. Then, it optimizes the coefficients, which are the coordinates of a set of basis functions for trajectory parameterization. To reduce the number of repeated constrained optimizations while handling joint-limit feasibility, the optimization is decoupled into a reduced-space Gauss-Newton (GN) descent for the main iterations and constrained quadratic programming for initialization and terminal feasibility repair. The two-phase acceptance rule with a non-monotone policy is applied to the GN model, which uses a hinge-squared penalty for inequality constraints, to ensure globalizability. The results of our benchmark tests against optimization-based planners, such as CHOMP, TrajOpt, GPMP2, and FACTO, and sampling-based planners, such as RRT-Connect, RRT*, and PRM, validate the high efficiency and reliability across diverse scenarios and tasks. The experiment involving grabbing an object from a drawer further demonstrates the potential for implementation in complex manipulation tasks. The supplemental video is available at https://youtu.be/XisFI37YyTQ.

Yichang Feng, Xiao Liang, Minghui Zheng

Yichang Feng and Minghui Zheng are with the J. Mike Walker ’66 Department of Mechanical Engineering, Texas A&M University, College Station, TX 77843, USA · Xiao Liang is with the Zachry Department of Civil and Environmental Engineering, Texas A&M University, College Station, TX 77843 USA

Trajectory Optimization Robotic Optimization Constrained Motion Planning Robotic Manipulation

Ask PDF

May 27, 2026

Natural Functional Gradients for Smooth Trajectory Optimization

Generating collision-free and smooth motions remains a central challenge in robotic manipulation, particularly in cluttered environments and narrow passages where feasible regions are highly constrained and fragmented. We propose a trajectory optimization framework that performs geometry-aware updates directly in function space using natural functional gradients. The method optimizes a Gaussian-smoothed surrogate objective that regularizes the optimization landscape through smooth trajectory perturbations while preserving trajectory-level structure. Because the updates are defined intrinsically in function space, trajectory regularity can be controlled independently of a particular time discretization. We derive a practical Monte-Carlo estimator of the natural functional gradient that requires only black-box trajectory evaluations, making the method applicable when analytic gradients are unavailable or unreliable due to collision checking and contact-rich simulation. Experiments on constrained robotic manipulation tasks demonstrate that the proposed method improves trajectory feasibility and produces smoother motions than representative planning and trajectory optimization baselines in environments with narrow geometric clearances. Additional results, videos, and implementation details are available at the project page: https://kisangpark.github.io/natural-functional-gradient/

Kisang Park, Chanwoo Kim, Kyungjae Lee +1

Department of Artificial Intelligence, Korea University, Seoul, Republic of Korea · Department of Statistics, Korea University, Seoul, Republic of Korea · 3RLWRLD, Seoul, Republic of Korea

Constrained Motion Planning Trajectory Optimization Trajectory Generation Robotic Manipulation Robot Motion Planning Robotic Optimization

Ask PDF

May 20, 2026

CMC-Opt: Constraint Manifold with Corners for Inequality-Constrained Optimization

We introduce a manifold-based framework for addressing optimization problems with equality and inequality constraints found in robotics. Our approach transforms the original problem into an unconstrained optimization problem directly on the constrained state space. To achieve this, we introduce ``constraint manifolds with corners" to represent the state space satisfying mixed nonlinear equality and inequality constraints. We further extend manifold optimization algorithms to operate on this new topological structure. We demonstrate the power and robustness of our framework in the context of a large-scale kinodynamic planning problem, successfully generating dynamically feasible trajectories where standard methods fail.

Yetong Zhang, Frank Dellaert

College of Computing Georgia Institute of Technology Atlanta, USA

Kinodynamic Motion Planning Riemannian Optimization Constrained Optimization Motion Planning Robot Motion Planning Robotic Optimization

Ask PDF

Abstract

Trajectory Optimization

Explore similar work

DRAFTO: Decoupled Reduced-space and Adaptive Feasibility-repair Trajectory Optimization for Robotic Manipulators

Natural Functional Gradients for Smooth Trajectory Optimization

CMC-Opt: Constraint Manifold with Corners for Inequality-Constrained Optimization