/** \brief Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called. */ void ompl::geometric::SimpleSetup::setGoal(const base::GoalPtr &goal) { pdef_->setGoal(goal); if (goal && goal->hasType(base::GOAL_SAMPLEABLE_REGION)) psk_ = std::make_shared<PathSimplifier>(si_, pdef_->getGoal()); else psk_ = std::make_shared<PathSimplifier>(si_); }
ompl::base::PlannerPtr ompl::tools::SelfConfig::getDefaultPlanner(const base::GoalPtr &goal) { base::PlannerPtr planner; if (!goal) throw Exception("Unable to allocate default planner for unspecified goal definition"); base::SpaceInformationPtr si(goal->getSpaceInformation()); control::SpaceInformationPtr siC(boost::dynamic_pointer_cast<control::SpaceInformation, base::SpaceInformation>(si)); if (siC) // kinodynamic planning { // if we have a default projection if (siC->getStateSpace()->hasDefaultProjection()) planner = base::PlannerPtr(new control::KPIECE1(siC)); // otherwise use a single-tree planner else planner = base::PlannerPtr(new control::RRT(siC)); } // if we can sample the goal region, use a bi-directional planner else if (goal->hasType(base::GOAL_SAMPLEABLE_REGION)) { // if we have a default projection if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection()) planner = base::PlannerPtr(new geometric::LBKPIECE1(goal->getSpaceInformation())); else planner = base::PlannerPtr(new geometric::RRTConnect(goal->getSpaceInformation())); } // otherwise use a single-tree planner else { // if we have a default projection if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection()) planner = base::PlannerPtr(new geometric::KPIECE1(goal->getSpaceInformation())); else planner = base::PlannerPtr(new geometric::RRT(goal->getSpaceInformation())); } if (!planner) throw Exception("Unable to allocate default planner"); return planner; }
ompl::base::PlannerPtr ompl::control::getDefaultPlanner(const base::GoalPtr &goal) { base::PlannerPtr planner; if (!goal) throw Exception("Unable to allocate default planner for unspecified goal definition"); SpaceInformationPtr si = boost::static_pointer_cast<SpaceInformation, base::SpaceInformation>(goal->getSpaceInformation()); if (si->getStateSpace()->hasDefaultProjection()) planner = base::PlannerPtr(new KPIECE1(si)); else planner = base::PlannerPtr(new RRT(si)); return planner; }