bool ompl::geometric::SBL::isPathValid(TreeData &tree, Motion *motion)
{
std::vector<Motion *> mpath;
/* construct the solution path */
while (motion != nullptr)
{
mpath.push_back(motion);
motion = motion->parent;
}
/* check the path */
for (int i = mpath.size() - 1; i >= 0; --i)
if (!mpath[i]->valid)
{
if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
mpath[i]->valid = true;
else
{
removeMotion(tree, mpath[i]);
return false;
}
}
return true;
}
void ompl::geometric::LBKPIECE1::removeMotion(Discretization<Motion> &disc, Motion *motion)
{
/* remove from grid */
Discretization<Motion>::Coord coord;
projectionEvaluator_->computeCoordinates(motion->state, coord);
disc.removeMotion(motion, coord);
/* remove self from parent list */
if (motion->parent)
{
for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
if (motion->parent->children[i] == motion)
{
motion->parent->children.erase(motion->parent->children.begin() + i);
break;
}
}
/* remove children */
for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
{
motion->children[i]->parent = NULL;
removeMotion(disc, motion->children[i]);
}
freeMotion(motion);
}
void ompl::geometric::LazyRRT::removeMotion(Motion *motion)
{
nn_->remove(motion);
/* remove self from parent list */
if (motion->parent)
{
for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
if (motion->parent->children[i] == motion)
{
motion->parent->children.erase(motion->parent->children.begin() + i);
break;
}
}
/* remove children */
for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
{
motion->children[i]->parent = NULL;
removeMotion(motion->children[i]);
}
if (motion->state)
si_->freeState(motion->state);
delete motion;
}
//-----------------------------------------------------------------------------
// purgeExcessMotion()
//-----------------------------------------------------------------------------
void LLMotionController::purgeExcessMotions()
{
if (mLoadedMotions.size() > MAX_MOTION_INSTANCES)
{
// clean up deprecated motions
for (motion_set_t::iterator deprecated_motion_it = mDeprecatedMotions.begin();
deprecated_motion_it != mDeprecatedMotions.end(); )
{
motion_set_t::iterator cur_iter = deprecated_motion_it++;
LLMotion* cur_motionp = *cur_iter;
if (!isMotionActive(cur_motionp))
{
// Motion is deprecated so we know it's not cannonical,
// we can safely remove the instance
removeMotionInstance(cur_motionp); // modifies mDeprecatedMotions
mDeprecatedMotions.erase(cur_iter);
}
}
}
std::set<LLUUID> motions_to_kill;
if (mLoadedMotions.size() > MAX_MOTION_INSTANCES)
{
// too many motions active this frame, kill all blenders
mPoseBlender.clearBlenders();
for (motion_set_t::iterator loaded_motion_it = mLoadedMotions.begin();
loaded_motion_it != mLoadedMotions.end();
++loaded_motion_it)
{
LLMotion* cur_motionp = *loaded_motion_it;
// motion isn't playing, delete it
if (!isMotionActive(cur_motionp))
{
motions_to_kill.insert(cur_motionp->getID());
}
}
}
// clean up all inactive, loaded motions
for (std::set<LLUUID>::iterator motion_it = motions_to_kill.begin();
motion_it != motions_to_kill.end();
++motion_it)
{
// look up the motion again by ID to get canonical instance
// and kill it only if that one is inactive
LLUUID motion_id = *motion_it;
LLMotion* motionp = findMotion(motion_id);
if (motionp && !isMotionActive(motionp))
{
removeMotion(motion_id);
}
}
if (mLoadedMotions.size() > 2*MAX_MOTION_INSTANCES)
{
LL_WARNS_ONCE("Animation") << "> " << 2*MAX_MOTION_INSTANCES << " Loaded Motions" << llendl;
}
}
void ompl::geometric::SBL::removeMotion(TreeData &tree, Motion *motion)
{
/* remove from grid */
Grid<MotionInfo>::Coord coord(projectionEvaluator_->getDimension());
projectionEvaluator_->computeCoordinates(motion->state, coord);
Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
if (cell)
{
for (unsigned int i = 0; i < cell->data.size(); ++i)
{
if (cell->data[i] == motion)
{
cell->data.erase(cell->data.begin() + i);
tree.size--;
break;
}
}
if (cell->data.empty())
{
tree.pdf.remove(cell->data.elem_);
tree.grid.remove(cell);
tree.grid.destroyCell(cell);
}
else
{
tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
}
}
/* remove self from parent list */
if (motion->parent)
{
for (unsigned int i = 0; i < motion->parent->children.size(); ++i)
{
if (motion->parent->children[i] == motion)
{
motion->parent->children.erase(motion->parent->children.begin() + i);
break;
}
}
}
/* remove children */
for (auto &i : motion->children)
{
i->parent = nullptr;
removeMotion(tree, i);
}
if (motion->state)
si_->freeState(motion->state);
delete motion;
}
void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen)
{
/* remove from grid */
seen[motion] = true;
Grid<MotionInfo>::Coord coord;
projectionEvaluator_->computeCoordinates(motion->state, coord);
Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
if (cell)
{
for (unsigned int i = 0 ; i < cell->data.size(); ++i)
if (cell->data[i] == motion)
{
cell->data.erase(cell->data.begin() + i);
tree.size--;
break;
}
if (cell->data.empty())
{
tree.pdf.remove(cell->data.elem_);
tree.grid.remove(cell);
tree.grid.destroyCell(cell);
}
else
{
tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
}
}
/* remove self from parent list */
if (motion->parent)
{
for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
if (motion->parent->children[i] == motion)
{
motion->parent->children.erase(motion->parent->children.begin() + i);
break;
}
}
/* remove children */
for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
{
motion->children[i]->parent = NULL;
removeMotion(tree, motion->children[i], seen);
}
if (motion->state)
si_->freeState(motion->state);
delete motion;
}
//-----------------------------------------------------------------------------
// purgeExcessMotion()
//-----------------------------------------------------------------------------
void LLMotionController::purgeExcessMotions()
{
//<singu>
// The old code attempted to remove non-active motions from mDeprecatedMotions,
// but that is nonsense: there are no motions in mDeprecatedMotions that are not active.
if (mLoadedMotions.size() <= MAX_MOTION_INSTANCES)
{
// Speed up, no need to create motions_to_kill.
return;
}
//</singu>
std::set<LLUUID> motions_to_kill;
if (1) // Singu: leave indentation alone...
{
// too many motions active this frame, kill all blenders
mPoseBlender.clearBlenders();
for (motion_set_t::iterator loaded_motion_it = mLoadedMotions.begin();
loaded_motion_it != mLoadedMotions.end();
++loaded_motion_it)
{
LLMotion* cur_motionp = *loaded_motion_it;
// motion isn't playing, delete it
if (!isMotionActive(cur_motionp))
{
motions_to_kill.insert(cur_motionp->getID());
}
}
}
// clean up all inactive, loaded motions
for (std::set<LLUUID>::iterator motion_it = motions_to_kill.begin();
motion_it != motions_to_kill.end();
++motion_it)
{
// look up the motion again by ID to get canonical instance
// and kill it only if that one is inactive
LLUUID motion_id = *motion_it;
LLMotion* motionp = findMotion(motion_id);
if (motionp && !isMotionActive(motionp))
{
removeMotion(motion_id);
}
}
if (mLoadedMotions.size() > 2*MAX_MOTION_INSTANCES)
{
LL_WARNS_ONCE("Animation") << "> " << 2*MAX_MOTION_INSTANCES << " Loaded Motions" << llendl;
}
}
bool ompl::geometric::LBKPIECE1::isPathValid(Discretization<Motion> &disc, Motion *motion, base::State *temp)
{
std::vector<Motion*> mpath;
/* construct the solution path */
while (motion != NULL)
{
mpath.push_back(motion);
motion = motion->parent;
}
std::pair<base::State*, double> lastValid;
lastValid.first = temp;
/* check the path */
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
if (!mpath[i]->valid)
{
if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
mpath[i]->valid = true;
else
{
Motion *parent = mpath[i]->parent;
removeMotion(disc, mpath[i]);
// add the valid part of the path, if sufficiently long
if (lastValid.second > minValidPathFraction_)
{
Motion* reAdd = new Motion(si_);
si_->copyState(reAdd->state, lastValid.first);
reAdd->parent = parent;
reAdd->root = parent->root;
parent->children.push_back(reAdd);
reAdd->valid = true;
Discretization<Motion>::Coord coord;
projectionEvaluator_->computeCoordinates(reAdd->state, coord);
disc.addMotion(reAdd, coord);
}
return false;
}
}
return true;
}
void ompl::LazyRRT::removeMotion(Motion_t motion)
{
assert(m_nn.remove(motion));
/* remove self from parent list */
if (motion->parent)
{
for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
if (motion->parent->children[i] == motion)
{
motion->parent->children.erase(motion->parent->children.begin() + i);
break;
}
}
/* remove children */
for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
{
motion->children[i]->parent = NULL;
removeMotion(motion->children[i]);
}
}
ompl::base::PlannerStatus ompl::geometric::LazyRRT::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->valid = true;
nn_->add(motion);
}
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", 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 = NULL;
double distsol = -1.0;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
bool solutionFound = false;
while (ptc == false && !solutionFound)
{
/* 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);
assert(nmotion != rmotion);
base::State *dstate = rstate;
/* find state to add */
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
nmotion->children.push_back(motion);
nn_->add(motion);
double dist = 0.0;
if (goal->isSatisfied(motion->state, &dist))
{
distsol = dist;
solution = motion;
solutionFound = true;
lastGoalMotion_ = solution;
// Check that the solution is valid:
// construct the solution path
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
// check each segment along the path for validity
for (int i = mpath.size() - 1 ; i >= 0 && solutionFound; --i)
if (!mpath[i]->valid)
{
if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
mpath[i]->valid = true;
else
{
removeMotion(mpath[i]);
solutionFound = false;
lastGoalMotion_ = NULL;
}
}
if (solutionFound)
{
// set the solution path
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, distsol, getName());
}
}
}
si_->freeState(xstate);
si_->freeState(rstate);
delete rmotion;
OMPL_INFORM("%s: Created %u states", getName().c_str(), nn_->size());
return solutionFound ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
{
RNG rng;
std::vector<Motion*> solution;
base::State *xstate = si_->allocState();
bool startTree = rng.uniformBool();
while (!sol->found && ptc == false)
{
bool retry = true;
while (retry && !sol->found && ptc == false)
{
removeList_.lock.lock();
if (!removeList_.motions.empty())
{
if (loopLock_.try_lock())
{
retry = false;
std::map<Motion*, bool> seen;
for (unsigned int i = 0 ; i < removeList_.motions.size() ; ++i)
if (seen.find(removeList_.motions[i].motion) == seen.end())
removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen);
removeList_.motions.clear();
loopLock_.unlock();
}
}
else
retry = false;
removeList_.lock.unlock();
}
if (sol->found || ptc)
break;
loopLockCounter_.lock();
if (loopCounter_ == 0)
loopLock_.lock();
loopCounter_++;
loopLockCounter_.unlock();
TreeData &tree = startTree ? tStart_ : tGoal_;
startTree = !startTree;
TreeData &otherTree = startTree ? tStart_ : tGoal_;
Motion *existing = selectMotion(rng, tree);
if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
continue;
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state, xstate);
motion->parent = existing;
motion->root = existing->root;
existing->lock.lock();
existing->children.push_back(motion);
existing->lock.unlock();
addMotion(tree, motion);
if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
{
sol->lock.lock();
if (!sol->found)
{
sol->found = true;
PathGeometric *path = new PathGeometric(si_);
for (unsigned int i = 0 ; i < solution.size() ; ++i)
path->append(solution[i]->state);
pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName());
}
sol->lock.unlock();
}
loopLockCounter_.lock();
loopCounter_--;
if (loopCounter_ == 0)
loopLock_.unlock();
loopLockCounter_.unlock();
}
si_->freeState(xstate);
}
bool ompl::LazyRRT::solve(double solveTime)
{
SpaceInformationKinematic_t si = dynamic_cast<SpaceInformationKinematic_t>(m_si);
SpaceInformationKinematic::GoalRegionKinematic_t goal_r = dynamic_cast<SpaceInformationKinematic::GoalRegionKinematic_t>(si->getGoal());
SpaceInformationKinematic::GoalStateKinematic_t goal_s = dynamic_cast<SpaceInformationKinematic::GoalStateKinematic_t>(si->getGoal());
unsigned int dim = si->getStateDimension();
if (!goal_s && !goal_r)
{
m_msg.error("Unknown type of goal (or goal undefined)");
return false;
}
time_utils::Time endTime = time_utils::Time::now() + time_utils::Duration(solveTime);
if (m_nn.size() == 0)
{
for (unsigned int i = 0 ; i < m_si->getStartStateCount() ; ++i)
{
Motion_t motion = new Motion(dim);
si->copyState(motion->state, dynamic_cast<SpaceInformationKinematic::StateKinematic_t>(si->getStartState(i)));
if (si->isValid(motion->state))
{
motion->valid = true;
m_nn.add(motion);
}
else
{
m_msg.error("Initial state is in collision!");
delete motion;
}
}
}
if (m_nn.size() == 0)
{
m_msg.error("There are no valid initial states!");
return false;
}
m_msg.inform("Starting with %u states", m_nn.size());
std::vector<double> range(dim);
for (unsigned int i = 0 ; i < dim ; ++i)
range[i] = m_rho * (si->getStateComponent(i).maxValue - si->getStateComponent(i).minValue);
Motion_t solution = NULL;
double distsol = -1.0;
Motion_t rmotion = new Motion(dim);
SpaceInformationKinematic::StateKinematic_t rstate = rmotion->state;
SpaceInformationKinematic::StateKinematic_t xstate = new SpaceInformationKinematic::StateKinematic(dim);
RETRY:
while (time_utils::Time::now() < endTime)
{
/* sample random state (with goal biasing) */
if (goal_s && random_utils::uniform(&m_rngState, 0.0, 1.0) < m_goalBias)
si->copyState(rstate, goal_s->state);
else
si->sample(rstate);
/* find closest state in the tree */
Motion_t nmotion = m_nn.nearest(rmotion);
assert(nmotion != rmotion);
/* find state to add */
for (unsigned int i = 0 ; i < dim ; ++i)
{
double diff = rmotion->state->values[i] - nmotion->state->values[i];
xstate->values[i] = fabs(diff) < range[i] ? rmotion->state->values[i] : nmotion->state->values[i] + diff * m_rho;
}
/* create a motion */
Motion_t motion = new Motion(dim);
si->copyState(motion->state, xstate);
motion->parent = nmotion;
nmotion->children.push_back(motion);
m_nn.add(motion);
double dist = 0.0;
if (goal_r->isSatisfied(motion->state, &dist))
{
distsol = dist;
solution = motion;
break;
}
}
if (solution != NULL)
{
/* construct the solution path */
std::vector<Motion_t> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
/* check the path */
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
if (!mpath[i]->valid)
{
if (si->checkMotionSubdivision(mpath[i]->parent->state, mpath[i]->state))
mpath[i]->valid = true;
else
{
removeMotion(mpath[i]);
goto RETRY;
}
}
/*set the solution path */
SpaceInformationKinematic::PathKinematic_t path = new SpaceInformationKinematic::PathKinematic(m_si);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
{
SpaceInformationKinematic::StateKinematic_t st = new SpaceInformationKinematic::StateKinematic(dim);
si->copyState(st, mpath[i]->state);
path->states.push_back(st);
}
goal_r->setDifference(distsol);
goal_r->setSolutionPath(path);
}
delete xstate;
delete rmotion;
m_msg.inform("Created %u states", m_nn.size());
return goal_r->isAchieved();
}
bool ompl::geometric::LazyRRT::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->valid = true;
nn_->add(motion);
}
if (nn_->size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
msg_.inform("Starting with %u states", nn_->size());
Motion *solution = NULL;
double distsol = -1.0;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
RETRY:
while (ptc() == false)
{
/* 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);
assert(nmotion != rmotion);
base::State *dstate = rstate;
/* find state to add */
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
/* create a motion */
Motion *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
nmotion->children.push_back(motion);
nn_->add(motion);
double dist = 0.0;
if (goal->isSatisfied(motion->state, &dist))
{
distsol = dist;
solution = motion;
break;
}
}
bool solved = false;
if (solution != NULL)
{
/* construct the solution path */
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
/* check the path */
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
if (!mpath[i]->valid)
{
if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
mpath[i]->valid = true;
else
{
removeMotion(mpath[i]);
goto RETRY;
}
}
/* set the solution path */
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
goal->addSolutionPath(base::PathPtr(path), false, distsol);
solved = true;
}
si_->freeState(xstate);
si_->freeState(rstate);
delete rmotion;
msg_.inform("Created %u states", nn_->size());
return solved;
}