int PointToSetMotionPlanner::AddMilestone(const Config& q) { int n=mp->AddMilestone(q); if(goalSpace->IsFeasible(q)) goalNodes.push_back(n); return n; }
int PointToSetMotionPlanner::PlanMore() { if(mp->CanAddMilestone()) { sampleGoalCounter++; if(sampleGoalCounter >= sampleGoalPeriod*((int)goalNodes.size()+1)) { sampleGoalCounter = 0; Config q; if(SampleGoal(q)) { return AddMilestone(q); } else return -1; } } int res = mp->PlanMore(); if(res >= 0) { Config q; mp->GetMilestone(res,q); if(goalSpace->IsFeasible(q)) goalNodes.push_back(res); } return res; }
bool PointToSetMotionPlanner::SampleGoal(Config& q) { goalSpace->Sample(q); return goalSpace->IsFeasible(q); }