fast_inverse_planning