KPIECE(const Workspace &workspace, const Agent &agent, const InstanceFileMap &args) :
workspace(workspace), agent(agent), goalEdge(NULL), samplesGenerated(0), edgesAdded(0), edgesRejected(0) {
collisionCheckDT = args.doubleVal("Collision Check Delta t");
dfpair(stdout, "collision check dt", "%g", collisionCheckDT);
const typename Workspace::WorkspaceBounds &agentStateVarRanges = agent.getStateVarRanges(workspace.getBounds());
stateSpaceDim = agentStateVarRanges.size();
StateSpace *baseSpace = new StateSpace(stateSpaceDim);
ompl::base::RealVectorBounds bounds(stateSpaceDim);
for(unsigned int i = 0; i < stateSpaceDim; ++i) {
bounds.setLow(i, agentStateVarRanges[i].first);
bounds.setHigh(i, agentStateVarRanges[i].second);
}
baseSpace->setBounds(bounds);
ompl::base::StateSpacePtr space = ompl::base::StateSpacePtr(baseSpace);
const std::vector< std::pair<double, double> > &controlBounds = agent.getControlBounds();
controlSpaceDim = controlBounds.size();
ompl::control::RealVectorControlSpace *baseCSpace = new ompl::control::RealVectorControlSpace(space, controlSpaceDim);
ompl::base::RealVectorBounds cbounds(controlSpaceDim);
for(unsigned int i = 0; i < controlSpaceDim; ++i) {
cbounds.setLow(i, controlBounds[i].first);
cbounds.setHigh(i, controlBounds[i].second);
}
baseCSpace->setBounds(cbounds);
ompl::control::ControlSpacePtr cspace(baseCSpace);
// construct an instance of space information from this control space
spaceInfoPtr = ompl::control::SpaceInformationPtr(new ompl::control::SpaceInformation(space, cspace));
// set state validity checking for this space
spaceInfoPtr->setStateValidityChecker(boost::bind(&KPIECE::isStateValid, this, spaceInfoPtr.get(), _1));
// set the state propagation routine
spaceInfoPtr->setStatePropagator(boost::bind(&KPIECE::propagate, this, _1, _2, _3, _4));
pdef = ompl::base::ProblemDefinitionPtr(new ompl::base::ProblemDefinition(spaceInfoPtr));
spaceInfoPtr->setPropagationStepSize(args.doubleVal("Steering Delta t"));
dfpair(stdout, "steering dt", "%g", args.doubleVal("Steering Delta t"));
spaceInfoPtr->setMinMaxControlDuration(stol(args.value("KPIECE Min Control Steps")),stol(args.value("KPIECE Max Control Steps")));
dfpair(stdout, "min control duration", "%u", stol(args.value("KPIECE Min Control Steps")));
dfpair(stdout, "max control duration", "%u", stol(args.value("KPIECE Max Control Steps")));
spaceInfoPtr->setup();
kpiece = new ompl::control::KPIECE1(spaceInfoPtr);
kpiece->setGoalBias(args.doubleVal("KPIECE Goal Bias"));
dfpair(stdout, "goal bias", "%g", args.doubleVal("KPIECE Goal Bias"));
kpiece->setBorderFraction(args.doubleVal("KPIECE Border Fraction"));
dfpair(stdout, "border fraction", "%g", args.doubleVal("KPIECE Border Fraction"));
kpiece->setCellScoreFactor(args.doubleVal("KPIECE Cell Score Good"), args.doubleVal("KPIECE Cell Score Bad"));
dfpair(stdout, "cell score good", "%g", args.doubleVal("KPIECE Cell Score Good"));
dfpair(stdout, "cell score bad", "%g", args.doubleVal("KPIECE Cell Score Bad"));
kpiece->setMaxCloseSamplesCount(stol(args.value("KPIECE Max Close Samples")));
dfpair(stdout, "max closed samples", "%u", stol(args.value("KPIECE Max Close Samples")));
ompl::base::RealVectorRandomLinearProjectionEvaluator *projectionEvaluator = new ompl::base::RealVectorRandomLinearProjectionEvaluator(baseSpace, stateSpaceDim);
ompl::base::ProjectionEvaluatorPtr projectionPtr = ompl::base::ProjectionEvaluatorPtr(projectionEvaluator);
kpiece->setProjectionEvaluator(projectionPtr);
}
