ompl::control::OpenDEStateValidityChecker::OpenDEStateValidityChecker(const SpaceInformationPtr &si)
: base::StateValidityChecker(si)
{
if (!dynamic_cast<OpenDEStateSpace *>(si->getStateSpace().get()))
throw Exception("Cannot create state validity checking for OpenDE without OpenDE state space");
osm_ = si->getStateSpace()->as<OpenDEStateSpace>();
}
ompl::control::OpenDEStatePropagator::OpenDEStatePropagator(const SpaceInformationPtr &si) : StatePropagator(si)
{
if (OpenDEStateSpace *oss = dynamic_cast<OpenDEStateSpace*>(si->getStateSpace().get()))
env_ = oss->getEnvironment();
else
throw Exception("OpenDE State Space needed for OpenDEStatePropagator");
}
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;
}