Esempio n. 1
0
void ompl::base::GoalLazySamples::goalSamplingThread()
{
    if (!si_->isSetup())
    {
        OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
        // wait for everything to be set up before performing computation
        while (!terminateSamplingThread_ && !si_->isSetup())
            std::this_thread::sleep_for(time::seconds(0.01));
    }
    unsigned int prevsa = samplingAttempts_;
    if (!terminateSamplingThread_ && samplerFunc_)
    {
        OMPL_DEBUG("Beginning sampling thread computation");
        ScopedState<> s(si_);
        while (!terminateSamplingThread_ && samplerFunc_(this, s.get()))
        {
            ++samplingAttempts_;
            if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
                addStateIfDifferent(s.get(), minDist_);
        }
    }
    else
        OMPL_WARN("Goal sampling thread never did any work.%s",
                  samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function "
                                                                                            "set.");
    terminateSamplingThread_ = true;
    OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
}
Esempio n. 2
0
ompl::base::PlannerStatus ompl::tools::OptimizePlan::solve(double solveTime, unsigned int maxSol, unsigned int nthreads)
{
    time::point end = time::now() + time::seconds(solveTime);
    unsigned int nt = std::min(nthreads, (unsigned int)planners_.size());
    OMPL_DEBUG("Using %u threads", nt);

    base::PlannerStatus result;
    unsigned int np = 0;
    const base::ProblemDefinitionPtr &pdef = getProblemDefinition();
    pp_.clearHybridizationPaths();

    while (time::now() < end)
    {
        pp_.clearPlanners();
        for (unsigned int i = 0; i < nt; ++i)
        {
            planners_[np]->clear();
            pp_.addPlanner(planners_[np]);
            np = (np + 1) % planners_.size();
        }
        base::PlannerStatus localResult = pp_.solve(std::max(time::seconds(end - time::now()), 0.0), true);
        if (localResult)
        {
            if (result != base::PlannerStatus::EXACT_SOLUTION)
                result = localResult;

            if (!pdef->hasOptimizationObjective())
            {
                OMPL_DEBUG("Terminating early since there is no optimization objective specified");
                break;
            }

            base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective());

            if (pdef->getOptimizationObjective()->isSatisfied(obj_cost))
            {
                OMPL_DEBUG("Terminating early since solution path satisfies the optimization objective");
                break;
            }
            if (pdef->getSolutionCount() >= maxSol)
            {
                OMPL_DEBUG("Terminating early since %u solutions were generated", maxSol);
                break;
            }
        }
    }

    // if we have more time, and we have a geometric path, we try to simplify it
    if (time::now() < end && result)
    {
        geometric::PathGeometric *p = dynamic_cast<geometric::PathGeometric *>(pdef->getSolutionPath().get());
        if (p)
        {
            geometric::PathSimplifier ps(getProblemDefinition()->getSpaceInformation());
            ps.simplify(*p, std::max(time::seconds(end - time::now()), 0.0));
        }
    }

    return result;
}
Esempio n. 3
0
bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
{
    bool result = false;

    bool b = si_->satisfiesBounds(state);
    bool v = false;
    if (b)
    {
        v = si_->isValid(state);
        if (!v)
            OMPL_DEBUG("%s state is not valid", start ? "Start" : "Goal");
    }
    else
        OMPL_DEBUG("%s state is not within space bounds", start ? "Start" : "Goal");

    if (!b || !v)
    {
        std::stringstream ss;
        si_->printState(state, ss);
        ss << " within distance " << dist;
        OMPL_DEBUG("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());

        State *temp = si_->allocState();
        if (si_->searchValidNearby(temp, state, dist, attempts))
        {
            si_->copyState(state, temp);
            result = true;
        }
        else
            OMPL_WARN("Unable to fix %s state", start ? "start" : "goal");
        si_->freeState(temp);
    }

    return result;
}
void ompl::tools::LightningDB::getAllPlannerDatas(std::vector<ompl::base::PlannerDataPtr> &plannerDatas) const
{
    OMPL_DEBUG("LightningDB: getAllPlannerDatas");

    // Convert the NN tree to a vector
    nn_->list(plannerDatas);

    OMPL_DEBUG("Number of paths found: %d", plannerDatas.size());
}
Esempio n. 5
0
void ompl::geometric::CForest::solve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)
{
    OMPL_DEBUG("Starting %s", planner->getName().c_str());
    time::point start = time::now();
    if (planner->solve(ptc))
    {
        double duration = time::seconds(time::now() - start);
        OMPL_DEBUG("Solution found by %s in %lf seconds", planner->getName().c_str(), duration);
    }
}
Esempio n. 6
0
    static void nearCallback(void *data, dGeomID o1, dGeomID o2)
    {
        // if a collision has not already been detected
        if (!reinterpret_cast<CallbackParam *>(data)->collision)
        {
            dBodyID b1 = dGeomGetBody(o1);
            dBodyID b2 = dGeomGetBody(o2);
            if ((b1 != nullptr) && (b2 != nullptr) && (dAreConnectedExcluding(b1, b2, dJointTypeContact) != 0))
                return;

            dContact contact[1];  // one contact is sufficient
            int numc = dCollide(o1, o2, 1, &contact[0].geom, sizeof(dContact));

            // check if there is really a collision
            if (numc != 0)
            {
                // check if the collision is allowed
                bool valid = reinterpret_cast<CallbackParam *>(data)->env->isValidCollision(o1, o2, contact[0]);
                reinterpret_cast<CallbackParam *>(data)->collision = !valid;
                if (reinterpret_cast<CallbackParam *>(data)->env->verboseContacts_)
                {
                    OMPL_DEBUG("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
                               reinterpret_cast<CallbackParam *>(data)->env->getGeomName(o1).c_str(),
                               reinterpret_cast<CallbackParam *>(data)->env->getGeomName(o2).c_str());
                }
            }
        }
    }
