Efficient Bezier Velocity Optimization for Free-Floating Space Manipulators

Duo Zhang; Zhizhuo Zhang; Xiaoli Bai; Jingjin Yu

cs.ROSep 25, 2026

Efficient Bezier Velocity Optimization for Free-Floating Space Manipulators

Authors: Duo Zhang, Zhizhuo Zhang, Xiaoli Bai, Jingjin Yu

Organizations: Department of Computer Science, Rutgers, the State University of New Jersey, Piscataway, NJ, USA · Department of Mechanical and Aerospace Engineering, Rutgers, the State University of New Jersey, Piscataway, NJ, USA

Abstract

We present FAVOR (Free-floating Arm Velocity Optimization with Recursive Sensitivities), a planner for collision-free reaching, tracking, and prescribed-time pre-grasp interception on an unactuated spacecraft. It optimizes Bezier joint-velocity curves with linear velocity, acceleration, and continuity constraints. Decision dimension is independent of rollout resolution. Analytical recursive sensitivities provide task and clearance gradients through the coupled base-arm motion. Parallel evaluation, caching, and incremental collision discovery reduce computation. With a seven-DoF arm and five simulated spacecraft models, FAVOR achieves 99.8% point-to-point success with 1.779 s mean computation, versus 62.9% and 50.270 s for an IK-initialized position-spline baseline. Means include failures and timeouts. FAVOR completes 30 of 36 tracking cases, versus 15 for single-step QP, and all 36 interception instances, versus 22 for the spline baseline. A controlled ablation shows that finite differences increase mean planning time 4.7-fold.

Figures & tables

Fig. 1 : A planned detour around CALIPSO. Given the initial free-floating state, goal pose, and arrival time, the planner determines the blue end-effector path and coupled base–arm motion. Green and magenta mark start and arrival; the robot is shown at arrival.

Panel of Figure 2 — Fig. 2 : Point-to-point scenes with five target spacecraft and a CloudSat chaser. Initial robot configurations are shown with the planned end-effector paths and start/arrival markers.

	$v_{b,0}^{b}$ [mm/s]	$\omega_{b,0}^{b}$ [mrad/s]
V0	$(0,\,0,\,0)$	$(0,\,0,\,0)$
V1	$(-5.073,\,-3.478,\,3.535)$	$(-10.112,\,14.558,\,-7.211)$
V2	$(-0.946,\,-0.813,\,0.545)$	$(-0.984,\,-1.133,\,-1.033)$
V3	$(2.032,\,-2.484,\,4.063)$	$(-10.278,\,-6.229,\,-2.592)$
V4	$(0.434,\,3.983,\,2.342)$	$(-1.420,\,1.323,\,-1.550)$

TABLE I : Initial base motion.

Spacecraft	$S$ [%]	Time [s]	$S$ [%]	Time [s]	$S$ [%]	Time [s]	$S$ [%]	Time [s]
	FAVOR		Spline (random)		Spline (IK)		RRT
CALIPSO	99.2	7.282	10.4	58.704	12.0	58.097	0.0	$\geq 60$
Landsat 7	100.0	0.548	48.0	45.922	56.0	52.448	0.0	$\geq 60$
Hubble	100.0	0.320	50.4	50.459	90.4	47.281	0.0	$\geq 60$
SMAP	100.0	0.262	64.0	46.631	74.4	48.115	0.0	$\geq 60$
QuikSCAT	100.0	0.482	60.0	39.210	81.6	45.408	0.0	$\geq 60$

TABLE II : Point-to-point planning.

S

: success rate.

Panel of Figure 5 — Fig. 3 : CALIPSO inspection tasks at zero initial base motion and mass ratio ten. Blue and orange curves show the planned and reference paths; markers identify start and arrival. The target moves in (a) and is static in (b)–(d).

Method	Success [%]	$\bar{e}_{p}$ [mm]	$\bar{e}_{R}$ [rad]	Avg. time [s]
Static
FAVOR (stop)	85.2	5.74	0.0031	0.293
FAVOR (refine)	85.2	3.14	0.0103	3.968
FAVOR (IK)	85.2	7.3	0.0308	4.037
Single-step QP	37.0	4.71	0.00275	4.325
Dynamic

TABLE III : Trajectory tracking.

Panel of Figure 7 — Fig. 4 : Timed pre-grasp approaches under compound tumble. The left and right columns show the payload box and canister, respectively. Corner insets provide a closer view of the end effector and object. Blue curves show the planned paths; green and magenta mark start and arrival.

Panel of Figure 8 — Fig. 5 : Illustrative Robotiq 2F-85 contact grasps.

Fig. 6 : The same CALIPSO goal with three quintic segments and

C^{0}

C^{1}

, or

C^{2}

velocity continuity. Rows show seven-joint position, velocity, acceleration, and jerk. Dotted lines mark joins; callouts highlight derivative continuity. Jerk is evaluated within segments.

	FAVOR		Spline (random)		Spline (IK)
Object	$S$ [%]	Time [s]	$S$ [%]	Time [s]	$S$ [%]	Time [s]
Payload box	100.0	2.754	22.2	39.736	83.3	32.669
Canister	100.0	3.332	22.2	39.617	38.9	36.978
Overall	100.0	3.043	22.2	39.676	61.1	34.824

TABLE IV : Pre-grasp interception.

S

: success rate.

Fig. 7 : Momentum conservation across all 877 main-benchmark trajectories from FAVOR. Diamonds show group means.

Variant	Success [%]	Plan [s]	Assembly [ms]
FAVOR (full)	100.0	1.635	1.924
No cache	100.0	3.301	2.079
Independent assembly	100.0	2.557	8.246
Finite differences	92.0	7.613	–

TABLE V : Computational ablation.

Robot Motion Planning

Robot motion planning finds safe, feasible paths and trajectories for robots moving through constrained, changing environments.

See all 269 papers

FACTO: Function-space Adaptive Constrained Trajectory Optimization for Robotic Manipulators
Feb 23, 2026Yichang Feng, Xiao Liang, Minghui ZhengTrajectory Optimization Robotic Optimization
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
Accelerated Spline-Based Time-Optimal Motion Planning with Continuous Safety Guarantees for Non-Differentially Flat Systems
Mar 25, 2026Dries Dirckx, Jan Swevers, Wilm DecréMotion Planning Safe Motion Planning
DRAFTO: Decoupled Reduced-space and Adaptive Feasibility-repair Trajectory Optimization for Robotic Manipulators
Mar 10, 2026Yichang Feng, Xiao Liang, Minghui ZhengTrajectory Optimization Robotic Optimization
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

Feb 23, 2026J/K move · Enter open · S save

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

Yichang Feng, Xiao Liang, Minghui Zheng

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

Trajectory Optimization Robotic Optimization Constrained Motion Planning Robotic Manipulation

Open paper AskPDF arXiv

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.

Abstract

Robot Motion Planning

Explore similar work

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

Accelerated Spline-Based Time-Optimal Motion Planning with Continuous Safety Guarantees for Non-Differentially Flat Systems

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