Added OptimizeTrajectory to CHOMP

This shouldn't be merged until the distance manager is merged