Non-linear constraints for path planning