OMPLGoalState::OMPLGoalState(const ob::SpaceInformationPtr &si,
OMPLProblem_ptr prob)
: ob::GoalState(si), prob_(prob)
{
type_ = ob::GOAL_STATE;
state_space_ = si->getStateSpace();
setThreshold(std::numeric_limits<double>::epsilon());
}
OMPLCSpace::OMPLCSpace( const ob::SpaceInformationPtr& si)
{
nD = si->getStateDimension();
ob::StateSpacePtr space = si->getStateSpace();
ob::RealVectorStateSpace* vs = dynamic_cast<ob::RealVectorStateSpace*>(&*space);
if(vs) {
qMin.copy(vs->getBounds().low);
qMax.copy(vs->getBounds().high);
}
else {
qMin.resize(nD);
qMax.resize(nD);
qMin.set(0.0);
qMax.set(1.0);
}
space->setup();
resolution = space->getLongestValidSegmentLength();
sampler_ = si->allocStateSampler();
si_ = si;
stemp1 = si->allocState();
stemp2 = si->allocState();
stemp3 = si->allocState();
/* debugging...
for(int i=0;i<100000;i++) {
sampler_->sampleUniform(stemp1);
sampler_->sampleUniform(stemp2);
bool visible = si_->checkMotion(stemp1,stemp2);
Config a,b;
FromOMPL(stemp1,a);
FromOMPL(stemp2,b);
EdgePlanner* e= LocalPlanner(a,b);
assert(visible == e->IsVisible());
delete e;
}
*/
}
///helper: convert an OMPL state to a Config
Config FromOMPL(const ob::SpaceInformationPtr& si_,const ob::State* s)
{
vector<Real> vq;
si_->getStateSpace()->copyToReals(vq,s);
return Config(vq);
}