unsigned int ManipulatorSpaceInformation::propagateWhileValid(const ompl::base::State *state, const ompl::control::Control *control, int steps, std::vector<ompl::base::State*> &result, bool alloc) const { double signedStepSize = steps > 0 ? stepSize_ : -stepSize_; steps = abs(steps); if (alloc) result.resize(steps); else { if (result.empty()) return 0; steps = std::min(steps, (int)result.size()); } int st = 0; if (st < steps) { if (alloc) result[st] = allocState(); statePropagator_->propagate(state, control, signedStepSize, result[st]); if (checkMotion(state, result[st])) { ++st; while (st < steps) { if (alloc) result[st] = allocState(); statePropagator_->propagate(result[st-1], control, signedStepSize, result[st]); if (!checkMotion(result[st-1], result[st])) { if (alloc) { freeState(result[st]); result.resize(st); } break; } else ++st; } } else { if (alloc) { freeState(result[st]); result.resize(st); } } } return st; }
unsigned int ompl::base::SpaceInformation::randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State *> &states, bool alloc) const { if (alloc) { states.resize(steps); for (unsigned int i = 0; i < steps; ++i) states[i] = allocState(); } else if (states.size() < steps) steps = states.size(); const State *prev = start; std::pair<State *, double> lastValid; unsigned int j = 0; for (unsigned int i = 0; i < steps; ++i) { sss->sampleUniform(states[j]); lastValid.first = states[j]; if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon()) prev = states[j++]; } return j; }
void AgiEngine::checkAllMotions() { VtEntry *v; for (v = _game.viewTable; v < &_game.viewTable[MAX_VIEWTABLE]; v++) { if ((v->flags & (ANIMATED | UPDATE | DRAWN)) == (ANIMATED | UPDATE | DRAWN) && v->stepTimeCount == 1) { checkMotion(v); } } }
void AgiEngine::checkAllMotions() { ScreenObjEntry *screenObj; for (screenObj = _game.screenObjTable; screenObj < &_game.screenObjTable[SCREENOBJECTS_MAX]; screenObj++) { if ((screenObj->flags & (fAnimated | fUpdate | fDrawn)) == (fAnimated | fUpdate | fDrawn) && screenObj->stepTimeCount == 1) { checkMotion(screenObj); } } }
double ompl::base::SpaceInformation::averageValidMotionLength(unsigned int attempts) const { // take the square root here because we in fact have a nested for loop // where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop // too) attempts = std::max((unsigned int)floor(sqrt((double)attempts) + 0.5), 2u); StateSamplerPtr ss = allocStateSampler(); auto uvss(std::make_shared<UniformValidStateSampler>(this)); uvss->setNrAttempts(attempts); State *s1 = allocState(); State *s2 = allocState(); std::pair<State *, double> lastValid; lastValid.first = nullptr; double d = 0.0; unsigned int count = 0; for (unsigned int i = 0; i < attempts; ++i) if (uvss->sample(s1)) { ++count; ss->sampleUniform(s2); if (checkMotion(s1, s2, lastValid)) d += distance(s1, s2); else d += distance(s1, s2) * lastValid.second; } freeState(s2); freeState(s1); if (count > 0) return d / (double)count; else return 0.0; }
void ompl::geometric::LBTRRT::considerEdge(Motion *parent, Motion *child, double c) { // optimization - check if the bounded approximation invariant // will be violated after the edge insertion (at least for the child node) // if this is the case - perform the local planning // this prevents the update of the graph due to the edge insertion and then the re-update as it is removed double potential_cost = parent->costLb_ + c; if (child->costApx_ > (1 + epsilon_) * potential_cost) if (!checkMotion(parent, child)) return; // update lowerBoundGraph_ std::list<std::size_t> affected; lowerBoundGraph_.addEdge(parent->id_, child->id_, c, true, affected); // now, check if the bounded apprimation invariant has been violated for each affected vertex // insert them into a priority queue ordered according to the lb cost std::list<std::size_t>::iterator iter; IsLessThanLB isLessThanLB(this); Lb_queue queue(isLessThanLB); for (iter = affected.begin(); iter != affected.end(); ++iter) { Motion *m = getMotion(*iter); m->costLb_ = lowerBoundGraph_.getShortestPathCost(*iter); if (m->costApx_ > (1 + epsilon_) * m->costLb_) queue.insert(m); } while (queue.empty() == false) { Motion *motion = *(queue.begin()); queue.erase(queue.begin()); if (motion->costApx_ > (1 + epsilon_) * motion->costLb_) { Motion *potential_parent = getMotion(lowerBoundGraph_.getShortestPathParent(motion->id_)); if (checkMotion(potential_parent, motion)) { double delta = lazilyUpdateApxParent(motion, potential_parent); updateChildCostsApx(motion, delta); } else { affected.clear(); lowerBoundGraph_.removeEdge(potential_parent->id_, motion->id_, true, affected); for (iter = affected.begin(); iter != affected.end(); ++iter) { Motion *affected = getMotion(*iter); Lb_queue_iter lb_queue_iter = queue.find(affected); if (lb_queue_iter != queue.end()) { queue.erase(lb_queue_iter); affected->costLb_ = lowerBoundGraph_.getShortestPathCost(affected->id_); if (affected->costApx_ > (1 + epsilon_) * affected->costLb_) queue.insert(affected); } else { affected->costLb_ = lowerBoundGraph_.getShortestPathCost(affected->id_); } } motion->costLb_ = lowerBoundGraph_.getShortestPathCost(motion->id_); if (motion->costApx_ > (1 + epsilon_) * motion->costLb_) queue.insert(motion); // optimization - we can remove the opposite edge lowerBoundGraph_.removeEdge(motion->id_, potential_parent->id_, false, affected); } } } return; }
ompl::base::PlannerStatus ompl::geometric::LBTRRT::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); // update goal and check validity base::Goal *goal = pdef_->getGoal().get(); base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal); if (!goal) { OMPL_ERROR("%s: Goal undefined", getName().c_str()); return base::PlannerStatus::INVALID_GOAL; } // update start and check validity while (const base::State *st = pis_.nextStart()) { Motion *motion = new Motion(si_); si_->copyState(motion->state_, st); motion->id_ = nn_->size(); idToMotionMap_.push_back(motion); nn_->add(motion); lowerBoundGraph_.addVertex(motion->id_); } if (nn_->size() == 0) { OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); return base::PlannerStatus::INVALID_START; } if (nn_->size() > 1) { OMPL_ERROR("%s: There are multiple start states - currently not supported!", getName().c_str()); return base::PlannerStatus::INVALID_START; } if (!sampler_) sampler_ = si_->allocStateSampler(); OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size()); Motion *solution = lastGoalMotion_; Motion *approxSol = nullptr; double approxdif = std::numeric_limits<double>::infinity(); // e*(1+1/d) K-nearest constant, as used in RRT* double k_rrg = boost::math::constants::e<double>() + boost::math::constants::e<double>() / (double)si_->getStateDimension(); Motion *rmotion = new Motion(si_); base::State *rstate = rmotion->state_; base::State *xstate = si_->allocState(); unsigned int statesGenerated = 0; bestCost_ = lastGoalMotion_ ? lastGoalMotion_->costApx_ : std::numeric_limits<double>::infinity(); while (ptc() == false) { iterations_++; /* sample random state (with goal biasing) */ if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample()) goal_s->sampleGoal(rstate); else sampler_->sampleUniform(rstate); /* find closest state in the tree */ Motion *nmotion = nn_->nearest(rmotion); base::State *dstate = rstate; /* find state to add */ double d = si_->distance(nmotion->state_, rstate); if (d == 0) // this takes care of the case that the goal is a single point and we re-sample it multiple times continue; if (d > maxDistance_) { si_->getStateSpace()->interpolate(nmotion->state_, rstate, maxDistance_ / d, xstate); dstate = xstate; } if (checkMotion(nmotion->state_, dstate)) { statesGenerated++; /* create a motion */ Motion *motion = new Motion(si_); si_->copyState(motion->state_, dstate); /* update fields */ double distN = distanceFunction(nmotion, motion); motion->id_ = nn_->size(); idToMotionMap_.push_back(motion); lowerBoundGraph_.addVertex(motion->id_); motion->parentApx_ = nmotion; std::list<std::size_t> dummy; lowerBoundGraph_.addEdge(nmotion->id_, motion->id_, distN, false, dummy); motion->costLb_ = nmotion->costLb_ + distN; motion->costApx_ = nmotion->costApx_ + distN; nmotion->childrenApx_.push_back(motion); std::vector<Motion*> nnVec; unsigned int k = std::ceil(k_rrg * log((double)(nn_->size() + 1))); nn_->nearestK(motion, k, nnVec); nn_->add(motion); // if we add the motion before the nearestK call, we will get ourselves... IsLessThan isLessThan(this, motion); std::sort(nnVec.begin(), nnVec.end(), isLessThan); //-------------------------------------------------// // Rewiring Part (i) - find best parent of motion // //-------------------------------------------------// if (motion->parentApx_ != nnVec.front()) { for (std::size_t i(0); i < nnVec.size(); ++i) { Motion *potentialParent = nnVec[i]; double dist = distanceFunction(potentialParent, motion); considerEdge(potentialParent, motion, dist); } } //------------------------------------------------------------------// // Rewiring Part (ii) // // check if motion may be a better parent to one of its neighbors // //------------------------------------------------------------------// for (std::size_t i(0); i < nnVec.size(); ++i) { Motion *child = nnVec[i]; double dist = distanceFunction(motion, child); considerEdge(motion, child, dist); } double dist = 0.0; bool sat = goal->isSatisfied(motion->state_, &dist); if (sat) { approxdif = dist; solution = motion; } if (dist < approxdif) { approxdif = dist; approxSol = motion; } if (solution != nullptr && bestCost_ != solution->costApx_) { OMPL_INFORM("%s: approximation cost = %g", getName().c_str(), solution->costApx_); bestCost_ = solution->costApx_; } } } bool solved = false; bool approximate = false; if (solution == nullptr) { solution = approxSol; approximate = true; } if (solution != nullptr) { lastGoalMotion_ = solution; /* construct the solution path */ std::vector<Motion*> mpath; while (solution != nullptr) { mpath.push_back(solution); solution = solution->parentApx_; } /* set the solution path */ PathGeometric *path = new PathGeometric(si_); for (int i = mpath.size() - 1 ; i >= 0 ; --i) path->append(mpath[i]->state_); // Add the solution path. base::PathPtr bpath(path); base::PlannerSolution psol(bpath); psol.setPlannerName(getName()); if (approximate) psol.setApproximate(approxdif); pdef_->addSolutionPath(psol); solved = true; } si_->freeState(xstate); if (rmotion->state_) si_->freeState(rmotion->state_); delete rmotion; OMPL_INFORM("%s: Created %u states", getName().c_str(), statesGenerated); return base::PlannerStatus(solved, approximate); }