Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

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

Yichang Feng, Xiao Liang, Minghui Zheng

Abstract

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.

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

Abstract

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.
Paper Structure (37 sections, 1 theorem, 33 equations, 4 figures, 4 tables, 1 algorithm)

This paper contains 37 sections, 1 theorem, 33 equations, 4 figures, 4 tables, 1 algorithm.

Table of Contents

  1. Introduction
  2. Related Works
  3. Waypoint-based trajectory optimization
  4. Function-space trajectory parameterization
  5. Globalization and feasibility handling
  6. Preliminary
  7. Function-space trajectory representation
  8. Problem Setup and Costs
  9. Constraint Construction
  10. Boundary constraints
  11. Task constraints
  12. Joint limit constraints
  13. Reduced-space Formulation
  14. DRAFTO: Decoupled Reduced-Space Search and Feasibility Repair Trajectory Optimization
  15. Initialization and equality-feasible starting point
  16. ...and 22 more sections

Key Result

Theorem 1

According to the definition of the modified working objective $\mathcal{J}$ in eq:work_obj_mod, let $\mathcal{L} := \{\bm\psi \mid \mathcal{J}(\bm\psi)\le \mathcal{J}(\bm\psi_0)\}$ be the level set and assume: (i) $\mathcal{J}$ is bounded below and $\nabla \mathcal{J}$ is Lipschitz continuous; (ii)

Figures (4)

  • Figure 1: Overview of DRAFTO with decoupled optimization processes: (i) constrained-QP for initialization and terminal feasibility repair; (ii) reduced-space Gauss–Newton iterations with null-space equality handling and hinge-squared inequality penalties. Comparison against FACTO, GPMP2, and RRT-Connect demonstrates high computational efficiency and success rate; a real-world drawer-grabbing execution via FCI is shown below.
  • Figure 2: Simulation result of the single FR3 arm in cage: representative snapshots along the trajectory are shown (green).
  • Figure 3: Simulation results of the dual FR3 arms in table_constr: the end-effector $x$-axis (red arrow) is constrained to align with the world $z$-axis; representative snapshots along the trajectory are shown (green).
  • Figure 4: Physical experiment of an FR3 arm grabbing an object inside a drawer.

Theorems & Definitions (1)

  • Theorem 1: Local Convergence