Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
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;
}