Try to optimize motion planning