fix default callback and disable infeas check
Skip QP when unnecessary, and catch cases when start pos is inside obstacle
refactor and rename test cases for consistency
default argument
Copy over grid obstacle distance function from convex segmentation