bool ompl::geometric::BiTRRT::connectTrees(Motion* nmotion, TreeData& tree, Motion* xmotion)
{
// Get the nearest state to nmotion in tree (nmotion is NOT in tree)
Motion *nearest = tree->nearest(nmotion);
double dist = si_->distance(nearest->state, nmotion->state);
// Do not attempt a connection if the trees are far apart
if (dist > connectionRange_)
return false;
// Copy the resulting state into our scratch space
si_->copyState(xmotion->state, nmotion->state);
// Do not try to connect states directly. Must chop up the
// extension into segments, just in case one piece fails
// the transition test
GrowResult result;
Motion* next = NULL;
do
{
// Extend tree from nearest toward xmotion
// Store the result into next
// This function MAY trash xmotion
result = extendTree(nearest, tree, xmotion, next);
if (result == ADVANCED)
{
nearest = next;
// xmotion may get trashed during extension, so we reload it here
si_->copyState(xmotion->state, nmotion->state); // xmotion may get trashed during extension, so we reload it here
}
} while (result == ADVANCED);
// Successful connection
if (result == SUCCESS)
{
bool treeIsStart = tree == tStart_;
Motion* startMotion = treeIsStart ? next : nmotion;
Motion* goalMotion = treeIsStart ? nmotion : next;
// Make sure start-goal pair is valid
if (pdef_->getGoal()->isStartGoalPairValid(startMotion->root, goalMotion->root))
{
// Since we have connected, nmotion->state and next->state have the same value
// We need to check one of their parents to avoid a duplicate state in the solution path
// One of these must be true, since we do not ever attempt to connect start and goal directly.
if (startMotion->parent)
startMotion = startMotion->parent;
else
goalMotion = goalMotion->parent;
connectionPoint_ = std::make_pair(startMotion, goalMotion);
return true;
}
}
return false;
}
ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi,
Motion *rmotion)
{
/* find closest state in the tree */
Motion *nmotion = tree->nearest(rmotion);
/* assume we can reach the state we go towards */
bool reach = true;
/* find state to add */
base::State *dstate = rmotion->state;
double d = si_->distance(nmotion->state, rmotion->state);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
dstate = tgi.xstate;
reach = false;
}
// 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 = tgi.start ?
si_->checkMotion(nmotion->state, dstate) :
si_->getStateValidityChecker()->isValid(dstate) && si_->checkMotion(dstate, nmotion->state);
if (validMotion)
{
/* create a motion */
auto *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->root = nmotion->root;
tgi.xmotion = motion;
tree->add(motion);
if (reach)
return REACHED;
else
return ADVANCED;
}
else
return TRAPPED;
}
ompl::geometric::BiTRRT::GrowResult ompl::geometric::BiTRRT::extendTree(Motion* toMotion, TreeData& tree, Motion*& result)
{
// Nearest neighbor
Motion *nearest = tree->nearest(toMotion);
return extendTree(nearest, tree, toMotion, result);
}
bool ompl::geometric::TRRTConnect::connect(TreeData &tree, TreeGrowingInfo &tgi,
Motion *rmotion) {
++connectCount;
unsigned int steps = 0;
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 false;
} else {
//Find nearest node in the tree
//The returned motion has not 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 false;
}
//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 not be updated
while (transitionTest(cost,d,tree,false)) {
//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);
steps++;
//Check if now the tree has a motion inside BoxZos
if (!tree.stateInBoxZos_) {
//tree.stateInBoxZos_ = cost.v < DBL_EPSILON*std::min(d,maxDistance_);
tree.stateInBoxZos_ = ((ompl::base::FOSOptimizationObjective*)opt_.get())->
boxZosDistance(motion->state) < DBL_EPSILON;
if (tree.stateInBoxZos_) tree.temp_ = initTemperature_;
}
//Update frontier nodes and non frontier nodes count
++tree.frontierCount_;//Participates in the tree expansion
//If reached
if (dstate == rmotion->state) {
// std::cout << steps << " steps" << std::endl;
return true;
}
//Current near motion is the motion just added
nmotion = motion;
//Distance from near state to random state
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 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 false;
//Compute motion 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);
}
}
// std::cout << steps << " steps" << std::endl;
return false;
}
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;
}