Esempio n. 7
0
void ompl::geometric::FMT::assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
{
    // Ensure that there is at least one node near each goal
    while (const base::State *goalState = pis_.nextGoal())
    {
        Motion *gMotion = new Motion(si_);
        si_->copyState(gMotion->getState(), goalState);

        std::vector<Motion*> nearGoal;
        nn_->nearestR(gMotion, goal->getThreshold(), nearGoal);

        // If there is no node in the goal region, insert one
        if (nearGoal.empty())
        {
            OMPL_DEBUG("No state inside goal region");
            if (si_->getStateValidityChecker()->isValid(gMotion->getState()))
            {
                nn_->add(gMotion);
                goalState_ = gMotion->getState();
            }
            else
            {
                si_->freeState(gMotion->getState());
                delete gMotion;
            }
        }
        else // There is already a sample in the goal region
        {
            goalState_ = nearGoal[0]->getState();
            si_->freeState(gMotion->getState());
            delete gMotion;
        }
    } // For each goal
}
Esempio n. 8
0
const ompl::base::State *ompl::base::PlannerInputStates::nextStart()
{
    if (pdef_ == nullptr || si_ == nullptr)
    {
        std::string error = "Missing space information or problem definition";
        if (planner_ != nullptr)
            throw Exception(planner_->getName(), error);
        else
            throw Exception(error);
    }

    while (addedStartStates_ < pdef_->getStartStateCount())
    {
        const base::State *st = pdef_->getStartState(addedStartStates_);
        addedStartStates_++;
        bool bounds = si_->satisfiesBounds(st);
        bool valid = bounds ? si_->isValid(st) : false;
        if (bounds && valid)
            return st;

        OMPL_WARN("%s: Skipping invalid start state (invalid %s)",
                  planner_ ? planner_->getName().c_str() : "PlannerInputStates", bounds ? "state" : "bounds");
        std::stringstream ss;
        si_->printState(st, ss);
        OMPL_DEBUG("%s: Discarded start state %s", planner_ ? planner_->getName().c_str() : "PlannerInputStates",
                   ss.str().c_str());
    }
    return nullptr;
}
Esempio n. 9
0
bool ompl::control::KPIECE1::selectMotion(Motion* &smotion, Grid::Cell* &scell)
{
    scell = rng_.uniform01() < std::max(selectBorderFraction_, tree_.grid.fracExternal()) ?
        tree_.grid.topExternal() : tree_.grid.topInternal();

    // We are running on finite precision, so our update scheme will end up
    // with 0 values for the score. This is where we fix the problem
    if (scell->data->score < std::numeric_limits<double>::epsilon())
    {
        OMPL_DEBUG("Numerical precision limit reached. Resetting costs.");
        std::vector<CellData*> content;
        content.reserve(tree_.grid.size());
        tree_.grid.getContent(content);
        for (std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
            (*it)->score += 1.0 + log((double)((*it)->iteration));
        tree_.grid.updateAll();
    }

    if (scell && !scell->data->motions.empty())
    {
        scell->data->selections++;
        smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
        return true;
    }
    else
        return false;
}
Esempio n. 10
0
void ompl::base::GoalLazySamples::goalSamplingThread()
{
    {
        /* Wait for startSampling() to finish assignment
         * samplingThread_ */
        std::lock_guard<std::mutex> slock(lock_);
    }

    if (!si_->isSetup())  // this looks racy
    {
        OMPL_DEBUG("Waiting for space information to be set up before the sampling thread can begin computation...");
        // wait for everything to be set up before performing computation
        while (!terminateSamplingThread_ && !si_->isSetup())
            std::this_thread::sleep_for(time::seconds(0.01));
    }
    unsigned int prevsa = samplingAttempts_;
    if (isSampling() && samplerFunc_)
    {
        OMPL_DEBUG("Beginning sampling thread computation");
        ScopedState<> s(si_);
        while (isSampling() && samplerFunc_(this, s.get()))
        {
            ++samplingAttempts_;
            if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
            {
                OMPL_DEBUG("Adding goal state");
                addStateIfDifferent(s.get(), minDist_);
            }
            else
            {
                OMPL_DEBUG("Invalid goal candidate");
            }
        }
    }
    else
        OMPL_WARN("Goal sampling thread never did any work.%s",
                  samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function "
                                                                                            "set.");
    {
        std::lock_guard<std::mutex> slock(lock_);
        terminateSamplingThread_ = true;
    }

    OMPL_DEBUG("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
}
Esempio n. 11
0
void ompl::base::GoalLazySamples::startSampling()
{
    if (samplingThread_ == nullptr)
    {
        OMPL_DEBUG("Starting goal sampling thread");
        terminateSamplingThread_ = false;
        samplingThread_ = new std::thread(&GoalLazySamples::goalSamplingThread, this);
    }
}
Esempio n. 12
0
 void configurePlannerRange(double &range, const std::string &context)
 {
     if (range < std::numeric_limits<double>::epsilon())
     {
         base::SpaceInformationPtr si = wsi_.lock();
         if (si)
         {
             range = si->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
             OMPL_DEBUG("%sPlanner range detected to be %lf", context.c_str(), range);
         }
         else
             OMPL_ERROR("%sUnable to detect planner range. SpaceInformation instance has expired.", context.c_str());
     }
 }
Esempio n. 13
0
void ompl::base::GoalLazySamples::stopSampling()
{
    if (isSampling())
    {
        OMPL_DEBUG("Attempting to stop goal sampling thread...");
        terminateSamplingThread_ = true;
        samplingThread_->join();
        delete samplingThread_;
        samplingThread_ = nullptr;
    }
    else if (samplingThread_)
    {  // join a finished thread
        samplingThread_->join();
        delete samplingThread_;
        samplingThread_ = nullptr;
    }
}
Esempio n. 14
0
void ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc)
{
    if (path.getStateCount() < 3)
        return;

    // try a randomized step of connecting vertices
    bool tryMore = false;
    if (ptc == false)
        tryMore = reduceVertices(path);

    // try to collapse close-by vertices
    if (ptc == false)
        collapseCloseVertices(path);

    // try to reduce verices some more, if there is any point in doing so
    int times = 0;
    while (tryMore && ptc == false && ++times <= 5)
        tryMore = reduceVertices(path);

    // if the space is metric, we can do some additional smoothing
    if(si_->getStateSpace()->isMetricSpace())
    {
        bool tryMore = true;
        unsigned int times = 0;
        do
        {
            bool shortcut = shortcutPath(path);                             // split path segments, not just vertices
            bool better_goal = gsr_ ? findBetterGoal(path, ptc) : false;    // Try to connect the path to a closer goal

            tryMore = shortcut || better_goal;
        } while(ptc == false && tryMore && ++times <= 5);

        // smooth the path with BSpline interpolation
        if(ptc == false)
            smoothBSpline(path, 3, path.length()/100.0);

        // we always run this if the metric-space algorithms were run.  In non-metric spaces this does not work.
        const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
        if (!p.second)
            OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
        else
            if (!p.first)
                OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed.");
    }
}
Esempio n. 15
0
void ompl::geometric::TRRT::setup()
{
    Planner::setup();
    tools::SelfConfig selfConfig(si_, getName());

    if (!pdef_ || !pdef_->hasOptimizationObjective())
    {
        OMPL_INFORM("%s: No optimization objective specified.  Defaulting to mechanical work minimization.",
                    getName().c_str());
        opt_ = std::make_shared<base::MechanicalWorkOptimizationObjective>(si_);
    }
    else
        opt_ = pdef_->getOptimizationObjective();

    // Set maximum distance a new node can be from its nearest neighbor
    if (maxDistance_ < std::numeric_limits<double>::epsilon())
    {
        selfConfig.configurePlannerRange(maxDistance_);
        maxDistance_ *= magic::COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
    }

    // Set the threshold that decides if a new node is a frontier node or non-frontier node
    if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
    {
        frontierThreshold_ = si_->getMaximumExtent() * 0.01;
        OMPL_DEBUG("%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_);
    }

    // Create the nearest neighbor function the first time setup is run
    if (!nearestNeighbors_)
        nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));

    // Set the distance function
    nearestNeighbors_->setDistanceFunction([this](const Motion *a, const Motion *b)
                                           {
                                               return distanceFunction(a, b);
                                           });

    // Setup TRRT specific variables ---------------------------------------------------------
    temp_ = initTemperature_;
    nonfrontierCount_ = 1;
    frontierCount_ = 1;  // init to 1 to prevent division by zero error
    bestCost_ = worstCost_ = opt_->identityCost();
}
Esempio n. 16
0
void ompl::geometric::BiTRRT::setup()
{
    Planner::setup();
    tools::SelfConfig sc(si_, getName());

    // Configuring the range of the planner
    if (maxDistance_ < std::numeric_limits<double>::epsilon())
    {
        sc.configurePlannerRange(maxDistance_);
        maxDistance_ *= magic::COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
    }

    // Configuring nearest neighbors structures for the planning trees
    if (!tStart_)
        tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
    if (!tGoal_)
        tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
    tStart_->setDistanceFunction(boost::bind(&BiTRRT::distanceFunction, this, _1, _2));
    tGoal_->setDistanceFunction(boost::bind(&BiTRRT::distanceFunction, this, _1, _2));

    // Setup the optimization objective, if it isn't specified
    if (!pdef_ || !pdef_->hasOptimizationObjective())
    {
        OMPL_INFORM("%s: No optimization objective specified.  Defaulting to mechanical work minimization.", getName().c_str());
        opt_.reset(new base::MechanicalWorkOptimizationObjective(si_));
    }
    else
        opt_ = pdef_->getOptimizationObjective();

    // Set the threshold that decides if a new node is a frontier node or non-frontier node
    if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
    {
        frontierThreshold_ = si_->getMaximumExtent() * 0.01;
        OMPL_DEBUG("%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_);
    }

    // initialize TRRT specific variables
    temp_ = initTemperature_;
    nonfrontierCount_ = 1;
    frontierCount_ = 1; // init to 1 to prevent division by zero error
    bestCost_ = worstCost_ = opt_->identityCost();
    connectionRange_ = 10.0 * si_->getStateSpace()->getLongestValidSegmentLength();
}
Esempio n. 17
0
void ompl::base::GoalLazySamples::stopSampling()
{
    /* Set termination flag */
    {
        std::lock_guard<std::mutex> slock(lock_);
        if (!terminateSamplingThread_)
        {
            OMPL_DEBUG("Attempting to stop goal sampling thread...");
            terminateSamplingThread_ = true;
        }
    }

    /* Join thread */
    if (samplingThread_ != nullptr)
    {
        samplingThread_->join();
        delete samplingThread_;
        samplingThread_ = nullptr;
    }
}
Esempio n. 18
0
void ompl::geometric::LightningRetrieveRepair::setup()
{
    Planner::setup();

    // Setup repair planner (for use by the rrPlanner)
    // Note: does not use the same pdef as the main planner in this class
    if (!repairPlanner_)
    {
        // Set the repair planner
        repairPlanner_.reset(new ompl::geometric::RRTConnect(si_));
        OMPL_DEBUG("LightningRetrieveRepair: No repairing planner specified. Using default: %s", repairPlanner_->getName().c_str() );
    }

    // Setup the problem definition for the repair planner
    repairProblemDef_->setOptimizationObjective(pdef_->getOptimizationObjective()); // copy primary problem def

    // Setup repair planner
    repairPlanner_->setProblemDefinition(repairProblemDef_);
    if (!repairPlanner_->isSetup())
        repairPlanner_->setup();
}
Esempio n. 19
0
    void nearCallback(void *data, dGeomID o1, dGeomID o2)
    {
        dBodyID b1 = dGeomGetBody(o1);
        dBodyID b2 = dGeomGetBody(o2);

        if (b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;

        CallbackParam *cp = reinterpret_cast<CallbackParam*>(data);

        const unsigned int maxContacts = cp->env->getMaxContacts(o1, o2);
        if (maxContacts <= 0) return;

        dContact *contact = new dContact[maxContacts];

        for (unsigned int i = 0; i < maxContacts; ++i)
            cp->env->setupContact(o1, o2, contact[i]);

        if (int numc = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact)))
        {
            for (int i = 0; i < numc; ++i)
            {
                dJointID c = dJointCreateContact(cp->env->world_, cp->env->contactGroup_, contact + i);
                dJointAttach(c, b1, b2);
                bool valid = cp->env->isValidCollision(o1, o2, contact[i]);
                if (!valid)
                    cp->collision = true;
                if (cp->env->verboseContacts_)
                {
                    OMPL_DEBUG("%s contact between %s and %s", (valid ? "Valid" : "Invalid"),
                             cp->env->getGeomName(o1).c_str(), cp->env->getGeomName(o1).c_str());
                }
            }
        }

        delete[] contact;
    }
