Fast Adaptation of Manipulator Trajectories to Task Perturbation By Differentiating through the Optimal Solution

TL;DR

This paper introduces a novel method for real-time adaptation of manipulator trajectories to task perturbations by differentiating through the optimal solution, enabling fast and efficient updates in complex robotic tasks.

Contribution

The authors develop an analytical gradient-based approach for trajectory adaptation, significantly speeding up the process compared to traditional warm-start optimization methods.

Findings

Achieves near real-time trajectory adaptation for various task perturbations.

Provides a 160x speed-up over traditional warm-start trajectory optimization.

Maintains trajectory quality comparable to full re-optimization.

Abstract

Joint space trajectory optimization under end-effector task constraints leads to a challenging non-convex problem. Thus, a real-time adaptation of prior computed trajectories to perturbation in task constraints often becomes intractable. Existing works use the so-called warm-starting of trajectory optimization to improve computational performance. We present a fundamentally different approach that relies on deriving analytical gradients of the optimal solution with respect to the task constraint parameters. This gradient map characterizes the direction in which the prior computed joint trajectories need to be deformed to comply with the new task constraints. Subsequently, we develop an iterative line-search algorithm for computing the scale of deformation. Our algorithm provides near real-time adaptation of joint trajectories for a diverse class of task perturbations such as (i) changes in initial and final joint configurations of end-effector orientation-constrained trajectories and (ii) changes in end-effector goal or way-points under end-effector orientation constraints. We relate each of these examples to real-world applications ranging from learning from demonstration to obstacle avoidance. We also show that our algorithm produces trajectories with quality similar to what one would obtain by solving the trajectory optimization from scratch with warm-start initialization. But most importantly, our algorithm achieves a worst-case speed-up of 160x over the latter approach.