[Paper Review] TORM: Fast and Accurate Trajectory Optimization of Redundant Manipulator given an End-Effector Path

Mincheul Kang, Heechan Shin+2 more|arXiv (Cornell University)|Sep 27, 2019

Robotic Mechanisms and Dynamics31 references4 citations

TL;DR

TORM presents a two-stage gradient descent optimization framework that integrates Jacobian-based inverse kinematics with motion planning to generate smooth, collision-free, and highly accurate trajectories for redundant manipulators along prescribed end-effector paths. By decoupling constraint satisfaction and path-following objectives and using iterative exploration, TORM achieves lower average and minimum pose errors than state-of-the-art methods, with robust convergence and anytime performance.

ABSTRACT

A redundant manipulator has multiple inverse kinematics solutions per end-effector pose. Accordingly, there can be many trajectories for joints that follow a given endeffector path in the Cartesian space. In this paper, we present a trajectory optimization of a redundant manipulator (TORM) to synthesize a trajectory that follows a given end-effector path accurately, while achieving smoothness and collisionfree manipulation. Our method holistically incorporates three desired properties into the trajectory optimization process by integrating the Jacobian-based inverse kinematics solving method and an optimization-based motion planning approach. Specifically, we optimize a trajectory using two-stage gradient descent to reduce potential competition between different properties during the update. To avoid falling into local minima, we iteratively explore different candidate trajectories with our local update. We compare our method with state-of-the-art methods in test scenes including external obstacles and two non-obstacle problems. Our method robustly minimizes the pose error in a progressive manner while satisfying various desirable properties.

Motivation & Objective

• To address the challenge of generating smooth, collision-free, and highly accurate joint trajectories for redundant manipulators given a desired end-effector path.
• To holistically integrate path accuracy, kinematic feasibility, and obstacle avoidance into a single optimization framework.
• To overcome local minima and conflicting objective gradients in trajectory optimization through a two-stage update and iterative exploration strategy.
• To achieve high-accuracy solutions within limited planning time, enabling real-time applicability.
• To validate the method on both obstacle-free and obstacle-embedded scenarios, including real robot experiments.

Proposed method

• The method uses a two-stage gradient descent (TSGD) that alternately updates for constraint satisfaction and path-following accuracy, reducing competition between objectives.
• It integrates Jacobian-based inverse kinematics with optimization-based motion planning to ensure kinematic feasibility and collision avoidance.
• Iterative exploration is employed to escape local minima by testing alternative trajectories and progressively refining the solution.
• The approach computes functional gradients efficiently, enabling fast convergence and anytime performance.
• The optimization is applied to sub-sampled poses along the end-effector path, with full trajectory reconstruction via interpolation.
• A signed distance function is used for real-time collision detection with external obstacles.

Figure 1: These figures show a sequence of maneuvering the Fetch manipulator to follow the specified end-effector path (red lines). Our method generates the trajectory that accurately follows the given end-effector path, while avoiding obstacles such as the pack of A4 paper and the table.

Experimental results

Research questions

• RQ1 Can a unified optimization framework effectively balance path accuracy, smoothness, and collision avoidance in redundant manipulator trajectory planning?
• RQ2 How does decoupling the optimization into two stages improve convergence and reduce conflicts between objectives?
• RQ3 To what extent does iterative exploration enhance robustness against local minima in trajectory optimization?
• RQ4 How does TORM compare to state-of-the-art methods in terms of pose error, planning time, and robustness across diverse environments?
• RQ5 Can the method achieve high-accuracy solutions with anytime performance, enabling real-time deployment?

Key findings

• TORM achieves the lowest average and minimum pose error among all compared methods, with a mean error of 1.91e-5, significantly outperforming RelaxedIK and Stampede.
• The method reduces pose error progressively and robustly, demonstrating strong anytime performance with consistent improvement over time.
• Ablation studies confirm that disabling two-stage gradient descent increases average pose error by over 10x, proving its necessity for conflict resolution.
• Without iterative exploration, the method gets stuck in local minima, resulting in the largest difference between min and max pose errors (8.50e-5 vs. 7.65e-5) across 20 trials.
• The method successfully generates a collision-free trajectory on a real Fetch robot, validating its practical feasibility.
• TORM maintains low trajectory length (9.5 rad) and initial solution time (5.2s), indicating efficiency and practicality for real-world deployment.

Figure 2: This figure shows our problem that is synthesizing a feasible and accurate trajectory $\xi$ for a given end-effector path $\mathcal{X}$ . The red line is $\mathcal{X}$ , which is approximated by end-effector poses $\widetilde{\mathcal{X}}=(\mathrm{x}_{0},\mathrm{x}_{1},...,\mathrm{x}_{n})$