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.

💡 Research Summary

The paper presents DRAFTO (Decoupled Reduced‑space and Adaptive Feasibility‑repair Trajectory Optimization), a novel trajectory‑optimization framework designed for robotic manipulators that combines function‑space parameterization with a split optimization strategy to achieve high speed and reliability. The authors first represent a full‑dimensional trajectory ξ(t) as a linear combination of a small set of orthogonal basis functions ϕ(t) and a boundary lift φ(t): ξ(t)=Φ(t)ψ+φ(t), where ψ is a low‑dimensional coefficient vector. This reduces the decision space from M·T (M joints over time) to M·(N+1) variables while preserving continuous‑time smoothness and collision‑avoidance properties.

The optimization problem includes smoothness, obstacle avoidance, safety margins, joint limits, and task‑specific constraints. Equality constraints (initial/final pose and task constraints) are handled exactly, while joint‑limit inequalities are numerous and would normally dominate computational cost. DRAFTO addresses this by decoupling the process into three stages:

1. Initialization (constrained QP) – Only smoothness and boundary constraints are considered, yielding an equality‑feasible starting point ψ₀. Because the smoothness matrix Q is diagonal (thanks to orthogonal bases), the QP is sparse and solved quickly.

2. Main iterations (reduced‑space Gauss‑Newton) – Joint‑limit violations are penalized with a hinge‑squared soft‑penalty b_F_jnt(ψ)=½σ_jnt⁻²∑_k max(0, v_k(ψ))², where v_k(ψ) measures the amount of violation at checkpoint k. Only the active set of violated checkpoints contributes to the gradient g_jnt and Hessian H_jnt, dramatically reducing the size of the linear system. Equality constraints are eliminated via a null‑space projection: ψ←ψ+Δψ₀+N z, where N spans the null‑space of A_eq. The reduced‑space system (NᵀHN+λI)z=−Nᵀg is solved with a Levenberg‑Marquardt‑style regularizer λ that is adapted based on a trust‑region reduction ratio.

3. Terminal feasibility repair (constrained QP) – After the GN loop, the algorithm checks joint‑limit feasibility. If violations remain above a tolerance, a second constrained QP (including all equality and inequality constraints) is solved to repair the trajectory before termination.

A two‑phase acceptance rule guarantees global convergence. In Phase I (exploratory), the GN step is taken with full step length (α=1) and λ is only updated; the step is always accepted. Once the trajectory becomes near‑feasible, Phase II (stabilization) activates a non‑monotone acceptance criterion based on a sliding window of recent objective values, allowing occasional non‑descent steps while still ensuring overall progress.

The authors evaluate DRAFTO on more than 1,000 planning tasks covering single‑arm and dual‑arm scenarios, comparing against four optimization‑based planners (CHOMP, TrajOpt, GPMP2, FACTO) and three sampling‑based planners (RRT‑Connect, RRT*, PRM). DRAFTO achieves average planning times between 0.03 s and 0.12 s (comparable to or faster than CHOMP) and success rates above 95 %, outperforming the baselines in both speed and reliability. Notably, in environments with dense joint‑limit checkpoints, DRAFTO’s soft‑penalty plus occasional QP repair prevents the frequent infeasibility observed in other methods.

A real‑world demonstration on a Franka Research 3 robot shows the complete planning‑execution pipeline: the robot successfully grasps an object from a drawer using the trajectory generated by DRAFTO, confirming that the method works under real sensor noise and controller latency.

In summary, DRAFTO’s contributions are:

• A decoupled framework that isolates equality‑feasible initialization and terminal repair (constrained QP) from the main search (reduced‑space GN), dramatically cutting the cost of handling many inequality constraints.
• Adaptive regularization and a two‑phase acceptance rule that provide both aggressive exploration and stable convergence.
• Soft hinge‑squared joint‑limit penalties with active‑set updates, enabling efficient handling of large numbers of joint‑limit checkpoints.
• Extensive empirical validation showing superior performance over state‑of‑the‑art optimization‑based and sampling‑based planners, and a successful hardware experiment.

The paper therefore offers a practical, high‑performance trajectory‑optimization tool that can be directly deployed in industrial and domestic robotic manipulation tasks.