/** \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;
}