SBPL update solve(), returns the planning status now...
CHOMP update solve, return the planning status, add some init checks too