Distance transforms of sampled functions
CHOMP: Gradient optimization techniques for efficient motion planning