Esempio n. 20
0
bool ompl::geometric::LightningRetrieveRepair::findBestPath(const base::State *startState, const base::State *goalState, ompl::base::PlannerDataPtr &chosenPath)
{
    OMPL_INFORM("LightningRetrieveRepair: Found %d similar paths. Filtering", nearestPaths_.size());

    // Filter down to just 1 chosen path
    ompl::base::PlannerDataPtr bestPath = nearestPaths_.front();
    std::size_t bestPathScore = std::numeric_limits<std::size_t>::max();

    // Track which path has the shortest distance
    std::vector<double> distances(nearestPaths_.size(), 0);
    std::vector<bool> isReversed(nearestPaths_.size());

    assert(isReversed.size() == nearestPaths_.size());

    for (std::size_t pathID = 0; pathID < nearestPaths_.size(); ++pathID)
    {
        const ompl::base::PlannerDataPtr &currentPath = nearestPaths_[pathID];

        // Error check
        if (currentPath->numVertices() < 2) // needs at least a start and a goal
        {
            OMPL_ERROR("A path was recalled that somehow has less than 2 vertices, which shouldn't happen");
            return false;
        }

        const ompl::base::State *pathStartState = currentPath->getVertex(0).getState();
        const ompl::base::State *pathGoalState = currentPath->getVertex(currentPath->numVertices()-1).getState();

        double regularDistance  = si_->distance(startState,pathStartState) + si_->distance(goalState,pathGoalState);
        double reversedDistance = si_->distance(startState,pathGoalState) + si_->distance(goalState,pathStartState);

        // Check if path is reversed from normal [start->goal] direction and cache the distance
        if ( regularDistance > reversedDistance )
        {
            // The distance between starts and goals is less when in reverse
            isReversed[pathID] = true;
            distances[pathID] = reversedDistance;
            // We won't actually flip it until later to save memory operations and not alter our NN tree in the LightningDB
        }
        else
        {
            isReversed[pathID] = false;
            distances[pathID] = regularDistance;
        }

        std::size_t pathScore = 0; // the score

        // Check the validity between our start location and the path's start
        // TODO: this might bias the score to be worse for the little connecting segment
        if (!isReversed[pathID])
            pathScore += checkMotionScore(startState, pathStartState);
        else
            pathScore += checkMotionScore(startState, pathGoalState);

        // Score current path for validity
        std::size_t invalidStates = 0;
        for (std::size_t vertex_id = 0; vertex_id < currentPath->numVertices(); ++vertex_id)
        {
            // Check if the sampled points are valid
            if (!si_->isValid( currentPath->getVertex(vertex_id).getState()))
            {
                invalidStates++;
            }
        }
        // Track separate for debugging
        pathScore += invalidStates;

        // Check the validity between our goal location and the path's goal
        // TODO: this might bias the score to be worse for the little connecting segment
        if (!isReversed[pathID])
            pathScore += checkMotionScore( goalState, pathGoalState );
        else
            pathScore += checkMotionScore( goalState, pathStartState );

        // Factor in the distance between start/goal and our new start/goal
        OMPL_INFORM("LightningRetrieveRepair: Path %d | %d verticies | %d invalid | score %d | reversed: %s | distance: %f",
            int(pathID), currentPath->numVertices(), invalidStates, pathScore,
            isReversed[pathID] ? "true" : "false", distances[pathID]);

        // Check if we have a perfect score (0) and this is the shortest path (the first one)
        if (pathID == 0 && pathScore == 0)
        {
            OMPL_DEBUG("LightningRetrieveRepair:  --> The shortest path (path 0) has a perfect score (0), ending filtering early.");
            bestPathScore = pathScore;
            bestPath = currentPath;
            nearestPathsChosenID_ = pathID;
            break; // end the for loop
        }

        // Check if this is the best score we've seen so far
        if (pathScore < bestPathScore)
        {
            OMPL_DEBUG("LightningRetrieveRepair:  --> This path is the best we've seen so far. Previous best: %d", bestPathScore);
            bestPathScore = pathScore;
            bestPath = currentPath;
            nearestPathsChosenID_ = pathID;
        }
        // if the best score is the same as a previous one we've seen,
        // choose the one that has the shortest connecting component
        else if (pathScore == bestPathScore && distances[nearestPathsChosenID_] > distances[pathID])
        {
            // This new path is a shorter distance
            OMPL_DEBUG("LightningRetrieveRepair:  --> This path is as good as the best we've seen so far, but its path is shorter. Previous best score: %d from index %d",
                       bestPathScore, nearestPathsChosenID_);
            bestPathScore = pathScore;
            bestPath = currentPath;
            nearestPathsChosenID_ = pathID;
        }
        else
            OMPL_DEBUG("LightningRetrieveRepair:  --> Not best. Best score: %d from index %d", bestPathScore, nearestPathsChosenID_);
    }

    // Check if we have a solution
    if (!bestPath)
    {
        OMPL_ERROR("LightningRetrieveRepair: No best path found from k filtered paths");
        return false;
    }
    else if(!bestPath->numVertices() || bestPath->numVertices() == 1)
    {
        OMPL_ERROR("LightningRetrieveRepair: Only %d verticies found in PlannerData loaded from file. This is a bug.", bestPath->numVertices());
        return false;
    }

    // Reverse the path if necessary. We allocate memory for this so that we don't alter the database
    if (isReversed[nearestPathsChosenID_])
    {
        OMPL_DEBUG("LightningRetrieveRepair: Reversing planner data verticies count %d", bestPath->numVertices());
        ompl::base::PlannerDataPtr newPath(new ompl::base::PlannerData(si_));
        for (std::size_t i = bestPath->numVertices(); i > 0; --i) // size_t can't go negative so subtract 1 instead
        {
            newPath->addVertex( bestPath->getVertex(i-1) );
        }
        // Set result
        chosenPath = newPath;
    }
    else
    {
        // Set result
        chosenPath = bestPath;
    }
    OMPL_DEBUG("LightningRetrieveRepair: Done Filtering\n");

    return true;
}
Esempio n. 21
0
bool ompl::geometric::LightningRetrieveRepair::repairPath(const base::PlannerTerminationCondition &ptc,
                                                          ompl::geometric::PathGeometric &primaryPath)
{
    // \todo: we should reuse our collision checking from the previous step to make this faster

    OMPL_INFORM("LightningRetrieveRepair: Repairing path");

    // Error check
    if (primaryPath.getStateCount() < 2)
    {
        OMPL_ERROR("LightningRetrieveRepair: Cannot repair a path with less than 2 states");
        return false;
    }

    // Loop through every pair of states and make sure path is valid.
    // If not, replan between those states
    for (std::size_t toID = 1; toID < primaryPath.getStateCount(); ++toID)
    {
        std::size_t fromID = toID - 1; // this is our last known valid state
        ompl::base::State *fromState = primaryPath.getState(fromID);
        ompl::base::State *toState = primaryPath.getState(toID);

        // Check if our planner is out of time
        if (ptc == true)
        {
            OMPL_DEBUG("LightningRetrieveRepair: Repair path function interrupted because termination condition is true.");
            return false;
        }

        // Check path between states
        if (!si_->checkMotion(fromState, toState))
        {
            // Path between (from, to) states not valid, but perhaps to STATE is
            // Search until next valid STATE is found in existing path
            std::size_t subsearchID = toID;
            ompl::base::State *new_to;
            OMPL_DEBUG("LightningRetrieveRepair: Searching for next valid state, because state %d to %d was not valid out  %d total states",
                       fromID,toID,primaryPath.getStateCount());
            while (subsearchID < primaryPath.getStateCount())
            {
                new_to = primaryPath.getState(subsearchID);
                if (si_->isValid(new_to))
                {
                    OMPL_DEBUG("LightningRetrieveRepair: State %d was found to valid, we can now repair between states", subsearchID);
                    // This future state is valid, we can stop searching
                    toID = subsearchID;
                    toState = new_to;
                    break;
                }
                ++subsearchID; // keep searching for a new state to plan to
            }
            // Check if we ever found a next state that is valid
            if (subsearchID >= primaryPath.getStateCount())
            {
                // We never found a valid state to plan to, instead we reached the goal state and it too wasn't valid. This is bad.
                // I think this is a bug.
                OMPL_ERROR("LightningRetrieveRepair: No state was found valid in the remainder of the path. Invalid goal state. This should not happen.");
                return false;
            }

            // Plan between our two valid states
            PathGeometric newPathSegment(si_);

            // Not valid motion, replan
            OMPL_DEBUG("LightningRetrieveRepair: Planning from %d to %d", fromID, toID);

            if (!replan(fromState, toState, newPathSegment, ptc))
            {
                OMPL_INFORM("LightningRetrieveRepair: Unable to repair path between state %d and %d", fromID, toID);
                return false;
            }

            // TODO make sure not approximate solution

            // Reference to the path
            std::vector<base::State*> &primaryPathStates = primaryPath.getStates();


            // Remove all invalid states between (fromID, toID) - not including those states themselves
            while (fromID != toID - 1)
            {
                OMPL_INFORM("LightningRetrieveRepair: Deleting state %d", fromID + 1);
                primaryPathStates.erase(primaryPathStates.begin() + fromID + 1);
                --toID; // because vector has shrunk
                OMPL_INFORM("LightningRetrieveRepair: toID is now %d", toID);
            }

            // Insert new path segment into current path
            OMPL_DEBUG("LightningRetrieveRepair: Inserting new %d states into old path. Previous length: %d",
                newPathSegment.getStateCount()-2, primaryPathStates.size());

            // Note: skip first and last states because they should be same as our start and goal state, same as `fromID` and `toID`
            for (std::size_t i = 1; i < newPathSegment.getStateCount() - 1; ++i)
            {
                std::size_t insertLocation = toID + i - 1;
                OMPL_DEBUG("LightningRetrieveRepair: Inserting newPathSegment state %d into old path at position %d",
                    i, insertLocation);
                primaryPathStates.insert(primaryPathStates.begin() + insertLocation,
                    si_->cloneState(newPathSegment.getStates()[i]) );
            }
            OMPL_DEBUG("LightningRetrieveRepair: Inserted new states into old path. New length: %d", primaryPathStates.size());

            // Set the toID to jump over the newly inserted states to the next unchecked state. Subtract 2 because we ignore start and goal
            toID = toID + newPathSegment.getStateCount() - 2;
            OMPL_DEBUG("LightningRetrieveRepair: Continuing searching at state %d", toID);
        }
    }

    OMPL_INFORM("LightningRetrieveRepair: Done repairing");

    return true;
}
Esempio n. 22
0
const ompl::base::State *ompl::base::PlannerInputStates::nextGoal(const PlannerTerminationCondition &ptc)
{
    if (pdef_ == nullptr || si_ == nullptr)
    {
        std::string error = "Missing space information or problem definition";
        if (planner_ != nullptr)
            throw Exception(planner_->getName(), error);
        else
            throw Exception(error);
    }

    const GoalSampleableRegion *goal =
        pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : nullptr;

    if (goal != nullptr)
    {
        time::point start_wait;
        bool first = true;
        bool attempt = true;
        while (attempt)
        {
            attempt = false;

            if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
            {
                if (tempState_ == nullptr)
                    tempState_ = si_->allocState();
                do
                {
                    goal->sampleGoal(tempState_);
                    sampledGoalsCount_++;
                    bool bounds = si_->satisfiesBounds(tempState_);
                    bool valid = bounds ? si_->isValid(tempState_) : false;
                    if (bounds && valid)
                    {
                        if (!first)  // if we waited, show how long
                        {
                            OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.",
                                       planner_ ? planner_->getName().c_str() : "PlannerInputStates",
                                       time::seconds(time::now() - start_wait));
                        }
                        return tempState_;
                    }

                    OMPL_WARN("%s: Skipping invalid goal state (invalid %s)",
                              planner_ ? planner_->getName().c_str() : "PlannerInputStates",
                              bounds ? "state" : "bounds");
                    std::stringstream ss;
                    si_->printState(tempState_, ss);
                    OMPL_DEBUG("%s: Discarded goal state:\n%s",
                               planner_ ? planner_->getName().c_str() : "PlannerInputStates", ss.str().c_str());
                } while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
            }
            if (goal->couldSample() && !ptc)
            {
                if (first)
                {
                    first = false;
                    start_wait = time::now();
                    OMPL_DEBUG("%s: Waiting for goal region samples ...",
                               planner_ ? planner_->getName().c_str() : "PlannerInputStates");
                }
                std::this_thread::sleep_for(time::seconds(0.01));
                attempt = !ptc;
            }
        }
    }

    return nullptr;
}
Esempio n. 23
0
ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTerminationCondition &ptc)
{
    if (lastGoalMotion_) {
        OMPL_INFORM("solve() called before clear(); returning previous solution");
        traceSolutionPathThroughTree(lastGoalMotion_);
        OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
        return base::PlannerStatus(true, false);
    }
    else if (Open_.size() > 0)
    {
        OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh");
        clear();
    }

    checkValidity();
    base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
    Motion *initMotion = nullptr;

    if (!goal)
    {
        OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
        return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
    }

    // Add start states to V (nn_) and Open
    while (const base::State *st = pis_.nextStart())
    {
        initMotion = new Motion(si_);
        si_->copyState(initMotion->getState(), st);
        Open_.insert(initMotion);
        initMotion->setSetType(Motion::SET_OPEN);
        initMotion->setCost(opt_->initialCost(initMotion->getState()));
        nn_->add(initMotion); // V <-- {x_init}
    }

    if (!initMotion)
    {
        OMPL_ERROR("Start state undefined");
        return base::PlannerStatus::INVALID_START;
    }

    // Sample N free states in the configuration space
    if (!sampler_)
        sampler_ = si_->allocStateSampler();
    sampleFree(ptc);
    assureGoalIsSampled(goal);
    OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());

    // Calculate the nearest neighbor search radius
    /// \todo Create a PRM-like connection strategy
    if (nearestK_)
    {
        NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
                        (boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
                        log((double)nn_->size()));
        OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
    }
    else
    {
        NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
        OMPL_DEBUG("Using radius of %f", NNr_);
    }

    // Execute the planner, and return early if the planner returns a failure
    bool plannerSuccess = false;
    bool successfulExpansion = false;
    Motion *z = initMotion; // z <-- xinit
    saveNeighborhood(z);

    while (!ptc)
    {
        if ((plannerSuccess = goal->isSatisfied(z->getState())))
            break;

        successfulExpansion = expandTreeFromNode(&z);

        if (!extendedFMT_ && !successfulExpansion)
            break;
        else if (extendedFMT_ && !successfulExpansion)
        {
            //Apply RRT*-like connections: sample and connect samples to tree
            std::vector<Motion*>       nbh;
            std::vector<base::Cost>    costs;
            std::vector<base::Cost>    incCosts;
            std::vector<std::size_t>   sortedCostIndices;

            // our functor for sorting nearest neighbors
            CostIndexCompare compareFn(costs, *opt_);

            Motion *m = new Motion(si_);
            while (!ptc && Open_.empty())
            {
                sampler_->sampleUniform(m->getState());

                if (!si_->isValid(m->getState()))
                    continue;

                if (nearestK_)
                    nn_->nearestK(m, NNk_, nbh);
                else
                    nn_->nearestR(m, NNr_, nbh);

                // Get neighbours in the tree.
                std::vector<Motion*> yNear;
                yNear.reserve(nbh.size());
                for (std::size_t j = 0; j < nbh.size(); ++j)
                {
                    if (nbh[j]->getSetType() == Motion::SET_CLOSED)
                    {
                        if (nearestK_)
                        {
                            // Only include neighbors that are mutually k-nearest
                            // Relies on NN datastructure returning k-nearest in sorted order
                            const base::Cost connCost = opt_->motionCost(nbh[j]->getState(), m->getState());
                            const base::Cost worstCost = opt_->motionCost(neighborhoods_[nbh[j]].back()->getState(), nbh[j]->getState());

                            if (opt_->isCostBetterThan(worstCost, connCost))
                                continue;
                            else
                                yNear.push_back(nbh[j]);
                        }
                        else
                            yNear.push_back(nbh[j]);
                    }
                }

                // Sample again if the new sample does not connect to the tree.
                if (yNear.empty())
                    continue;

                // cache for distance computations
                //
                // Our cost caches only increase in size, so they're only
                // resized if they can't fit the current neighborhood
                if (costs.size() < yNear.size())
                {
                    costs.resize(yNear.size());
                    incCosts.resize(yNear.size());
                    sortedCostIndices.resize(yNear.size());
                }

                // Finding the nearest neighbor to connect to
                // By default, neighborhood states are sorted by cost, and collision checking
                // is performed in increasing order of cost
                //
                // calculate all costs and distances
                for (std::size_t i = 0 ; i < yNear.size(); ++i)
                {
                    incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
                    costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
                }

                // sort the nodes
                //
                // we're using index-value pairs so that we can get at
                // original, unsorted indices
                for (std::size_t i = 0; i < yNear.size(); ++i)
                    sortedCostIndices[i] = i;
                std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(),
                          compareFn);

               // collision check until a valid motion is found
               for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
                    i != sortedCostIndices.begin() + yNear.size();
                    ++i)
               {
                   if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
                   {
                       m->setParent(yNear[*i]);
                       yNear[*i]->getChildren().push_back(m);
                       const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
                       m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
                       m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
                       m->setSetType(Motion::SET_OPEN);

                       nn_->add(m);
                       saveNeighborhood(m);
                       updateNeighborhood(m,nbh);

                       Open_.insert(m);
                       z = m;
                       break;
                   }
               }
            } // while (!ptc && Open_.empty())
        } // else if (extendedFMT_ && !successfulExpansion)
    } // While not at goal

    if (plannerSuccess)
    {
        // Return the path to z, since by definition of planner success, z is in the goal region
        lastGoalMotion_ = z;
        traceSolutionPathThroughTree(lastGoalMotion_);

        OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());

        return base::PlannerStatus(true, false);
    } // if plannerSuccess
    else
    {
        // Planner terminated without accomplishing goal
        return base::PlannerStatus(false, false);
    }
}
Esempio n. 24
0
void ompl::geometric::TRRTConnect::setup() {
    Planner::setup();
    tools::SelfConfig sc(si_,getName());

    //Set optimization objective
    if (pdef_->hasOptimizationObjective()) {
        opt_ = pdef_->getOptimizationObjective();
    } else {
        opt_.reset(new base::MechanicalWorkOptimizationObjective(si_));
        OMPL_INFORM("%s: No optimization objective specified.",getName().c_str());
        OMPL_INFORM("%s: Defaulting to optimizing path length.",getName().c_str());
    }

    //Set maximum distance a new node can be from its nearest neighbor
    if (maxDistance_ < std::numeric_limits<double>::epsilon()) {
        sc.configurePlannerRange(maxDistance_);
        maxDistance_ *= magic::COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
    }

    //Set the threshold to decide if a new node is a frontier node
    if (frontierThreshold_ < std::numeric_limits<double>::epsilon()) {
        frontierThreshold_ = 0.01*si_->getMaximumExtent();
        OMPL_DEBUG("%s: Frontier threshold detected to be %lf",
                   getName().c_str(),frontierThreshold_);
    }

    //Autoconfigure the K constant
    if (kConstant_ < std::numeric_limits<double>::epsilon()) {
        //Find the average cost of states by sampling
        kConstant_ = opt_->averageStateCost(magic::TEST_STATE_COUNT).value();
    }

    //Create the nearest neighbor function the first time setup is run
#if OMPL_VERSION_VALUE < 1001000 // 1.1.0
    if (!tStart_) tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>
                                   (si_->getStateSpace()));
    if (!tGoal_) tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>
                                   (si_->getStateSpace()));
#else
    if (!tStart_) tStart_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
    if (!tGoal_) tGoal_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(this));
#endif

    //Set the distance function
    tStart_->setDistanceFunction(std::bind(&TRRTConnect::distanceFunction,this,std::placeholders::_1,std::placeholders::_2));
    tGoal_->setDistanceFunction(std::bind(&TRRTConnect::distanceFunction,this,std::placeholders::_1,std::placeholders::_2));

    //Setup TRRTConnect specific variables
    clearTree(tStart_);
    clearTree(tGoal_);

    if (tStart_.compareFn_) delete tStart_.compareFn_;
    tStart_.compareFn_ = new CostIndexCompare(tStart_.costs_,*opt_);

    if (tGoal_.compareFn_) delete tGoal_.compareFn_;
    tGoal_.compareFn_ = new CostIndexCompare(tGoal_.costs_,*opt_);

    k_rrg_ = boost::math::constants::e<double>()*
            double(si_->getStateSpace()->getDimension()+1)/
            double(si_->getStateSpace()->getDimension());//e+e/d
}