Efficient Contact-Based Trajectory Optimisation
via Approximated Derivatives
(website still under creation)

Gradient-based trajectory optimisation is a powerful tool for robotic manipulation, enabling complex manipulation tasks to be solved. However, these optimisers take significant time for contact-interaction tasks, particularly in cluttered scenes. A large contributor to this is that calculating dynamics derivatives via numerical methods, such as finite-differencing, is computationally time-consuming when querying expensive physics simulators. In this paper, we propose a general method of speeding up trajectory optimisation by only calculating dynamics gradients at key-points with a computationally expensive method (finite-differencing or automatic-differentiation) and then using cheap linear interpolation in-between for the remaining derivatives. The result is a significant decrease in trajectory optimisation times without affecting the quality of the final solution noticeably.

Here are a few example of contact-based manipulation tasks solved with trajectory optimisation. We show the final converged trajectories generated by our method, TrajOptKP, on the left compared with the baseline on the right. In all instances our method is able to find a similar final solution to the baseline, whilst significantly reducing the optimisation time.

Push moderate clutter (Simulator = MuJoCo, Optimiser = iLQR, Gradients = finite-differencing): Push a green cylinder to a goal location (green silhouette) through clutter (red obstacles).

Box sweep (Simulator = MuJoCo, Optimiser = iLQR, Gradients = finite-differencing): Sweep a heavy box to a goal location (green silhouette).

Sphere push forward (Simulator = Drake, Optimiser = iLQR, Gradients = automatic-differentiation): Slide a sphere forwards to a goal pose. Task provided by Vince Kurtz from this repository.

Box slide (Simulator = MuJoCo, Optimiser = SCVX, Gradients = finite-differencing): Slide a box to a goal location (green silhouette). Task provided by Aykut Önol from this repository.

Our key-point methods also work for contact-implicit locomotion trajectory optimisation. We ran two different locomotion tasks using MPC and our key-point methods. Our key-point methods enable one of two ideas, either maintaining the same control frequency and extending the optimisation horizon, or, maintaining the same optimisation horizon and running MPC at a higher control frequency.

Walker locomotion (Simulator = MuJoCo, Optimiser = iLQR, Gradients = finite-differencing): Keep walker model upright at a target height whilst moving forward at some target velocity. MPC control, optimisation horizon = 80 time-steps.

Mini-cheetah locomotion (Simulator = Drake, Optimiser = iLQR, Gradients = automatic-differentiation): Keep mini-cheetah body upright whilst moving forward at some target velocity. Task provided by Vince Kurtz from this repository.