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);
}
