void ompl::base::SpaceInformation::setStateValidityChecker(const StateValidityCheckerFn &svc)
{
class BoostFnStateValidityChecker : public StateValidityChecker
{
public:
BoostFnStateValidityChecker(SpaceInformation* si,
const StateValidityCheckerFn &fn) : StateValidityChecker(si), fn_(fn)
{
}
virtual bool isValid(const State *state) const
{
return fn_(state);
}
protected:
StateValidityCheckerFn fn_;
};
if (!svc)
throw Exception("Invalid function definition for state validity checking");
setStateValidityChecker(StateValidityCheckerPtr(dynamic_cast<StateValidityChecker*>(new BoostFnStateValidityChecker(this, svc))));
}
void oc::LTLSpaceInformation::extendValidityChecker(const oc::SpaceInformationPtr& oldsi)
{
class LTLStateValidityChecker : public ob::StateValidityChecker
{
public:
LTLStateValidityChecker(oc::LTLSpaceInformation* ltlsi,
const oc::ProductGraphPtr& prod,
const ob::StateValidityCheckerPtr& lowChecker)
: ob::StateValidityChecker(ltlsi), prod_(prod), lowChecker_(lowChecker), ltlsi_(ltlsi)
{
}
virtual ~LTLStateValidityChecker() { }
virtual bool isValid(const ob::State* s) const
{
return ltlsi_->getProdGraphState(s)->isValid()
&& lowChecker_->isValid(ltlsi_->getLowLevelState(s));
}
private:
const oc::ProductGraphPtr prod_;
const ob::StateValidityCheckerPtr lowChecker_;
oc::LTLSpaceInformation* ltlsi_;
};
// Some compilers have trouble with LTLStateValidityChecker being hidden in this function,
// and so we explicitly cast it to its base type.
setStateValidityChecker(ob::StateValidityCheckerPtr(static_cast<ob::StateValidityChecker*>(
new LTLStateValidityChecker(this, prod_, oldsi->getStateValidityChecker()))));
}
CSpaceOMPLSpaceInformation::CSpaceOMPLSpaceInformation(CSpace* _space)
:ob::SpaceInformation(ob::StateSpacePtr(new CSpaceOMPLStateSpace(_space,this))),cspace(_space)
{
setStateValidityChecker(ob::StateValidityCheckerPtr(new CSpaceOMPLValidityChecker(this)));
setMotionValidator(ob::MotionValidatorPtr(new CSpaceOMPLMotionValidator(this)));
setup();
}
void DensoSetup::initialize(RobotBasePtr probot)
{
const ob::StateSpacePtr &sSpace = getStateSpace();
sSpace->setup();
ob::ScopedState<> start(sSpace);
ob::ScopedState<> goal(sSpace);
std::vector<double> startVec(sSpace->getDimension(), 0.);
std::vector<double> goalVec(sSpace->getDimension(), 0.);
unsigned int ndof = si_->getStateSpace()->as<DensoStateSpace>()->
_probot->GetActiveDOF();
for (unsigned int i = 0; i < ndof; i++)
goalVec[i] = 1.0;
if (_useRealScene)
{
startVec[0] = -1.576;
startVec[1] = 1.374;
startVec[2] = 0.0;
startVec[3] = 0.0;
startVec[4] = -1.36751533;
startVec[5] = 1.615;
goalVec[0] = 1.55;
goalVec[1] = 1.35;
goalVec[2] = 0.1;
goalVec[3] = -000057;
goalVec[4] = -1.40754;
goalVec[5] = 1.6;
}
sSpace->copyFromReals(start.get(), startVec);
sSpace->copyFromReals(goal.get(), goalVec);
/* set start, goal, and threshold*/
setStartAndGoalStates(start, goal, threshold);
/* set propagation step size and min-max propagation steps*/
si_->setPropagationStepSize(propagationStepSize);
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
/* initialize a new KPIECE planner */
setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_)));
/* set default parameters */
getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount);
getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias);
/* set a state validity checker */
setStateValidityChecker(ob::StateValidityCheckerPtr
(new DensoStateValidityChecker(si_)));
si_->setStateValidityCheckingResolution(0.05); //5%
/* set a state propagator */
setStatePropagator(oc::StatePropagatorPtr(new DensoStatePropagator(si_)));
}
void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerName,
const std::vector<double>& stateVec)
{
const ob::StateSpacePtr& space = getStateSpace();
space->setup();
// setup start state
ob::ScopedState<> start(space);
if (stateVec.size() == space->getDimension())
space->copyFromReals(start.get(), stateVec);
else
{
// Pick koule positions evenly radially distributed, but at a linearly
// increasing distance from the center. The ship's initial position is
// at the center. Initial velocities are 0.
std::vector<double> startVec(space->getDimension(), 0.);
double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
startVec[0] = .5 * sideLength;
startVec[1] = .5 * sideLength;
startVec[4] = .5 * delta;
for (unsigned int i = 0; i < numKoules; ++i, theta += delta)
{
r = .1 + i * .1 / numKoules;
startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
}
space->copyFromReals(start.get(), startVec);
}
setStartState(start);
// set goal
setGoal(std::make_shared<KoulesGoal>(si_));
// set propagation step size
si_->setPropagationStepSize(propagationStepSize);
// set min/max propagation steps
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// set directed control sampler; when using the PDST planner, propagate as long as possible
bool isPDST = plannerName == "pdst";
const ompl::base::GoalPtr& goal = getGoal();
si_->setDirectedControlSamplerAllocator(
[&goal, isPDST](const ompl::control::SpaceInformation *si)
{
return KoulesDirectedControlSamplerAllocator(si, goal, isPDST);
});
// set planner
setPlanner(getConfiguredPlannerInstance(plannerName));
// set validity checker
setStateValidityChecker(std::make_shared<KoulesStateValidityChecker>(si_));
// set state propagator
setStatePropagator(std::make_shared<KoulesStatePropagator>(si_));
}
void SuperBotSetup::initialize(RobotBasePtr probot)
{
const ob::StateSpacePtr &sSpace = getStateSpace();
sSpace->setup();
ob::ScopedState<> start(sSpace);
ob::ScopedState<> goal(sSpace);
std::vector<double> startVec(sSpace->getDimension(), 0.);
std::vector<double> goalVec(sSpace->getDimension(), 0.);
unsigned int ndof = si_->getStateSpace()->as<SuperBotStateSpace>()->_probot->GetActiveDOF();
for (unsigned int i = 0; i < ndof; i++)
goalVec[i] = 1.0;
sSpace->copyFromReals(start.get(), startVec);
sSpace->copyFromReals(goal.get(), goalVec);
// set start, goal, and threshold
setStartAndGoalStates(start, goal, threshold);
// set propagation step size and min-max propagation steps
si_->setPropagationStepSize(propagationStepSize);
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// initialize a new KPIECE planner
setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_)));
// set default parameters
getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount);
getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias);
// set a state validity checker
setStateValidityChecker(ob::StateValidityCheckerPtr(new SuperBotStateValidityChecker(si_)));
si_->setStateValidityCheckingResolution(0.05); //5%
// set a state propagator
setStatePropagator(oc::StatePropagatorPtr(new SuperBotStatePropagator(si_)));
}
void ompl::base::SpaceInformation::setStateValidityChecker(const StateValidityCheckerFn &svc)
{
class FnStateValidityChecker : public StateValidityChecker
{
public:
FnStateValidityChecker(SpaceInformation *si, StateValidityCheckerFn fn)
: StateValidityChecker(si), fn_(std::move(fn))
{
}
bool isValid(const State *state) const override
{
return fn_(state);
}
protected:
StateValidityCheckerFn fn_;
};
if (!svc)
throw Exception("Invalid function definition for state validity checking");
setStateValidityChecker(std::make_shared<FnStateValidityChecker>(this, svc));
}