Added check for only 1dof in ompl_planning.yaml, cleaned up old code