ompl::geometric::BiTRRT::GrowResult ompl::geometric::BiTRRT::extendTree(Motion* nearest, TreeData& tree, Motion* toMotion, Motion*& result)
{
bool reach = true;
// Compute the state to extend toward
double d = si_->distance(nearest->state, toMotion->state);
// Truncate the random state to be no more than maxDistance_ from nearest neighbor
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nearest->state, toMotion->state, maxDistance_ / d, toMotion->state);
d = maxDistance_;
reach = false;
}
// Validating the motion
// If we are in the goal tree, we validate the motion in reverse
// si_->checkMotion assumes that the first argument is valid, so we must check this explicitly
// If the motion is valid, check the probabilistic transition test and the
// expansion control to ensure high quality nodes are added.
bool validMotion = (tree == tStart_ ? si_->checkMotion(nearest->state, toMotion->state) :
si_->isValid(toMotion->state) && si_->checkMotion(toMotion->state, nearest->state)) &&
transitionTest(opt_->motionCost(nearest->state, toMotion->state)) &&
minExpansionControl(d);
if (validMotion)
{
result = addMotion(toMotion->state, tree, nearest);
return reach ? SUCCESS : ADVANCED;
}
return FAILED;
}
ompl::base::PlannerStatus
ompl::geometric::TRRT::solve(const base::PlannerTerminationCondition &plannerTerminationCondition)
{
// Basic error checking
checkValidity();
// Goal information
base::Goal *goal = pdef_->getGoal().get();
auto *goalRegion = dynamic_cast<base::GoalSampleableRegion *>(goal);
// Input States ---------------------------------------------------------------------------------
// Loop through valid input states and add to tree
while (const base::State *state = pis_.nextStart())
{
// Allocate memory for a new start state motion based on the "space-information"-size
auto *motion = new Motion(si_);
// Copy destination <= source
si_->copyState(motion->state, state);
// Set cost for this start state
motion->cost = opt_->stateCost(motion->state);
if (nearestNeighbors_->size() == 0) // do not overwrite best/worst from previous call to solve
worstCost_ = bestCost_ = motion->cost;
// Add start motion to the tree
nearestNeighbors_->add(motion);
}
// Check that input states exist
if (nearestNeighbors_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
// Create state sampler if this is TRRT's first run
if (!sampler_)
sampler_ = si_->allocStateSampler();
// Debug
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(),
nearestNeighbors_->size());
// Solver variables ------------------------------------------------------------------------------------
// the final solution
Motion *solution = nullptr;
// the approximate solution, returned if no final solution found
Motion *approxSolution = nullptr;
// track the distance from goal to closest solution yet found
double approxDifference = std::numeric_limits<double>::infinity();
// distance between states - the intial state and the interpolated state (may be the same)
double randMotionDistance;
// Create random motion and a pointer (for optimization) to its state
auto *randMotion = new Motion(si_);
Motion *nearMotion;
// STATES
// The random state
base::State *randState = randMotion->state;
// The new state that is generated between states *to* and *from*
base::State *interpolatedState = si_->allocState(); // Allocates "space information"-sized memory for a state
// The chosen state btw rand_state and interpolated_state
base::State *newState;
// Begin sampling --------------------------------------------------------------------------------------
while (plannerTerminationCondition() == false)
{
// I.
// Sample random state (with goal biasing probability)
if (goalRegion && rng_.uniform01() < goalBias_ && goalRegion->canSample())
{
// Bias sample towards goal
goalRegion->sampleGoal(randState);
}
else
{
// Uniformly Sample
sampler_->sampleUniform(randState);
}
// II.
// Find closest state in the tree
nearMotion = nearestNeighbors_->nearest(randMotion);
// III.
// Distance from near state q_n to a random state
randMotionDistance = si_->distance(nearMotion->state, randState);
// Check if the rand_state is too far away
if (randMotionDistance > maxDistance_)
{
// Computes the state that lies at time t in [0, 1] on the segment that connects *from* state to *to* state.
// The memory location of *state* is not required to be different from the memory of either *from* or *to*.
si_->getStateSpace()->interpolate(nearMotion->state, randState, maxDistance_ / randMotionDistance,
interpolatedState);
// Update the distance between near and new with the interpolated_state
randMotionDistance = si_->distance(nearMotion->state, interpolatedState);
// Use the interpolated state as the new state
newState = interpolatedState;
}
else // Random state is close enough
newState = randState;
// IV.
// this stage integrates collision detections in the presence of obstacles and checks for collisions
if (!si_->checkMotion(nearMotion->state, newState))
continue; // try a new sample
// Minimum Expansion Control
// A possible side effect may appear when the tree expansion toward unexplored regions remains slow, and the
// new nodes contribute only to refine already explored regions.
if (!minExpansionControl(randMotionDistance))
continue; // give up on this one and try a new sample
base::Cost childCost = opt_->stateCost(newState);
// Only add this motion to the tree if the transition test accepts it
if (!transitionTest(opt_->motionCost(nearMotion->state, newState)))
continue; // give up on this one and try a new sample
// V.
// Create a motion
auto *motion = new Motion(si_);
si_->copyState(motion->state, newState);
motion->parent = nearMotion; // link q_new to q_near as an edge
motion->cost = childCost;
// Add motion to data structure
nearestNeighbors_->add(motion);
if (opt_->isCostBetterThan(motion->cost, bestCost_)) // motion->cost is better than the existing best
bestCost_ = motion->cost;
if (opt_->isCostBetterThan(worstCost_, motion->cost)) // motion->cost is worse than the existing worst
worstCost_ = motion->cost;
// VI.
// Check if this motion is the goal
double distToGoal = 0.0;
bool isSatisfied = goal->isSatisfied(motion->state, &distToGoal);
if (isSatisfied)
{
approxDifference = distToGoal; // the tolerated error distance btw state and goal
solution = motion; // set the final solution
break;
}
// Is this the closest solution we've found so far
if (distToGoal < approxDifference)
{
approxDifference = distToGoal;
approxSolution = motion;
}
} // end of solver sampling loop
// Finish solution processing --------------------------------------------------------------------
bool solved = false;
bool approximate = false;
// Substitute an empty solution with the best approximation
if (solution == nullptr)
{
solution = approxSolution;
approximate = true;
}
// Generate solution path for real/approx solution
if (solution != nullptr)
{
lastGoalMotion_ = solution;
// construct the solution path
std::vector<Motion *> mpath;
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
auto path(std::make_shared<PathGeometric>(si_));
for (int i = mpath.size() - 1; i >= 0; --i)
path->append(mpath[i]->state);
pdef_->addSolutionPath(path, approximate, approxDifference, getName());
solved = true;
}
// Clean up ---------------------------------------------------------------------------------------
si_->freeState(interpolatedState);
if (randMotion->state)
si_->freeState(randMotion->state);
delete randMotion;
OMPL_INFORM("%s: Created %u states", getName().c_str(), nearestNeighbors_->size());
return base::PlannerStatus(solved, approximate);
}
ompl::geometric::TRRTConnect::ExtendResult
ompl::geometric::TRRTConnect::extend(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion) {
++extendCount;
Motion *nmotion;
/*if (tree.stateInBoxZos_) {
//Among the nearest K nodes in the tree, find state with lowest cost to go
//The returned nmotion is collision-free
//If no collision-free motion is found, NULL is returned
nmotion = minCostNeighbor(tree,tgi,rmotion);
if (!nmotion) return TRAPPED;
} else {*/
//Find nearest node in the tree
//The returned motion has not been checked for collisions
//It never returns NULL
nmotion = tree->nearest(rmotion);
//}
//State to add
base::State *dstate;
//Distance from near state to random state
double d(si_->distance(nmotion->state,rmotion->state));
//Check if random state is too far away
if (d > maxDistance_) {
si_->getStateSpace()->interpolate(nmotion->state,rmotion->state,
maxDistance_/d,tgi.xstate);
//Use the interpolated state as the new state
dstate = tgi.xstate;
} else {
//Random state is close enough
dstate = rmotion->state;
}
//Check for collisions
//if (!tree.stateInBoxZos_) {
//If we are in the start tree, we just check the motion like we normally do.
//If we are in the goal tree, we need to check the motion in reverse,
//but checkMotion() assumes the first state it receives as argument is valid,
//so we check that one first.
bool validMotion(tree.start_ ? si_->checkMotion(nmotion->state,dstate) :
(si_->getStateValidityChecker()->isValid(dstate) &&
si_->checkMotion(dstate,nmotion->state)));
if (!validMotion) return TRAPPED;
//}
//Minimum Expansion Control
//A possible side effect may appear when the tree expansion towards
//unexplored regions remains slow, and the new nodes contribute
//only to refine already explored regions.
if (!minExpansionControl(d,tree)) {
return TRAPPED;//Give up on this one and try a new sample
}
//Compute motion cost
base::Cost cost;
if (tree.stateInBoxZos_) {
if (tree.start_) {
cost = opt_->motionCost(nmotion->state,dstate);
} else {
cost = opt_->motionCost(dstate,nmotion->state);
}
} else {
//opt_ is of type FOSOptimizationObjective,
//there is no need to check the conversion
cost = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
preSolveMotionCost(nmotion->state,dstate);
}
//Only add this motion to the tree if the transition test accepts it
//Temperature must be updated
if (!transitionTest(cost,d,tree,true)) {
return TRAPPED;//Give up on this one and try a new sample
}
//Create a motion
Motion *motion(new Motion(si_));
si_->copyState(motion->state,dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tgi.xmotion = motion;
//Add motion to the tree
tree->add(motion);
//Check if now the tree has a motion inside BoxZos
if (!tree.stateInBoxZos_) {
tree.stateInBoxZos_ = cost.value() < DBL_EPSILON*std::min(d,maxDistance_);
if (tree.stateInBoxZos_) tree.temp_ = initTemperature_;
}
//Update frontier nodes and non frontier nodes count
if (d > frontierThreshold_) {//Participates in the tree expansion
++tree.frontierCount_;
} else {//Participates in the tree refinement
++tree.nonFrontierCount_;
}
return (d > maxDistance_) ? ADVANCED : REACHED;
}
