Finished QuadShift with planning in configuration space