cs.RONov 13, 2026

Optimal Operating-Point Selection for Single-Linearization Trajectory Planning of Nonlinear Robot Manipulators

Authors: Bhanuka Dayawansa, Rohan Munasinghe

Organizations: Department of Electronic and Telecomm. Engineering, University of Moratuwa, Katubedda, Sri Lanka · Robotics Laboratory, Narva College, University of Tartu, Tartu 50090, Estonia

Abstract

Single-point linearization provides a computationally efficient basis for optimization-based trajectory planning of nonlinear robot manipulators. However, planning performance depends on the selected linearization point. This paper presents an offline performance-driven approach that treats the location of a single linearization as an optimization variable. Candidate operating points are sampled from a common cubic seed trajectory, and each corresponding affine model is used to solve an identical constrained finite-horizon quadratic program (QP). The resulting control sequences are executed on the original nonlinear dynamics, and the operating point minimizing the nonlinear trajectory objective is selected through grid search and bounded refinement. Evaluation on four point-to-point motions of a three-degree-of-freedom UR5 manipulator yields optimal normalized operating-point locations between 0.325 and 0.935, demonstrating trajectory-dependent selection. Compared with the best-performing conventional initial, midpoint, or terminal linearization, the proposed approach reduces the nonlinear trajectory objective by 17-18%, 21-22%, 37-38%, and 7-8% across the four tasks, respectively. Additional nonlinear execution with identical PID feedback correction confirms that the performance advantage persists for a representative task. These results demonstrate that systematic operating-point selection can substantially improve single-linearization trajectory planning while preserving a fixed affine planning model.

Figures & tables

Explore similar work

Jul 6, 2026

Dynamic Evaluation of Classical and Control-Aware Optimal Trajectory Planning in Robot Manipulators

Trajectory planning strongly influences tracking accuracy, actuator demand, and overall execution behavior in robotic manipulators. Classical planners such as cubic, quintic, and trapezoidal profiles are widely used for their simplicity and smoothness, yet they remain purely kinematic and ignore system dynamics and control effort during trajectory generation. As a result, nominally smooth trajectories can lead to inefficient nonlinear execution and increased corrective control action. This paper presents a control-aware optimal trajectory planning framework that explicitly incorporates manipulator dynamics and actuator effort within a finite-horizon formulation. A midpoint linearization strategy is introduced to improve approximation accuracy for large point-to-point motions. In contrast to prior comparisons, the proposed approach enables fair, isolated evaluation of trajectory generation effects under identical closed-loop nonlinear execution conditions. To this end, a unified evaluation framework is developed in which all planners are executed under identical nonlinear dynamics, controller structure, and actuator constraints. Simulations on a nonlinear simplified UR5 manipulator show that the proposed approach consistently reduces tracking error, corrective torque, and closed-loop execution cost compared to classical methods, achieving substantial reductions in actuator effort and execution cost across all evaluated scenarios, demonstrating that kinematic smoothness alone does not ensure dynamically efficient execution.
Feb 23, 2026

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

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