virtual void output(FILE *out) {
SearchAlgorithm<D>::output(out);
closed.prstats(stdout, "closed ");
dfpair(stdout, "open list type", "%s", "binary heap");
dfpair(stdout, "node size", "%u", sizeof(Node));
dfpair(stdout, "weight", "%lg", wt);
}
AnytimeBidirectionalEST(const Workspace &workspace, const Agent &agent, const InstanceFileMap &args, bool quiet = false,
double gBound = std::numeric_limits<double>::infinity()) :
workspace(workspace), agent(agent),
forwardKDTree(KDTreeType(), agent.getDistanceEvaluator(), agent.getTreeStateSize()), backwardKDTree(KDTreeType(), agent.getDistanceEvaluator(), agent.getTreeStateSize()),
quiet(quiet), gBound(gBound) {
collisionCheckDT = args.doubleVal("Collision Check Delta t");
goalBias = args.doubleVal("Goal Bias");
howManySamplePoints = args.integerVal("Number of Sample Points");
samplePointRadius = args.doubleVal("Sample Point Radius");
linkRadius = args.doubleVal("Link Radius");
if(!quiet) {
dfpair(stdout, "collision check dt", "%g", collisionCheckDT);
dfpair(stdout, "number of sample points", "%u", howManySamplePoints);
}
unsigned int regionCount = discretization.getCellCount();
forwardRegions.reserve(regionCount);
backwardRegions.reserve(regionCount);
for(unsigned int i = 0; i < regionCount; ++i) {
forwardRegions.push_back(new RegionNode(i));
backwardRegions.push_back(new RegionNode(i));
}
samplesGenerated = edgesGenerated = samplesAdded = edgesAdded = 0;
timeout = args.doubleVal("Timeout");
selfPtr = this;
}
void go_AnytimeRRT(const InstanceFileMap &args, const Agent &agent, const Workspace &workspace,
const typename Agent::State &start, const typename Agent::State &goal) {
clock_t startT = clock();
dfpair(stdout, "planner", "%s", "Anytime RRT");
// typedef flann::KDTreeSingleIndexParams KDTreeType;
typedef flann::KDTreeIndexParams KDTreeType;
typedef FLANN_KDTreeWrapper<KDTreeType, typename Agent::DistanceEvaluator, typename Agent::Edge> KDTree;
typedef UniformSampler<Workspace, Agent, KDTree> USampler;
typedef GoalBiasSampler<Agent, USampler> GBSampler;
typedef TreeInterface<Agent, KDTree, GBSampler> TreeInterface;
typedef AnytimeRRT<Workspace, Agent, TreeInterface> Planner;
/* planner config */
KDTreeType kdtreeType(1);
KDTree kdtree(kdtreeType, agent.getDistanceEvaluator(), agent.getTreeStateSize());
USampler uniformsampler(workspace, agent, kdtree);
double goalBias = args.exists("Goal Bias") ? args.doubleVal("Goal Bias") : 0;
dfpair(stdout, "goal bias", "%g", goalBias);
GBSampler goalbiassampler(uniformsampler, goal, goalBias);
TreeInterface treeInterface(kdtree, goalbiassampler);
Planner planner(workspace, agent, treeInterface, args);
go_COMMONANYTIME<Planner, Workspace, Agent>(args, planner, workspace, agent, start, goal, startT);
}
virtual void output(FILE *out) {
SearchAlgorithm<D>::output(out);
dfpair(stdout, "node size", "%u", sizeof(Node));
dfpair(stdout, "RRT seed", "%lu", (unsigned long) seed);
dfpair(stdout, "KD-tree size", "%lu", (unsigned long) tree.size());
dfpair(stdout, "KD-tree depth", "%lu", (unsigned long) tree.depth());
}
void go_AnytimePPRM(const InstanceFileMap &args, const Agent &agent, const Workspace &workspace,
const typename Agent::State &start, const typename Agent::State &goal) {
clock_t startT = clock();
dfpair(stdout, "planner", "%s", "PPRM");
typedef PRMLite<Workspace, Agent> PRMLite;
typedef PlakuTreeInterface<Workspace, Agent, PRMLite> PlakuTreeInterfaceT;
typedef AnytimeRRT<Workspace, Agent, PlakuTreeInterfaceT> Planner;
unsigned int numberOfPRMVertices = stol(args.value("Number Of PRM Vertices"));
unsigned int numberOfNearestNeighborEdgeConsiderations = stol(args.value("Nearest Neighbors To Consider In PRM Edge Construction"));
double prmCollisionCheckDT = args.doubleVal("PRM Collision Check DT");
PRMLite prmLite(workspace, agent, numberOfPRMVertices, numberOfNearestNeighborEdgeConsiderations, prmCollisionCheckDT);
double alpha = args.doubleVal("Plaku Alpha Value");
double b = args.doubleVal("Plaku b Value");
double stateRadius = args.doubleVal("Plaku PRM State Selection Radius");
double goalBias = args.exists("Goal Bias") ? args.doubleVal("Goal Bias") : 0;
dfpair(stdout, "goal bias", "%g", goalBias);
PlakuTreeInterfaceT plakuTreeInterface(workspace, agent, prmLite, start, goal, alpha, b, stateRadius, goalBias);
Planner planner(workspace, agent, plakuTreeInterface, args);
go_COMMONANYTIME<Planner, Workspace, Agent>(args, planner, workspace, agent, start, goal, startT);
}
virtual void output(FILE *out) {
SearchAlgorithm<D>::output(out);
// We output the 'closed ' prefix here so that they key
// matches the closed keys for other search algorithms.
// This is desriable because the seen list performs a
// similar functionality as a closed list, and we will
// probably want to compare with closed list stats.
seen.prstats(stdout, "closed ");
dfpair(stdout, "lookahead depth", "%u", nlook);
dfpair(stdout, "node size", "%u", sizeof(Node));
}
RRTConnect(const Workspace &workspace, const Agent &agent, TreeInterface &treeInterface, const InstanceFileMap &args) :
workspace(workspace), agent(agent), treeInterface(treeInterface), solutionCost(-1),
samplesGenerated(0), edgesAdded(0), edgesRejected(0) {
steeringDT = args.doubleVal("Steering Delta t");
collisionCheckDT = args.doubleVal("Collision Check Delta t");
maxExtensions = args.integerVal("RRTConnect Max Extensions");
dfpair(stdout, "steering dt", "%g", steeringDT);
dfpair(stdout, "collision check dt", "%g", collisionCheckDT);
dfpair(stdout, "max extensions", "%u", maxExtensions);
}
// prstats prints statistics about the hash table.
void prstats(FILE *f, const char *prefix) {
char key[strlen(prefix) + strlen("collisions") + 1];
strcpy(key, prefix);
strcat(key+strlen(prefix), "fill");
dfpair(f, key, "%lu", fill);
strcpy(key+strlen(prefix), "collisions");
dfpair(f, key, "%lu", ncollide);
strcpy(key+strlen(prefix), "resizes");
dfpair(f, key, "%lu", nresize);
}
void grow() {
numVertices *= 2;
generateVertices(numVertices);
edges.clear();
if(shouldGenerateEdges) {
clock_t edgeStart = clock();
generateEdges(edgeSetSize);
dfpair(stdout, "prm edge build time", "%g", (double)(clock()-edgeStart) / CLOCKS_PER_SEC);
dfpair(stdout, "prm collision checks", "%u", collisionChecks);
}
}
void search(D &d, typename D::State &s0) {
bool optimal = false;
this->rowhdr();
this->start();
this->closed.init(d);
this->incons.init(d);
Node *n0 = this->init(d, s0);
this->closed.add(n0);
this->open.push(n0);
unsigned long n = 0;
do {
if (improve(d)) {
n++;
double epsprime = this->wt == 1.0 ? 1.0 : this->findbound();
if (this->wt < epsprime)
epsprime = this->wt;
this->row(n, epsprime);
}
if (this->wt <= 1.0)
optimal = true;
if (this->wt <= 1.0 || shouldstop())
break;
this->nextwt();
this->updateopen();
this->closed.clear();
} while(!shouldstop() && !this->limit() && !this->open.empty());
this->finish();
dfpair(stdout, "converged", "%s", optimal ? "yes" : "no");
}
void solveWithKPIECE(const VREPInterface *interface, const InstanceFileMap *args, const VREPInterface::State &start, const VREPInterface::State &goal) {
dfpair(stdout, "planner", "%s", "KPIECE");
KPIECE<VREPInterface, VREPInterface> kpiece(*interface, *interface, *args);
kpiece.query(start, goal);
kpiece.dfpairs();
}
void initialize(unsigned int numVertices, unsigned int edgeSetSize, bool shouldGenerateEdges) {
dfpair(stdout, "prm vertex set size", "%lu", numVertices);
dfpair(stdout, "prm edge set size", "%lu", edgeSetSize);
dfpair(stdout, "prm edge collision check dt", "%g", collisionCheckDT);
startTime = clock();
clock_t vertexStart = clock();
generateVertices(numVertices);
clock_t end = clock();
double time = (double)(end-vertexStart) / CLOCKS_PER_SEC;
dfpair(stdout, "prm vertex build time", "%g", time);
if(shouldGenerateEdges) {
clock_t edgeStart = clock();
generateEdges(edgeSetSize);
end = clock();
time = (double)(end-edgeStart) / CLOCKS_PER_SEC;
dfpair(stdout, "prm edge build time", "%g", time);
dfpair(stdout, "prm collision checks", "%u", collisionChecks);
time = (double)(end-startTime) / CLOCKS_PER_SEC;
dfpair(stdout, "prm build time", "%g", time);
}
}
LazyPRMLite(const Workspace &workspace, const Agent &agent, unsigned int numVertices,
unsigned int edgeSetSize, double collisionCheckDT) :
PRMLite<Workspace, Agent>(workspace, agent, numVertices, edgeSetSize, collisionCheckDT, false) {
// startTime is set in the parent constructor call
clock_t edgeStart = clock();
generateEdges(edgeSetSize);
clock_t end = clock();
double time = (double)(end-edgeStart) / CLOCKS_PER_SEC;
dfpair(stdout, "prm edge build time", "%g", time);
time = (double)(end-this->startTime) / CLOCKS_PER_SEC;
dfpair(stdout, "prm build time", "%g", time);
}
void go_AnytimeBidirectionalEST(const InstanceFileMap &args, const Agent &agent, const Workspace &workspace,
const typename Agent::State &start, const typename Agent::State &goal) {
clock_t startT = clock();
dfpair(stdout, "planner", "%s", "Anytime EST");
typedef AnytimeBidirectionalEST<Workspace, Agent> Planner;
Planner planner(workspace, agent, args);
go_COMMONANYTIME<Planner, Workspace, Agent>(args, planner, workspace, agent, start, goal, startT);
}
virtual void output(FILE *out) {
SearchAlgorithm<D>::output(out);
closed.prstats(stdout, "closed ");
dfpair(stdout, "open list type", "%s", "binary heap");
dfpair(stdout, "node size", "%u", sizeof(Node));
dfpair(stdout, "wf", "%g", wf);
dfpair(stdout, "wt", "%g", wt);
dfpair(stdout, "final time per expand", "%g", timeper);
dfpair(stdout, "number of resorts", "%lu", nresort);
dfpair(stdout, "mean expansion delay", "%g", avgdelay);
}
void go_MRRT(const InstanceFileMap &args, const Agent &agent, const Workspace &workspace,
const typename Agent::State &start, const typename Agent::State &goal) {
clock_t startT = clock();
dfpair(stdout, "planner", "%s", "MRRT");
typedef NoOpPostProcessor<Workspace, Agent> PostProccesor;
typedef AnytimeRestartingRRTWithPostProcessing<Workspace, Agent, PostProccesor> Planner;
PostProccesor postProccesor;
Planner planner(workspace, agent, postProccesor, args);
go_COMMONANYTIME<Planner, Workspace, Agent>(args, planner, workspace, agent, start, goal, startT);
}
void solveWithRRTConnect(const VREPInterface *interface, const InstanceFileMap *args, const VREPInterface::State &start, const VREPInterface::State &goal) {
dfpair(stdout, "planner", "%s", "RRT Connect");
typedef flann::KDTreeSingleIndexParams KDTreeType;
typedef FLANN_KDTreeWrapper<KDTreeType, flann::L2<double>, VREPInterface::Edge> KDTree;
typedef UniformSampler<VREPInterface, VREPInterface, KDTree> UniformSampler;
typedef TreeInterface<VREPInterface, KDTree, UniformSampler> TreeInterface;
typedef RRTConnect<VREPInterface, VREPInterface, TreeInterface> RRTConnect;
KDTreeType kdtreeType;
KDTree kdtree(kdtreeType, interface->getTreeStateSize());
UniformSampler uniformSampler(*interface, *interface, kdtree);
TreeInterface treeInterface(kdtree, uniformSampler);
RRTConnect rrtconnect(*interface, *interface, treeInterface, *args);
rrtconnect.query(start, goal);
rrtconnect.dfpairs();
}
void solveWithPlaku(const VREPInterface *interface, const InstanceFileMap *args, const VREPInterface::State &start, const VREPInterface::State &goal) {
dfpair(stdout, "planner", "%s", "Plaku IROS 2014");
typedef PRMLite<VREPInterface, VREPInterface> PRMLite;
typedef PlakuTreeInterface<VREPInterface, VREPInterface, PRMLite> PlakuTreeInterface;
typedef RRT<VREPInterface, VREPInterface, PlakuTreeInterface> Plaku;
unsigned int numberOfPRMVertices = stol(args->value("Number Of PRM Vertices"));
unsigned int numberOfNearestNeighborEdgeConsiderations = stol(args->value("Nearest Neighbors To Consider In PRM Edge Construction"));
double prmCollisionCheckDT = stod(args->value("PRM Collision Check DT"));
PRMLite prmLite(*interface, *interface, start, numberOfPRMVertices, numberOfNearestNeighborEdgeConsiderations, prmCollisionCheckDT);
double alpha = stod(args->value("Plaku Alpha Value"));
double b = stod(args->value("Plaku b Value"));
double stateRadius = stod(args->value("Plaku PRM State Selection Radius"));
PlakuTreeInterface plakuTreeInterface(*interface, *interface, prmLite, start, goal, alpha, b, stateRadius);
// plakuTreeInterface.draw();
Plaku plaku(*interface, *interface, plakuTreeInterface, *args);
plaku.query(start, goal);
}
void prstats(FILE *out, const char *prefix) {
dfpair(out, "closed list type", "%s", "hash table");
std::string key = prefix + std::string("fill");
dfpair(out, key.c_str(), "%lu", fill);
key = prefix + std::string("collisions");
dfpair(out, key.c_str(), "%lu", ncollide);
key = prefix + std::string("resizes");
dfpair(out, key.c_str(), "%lu", nresize);
key = prefix + std::string("buckets");
dfpair(out, key.c_str(), "%lu", nbins);
unsigned int m = 0;
for (unsigned int i = 0; i < nbins; i++)
m = std::max(m, fills[i]);
key = prefix + std::string("max bucket fill");
dfpair(out, key.c_str(), "%u", m);
}
void dfPairs() const {
dfpair(stdout, "prm collision checks", "%u", this->collisionChecks);
}
virtual void output(FILE *out) {
SearchAlgorithm<D>::output(out);
Arastar<D>::output(out);
dfpair(stdout, "wf", "%g", wcost);
dfpair(stdout, "wt", "%g", wtime);
}
std::vector<const Edge*> query(const State &start, const State &goal, int iterationsAtATime = -1, bool firstInvocation = true) {
bool foundGoal = false;
#ifdef WITHGRAPHICS
auto green = OpenGLWrapper::Color::Green();
start.draw(green);
agent.drawMesh(start);
goal.draw(green);
#endif
if(agent.isGoal(start, goal)) {
dfpair(stdout, "solution cost", "0");
dfpair(stdout, "solution length", "0");
return std::vector<const Edge*>();
}
if(firstInvocation) {
auto root = pool.construct(start);
treeInterface.insertIntoTree(root);
}
unsigned int iterations = 0;
while(!foundGoal) {
std::pair<Edge*, State> treeSample = treeInterface.getTreeSample();
samplesGenerated++;
#ifdef WITHGRAPHICS
samples.push_back(treeSample.second);
#endif
auto edge = agent.steer(treeSample.first->end, treeSample.second, steeringDT);
unsigned int added = 0;
Edge *parent = treeSample.first;
while(added < maxExtensions && workspace.safeEdge(agent, edge, collisionCheckDT)) {
added++;
edgesAdded++;
Edge *e = pool.construct(edge);
bool addedIntoTree = treeInterface.insertIntoTree(e);
if(!addedIntoTree) {
pool.destroy(e);
break;
}
e->updateParent(parent);
parent = e;
#ifdef WITHGRAPHICS
treeEdges.push_back(e);
#endif
if(agent.isGoal(e->end, goal)) {
dfpair(stdout, "solution cost", "%g", e->gCost());
std::vector<const Edge *> newSolution;
newSolution.push_back(e);
unsigned int edgeCount = 1;
while(newSolution.back()->parent != NULL) {
edgeCount++;
newSolution.push_back(newSolution.back()->parent);
}
dfpair(stdout, "solution length", "%u", edgeCount);
if(solutionCost < 0 || e->gCost() < solutionCost) {
std::reverse(newSolution.begin(), newSolution.end());
solution.clear();
solution.insert(solution.begin(), newSolution.begin(), newSolution.end());
}
foundGoal = true;
#ifdef WITHGRAPHICS
break;
#endif
return solution;
}
edge = agent.steerWithControl(edge.end, edge, steeringDT);
}
edgesRejected++;
++iterations;
if(iterationsAtATime > 0 && ++iterations > iterationsAtATime) break;
}
#ifdef WITHGRAPHICS
for(const Edge *edge : treeEdges) {
edge->draw(OpenGLWrapper::Color::Red());
}
for(const State &sample : samples) {
sample.draw();
}
if(solution.size() > 0) {
auto red = OpenGLWrapper::Color::Red();
for(const Edge *edge : solution) {
edge->draw(red);
}
agent.drawSolution(solution);
// if(poseNumber >= solution.size() * 2) poseNumber = -1;
// if(poseNumber >= 0)
// agent.animateSolution(solution, poseNumber++);
return solution;
}
#endif
#ifdef VREPPLUGIN
if(solution.size() > 0) {
if(agent.validateSolution(solution, goal)) {
fprintf(stderr, "VALID SOLUTION!\n");
} else {
fprintf(stderr, "INVALID SOLUTION!\n");
}
agent.animateSolution(solution);
}
#endif
dfpair(stdout, "solution cost", "-1");
dfpair(stdout, "solution length", "-1");
return std::vector<const Edge*>();
}
virtual void output(FILE *out) {
Arastar<D>::output(out);
dfpair(stdout, "wf", "%f", wcost);
dfpair(stdout, "wt", "%f", wtime);
dfpair(stdout, "lambda", "%f", lambda);
}
std::vector<const Edge*> query(const State &start, const State &goal, int iterationsAtATime = -1, bool firstInvocation = true) {
#ifdef WITHGRAPHICS
discretization.draw();
workspace.draw();
unsigned int iterations = 0;
#endif
unsigned int cellId = discretization.getCellId(start);
if(!forwardRegions[cellId]->inPDF) {
typename ProbabilityDensityFunction<RegionNode>::Element *pdfElem = NULL;
auto root = pool.construct(start);
forwardRegions[cellId]->addEdge(root);
pdfElem = forwardPDF.add(forwardRegions[cellId], forwardRegions[cellId]->getWeight()); discretization.setInPDF(cellId);
forwardRegions[cellId]->setInPDF(true, pdfElem->getId());
forwardTree.push_back(root);
forwardKDTree.insertPoint(root);
cellId = discretization.getCellId(goal);
root = pool.construct(goal);
backwardRegions[cellId]->addEdge(root);
pdfElem = backwardPDF.add(backwardRegions[cellId], backwardRegions[cellId]->getWeight()); discretization.setInPDF(cellId);
backwardRegions[cellId]->setInPDF(true, pdfElem->getId());
backwardTree.push_back(root);
backwardKDTree.insertPoint(root);
}
while(true) {
#ifdef WITHGRAPHICS
if(iterations++ > 10) { std::cin.ignore(); break; }
#endif
expand(forwardRegions, forwardPDF, forwardTree, forwardKDTree, goal);
expand(backwardRegions, backwardPDF, backwardTree, backwardKDTree, start);
auto solution = connection(forwardTree, backwardKDTree);
if(solution.size() > 0) {
if(!quiet) {
dfpair(stdout, "solution cost", "%g", solution.back()->gCost());
dfpair(stdout, "solution length", "%u", solution.size());
}
return solution;
}
// solution = connection(backwardTree, forwardKDTree);
// if(solution.size() > 0) return solution;
}
#ifdef WITHGRAPHICS
for(const Edge *edge : treeEdges) {
edge->draw(OpenGLWrapper::Color::Red());
}
treeEdges.clear();
for(const State &sample : samples) {
sample.draw();
}
samples.clear();
#endif
return std::vector<const Edge*>();
}
std::vector<const typename Agent::Edge *> query(const typename Agent::State &start, const typename Agent::State &goal, int iterationsAtATime = -1, bool firstInvocation = true) {
ompl::base::ScopedState<StateSpace> omplStart(spaceInfoPtr);
omplStart->agentEdge = new typename Agent::Edge(start);
omplStart->valid = true;
const typename Agent::StateVars &startStateVars = omplStart->agentEdge->getTreeStateVars();
for(unsigned int i = 0; i < startStateVars.size(); ++i) {
omplStart->values[i] = startStateVars[i];
}
agentGoal = new typename Agent::State(goal);
ompl::base::ScopedState<StateSpace> omplGoal(spaceInfoPtr);
omplGoal->agentEdge = new typename Agent::Edge(start);
omplGoal->valid = true;
const typename Agent::StateVars &goalStateVars = omplGoal->agentEdge->getTreeStateVars();;
omplGoal->values = new double[goalStateVars.size()];
for(unsigned int i = 0; i < goalStateVars.size(); ++i) {
omplGoal->values[i] = goalStateVars[i];
}
pdef->setStartAndGoalStates(omplStart, omplGoal);
kpiece->setProblemDefinition(pdef);
kpiece->setup();
ompl::base::PlannerTerminationCondition tc = ompl::base::PlannerTerminationCondition(boost::bind(&KPIECE::didFindGoal, this));
/*ompl::base::plannerOrTerminationCondition(
ompl::base::timedPlannerTerminationCondition(300),
ompl::base::PlannerTerminationCondition(boost::bind(&KPIECE::didFindGoal, this))); */
// ompl::base::PlannerStatus solved =
kpiece->solve(tc);
// if(solved) {
// fprintf(stderr, "found goal\n");
// ompl::base::PathPtr path = pdef->getSolutionPath();
// path->print(std::cout);
// } else {
// fprintf(stderr, "did not find goal\n");
// }
#ifdef WITHGRAPHICS
for(const typename Agent::Edge *edge : treeEdges) {
edge->draw(OpenGLWrapper::Color::Red());
}
for(const typename Agent::Edge *edge : rejectedTreeEdges) {
edge->draw(OpenGLWrapper::Color::Blue());
}
#endif
if(goalEdge != NULL) {
fprintf(stderr, "found goal\n");
dfpair(stdout, "solution cost", "%g", goalEdge->gCost());
std::vector<const typename Agent::Edge *> solution;
solution.push_back(goalEdge);
unsigned int edgeCount = 1;
while(solution.back()->parent != NULL) {
edgeCount++;
solution.push_back(solution.back()->parent);
}
dfpair(stdout, "solution length", "%u", edgeCount);
return solution;
}
dfpair(stdout, "solution cost", "-1");
dfpair(stdout, "solution length", "-1");
return std::vector<const typename Agent::Edge *>();
}
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);
}
void dfpairs() const {
dfpair(stdout, "samples generated", "%u", samplesGenerated);
dfpair(stdout, "samples rejected", "%u", samplesGenerated - samplesAdded);
dfpair(stdout, "edges generated", "%u", edgesGenerated);
dfpair(stdout, "edges rejected", "%u", edgesGenerated - edgesAdded);
}