Beispiel #1
0
 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());
 }
Beispiel #2
0
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;
  }
  */
}
Beispiel #3
0
///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);
}