minor cleanup. moved ompl planner initialization
added shift thing