Using pointers for Kinematic_constraints stuff. Allows early initialization
SBPL - Add planner selection
Really use the selected planner this time
Move sbpl_interface instance to plugin, so the planners list can be dynamically obtained