Ejemplo n.º 1
0
ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context) :
    context_(context.empty() ? "" : context + ": ")
{
    typedef std::map<base::SpaceInformation*, boost::shared_ptr<SelfConfigImpl> > ConfigMap;

    static ConfigMap    SMAP;
    static boost::mutex LOCK;

    boost::mutex::scoped_lock smLock(LOCK);

    // clean expired entries from the map
    ConfigMap::iterator dit = SMAP.begin();
    while (dit != SMAP.end())
    {
        if (dit->second->expired())
            SMAP.erase(dit++);
        else
          ++dit;
    }

    ConfigMap::const_iterator it = SMAP.find(si.get());

    if (it != SMAP.end())
        impl_ = it->second.get();
    else
    {
        impl_ = new SelfConfigImpl(si);
        SMAP[si.get()].reset(impl_);
    }
}
Ejemplo n.º 2
0
 void checkSetup(const base::SpaceInformationPtr &si)
 {
     if (si)
     {
         if (!si->isSetup())
         {
             si->setup();
             probabilityOfValidState_ = -1.0;
             averageValidMotionLength_ = -1.0;
         }
     }
     else
     {
         probabilityOfValidState_ = -1.0;
         averageValidMotionLength_ = -1.0;
     }
 }
Ejemplo n.º 3
0
ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context) : context_(context)
{
    typedef std::map<base::SpaceInformation*, boost::shared_ptr<SelfConfigImpl> > ConfigMap;

    static ConfigMap    SMAP;
    static boost::mutex LOCK;

    boost::mutex::scoped_lock smLock(LOCK);
    ConfigMap::const_iterator it = SMAP.find(si.get());

    if (it != SMAP.end())
        impl_ = it->second.get();
    else
    {
        impl_ = new SelfConfigImpl(si);
        SMAP[si.get()].reset(impl_);
    }
}
Ejemplo n.º 4
0
    base::PlannerPtr newPlanner(const base::SpaceInformationPtr &si) override
    {
        auto pdst(std::make_shared<geometric::PDST>(si));

        std::vector<unsigned int> projection = {0, 1};
        std::vector<double> cdim = {1, 1};

        pdst->setProjectionEvaluator(
            std::make_shared<base::RealVectorOrthogonalProjectionEvaluator>(
                si->getStateSpace(), cdim, projection));

        return pdst;
    }
Ejemplo n.º 5
0
    base::PlannerPtr newPlanner(const base::SpaceInformationPtr &si) override
    {
        auto kpiece(std::make_shared<geometric::BKPIECE1>(si));
        kpiece->setRange(10.0);

        std::vector<unsigned int> projection = {0, 1};
        std::vector<double> cdim = {1, 1};

        kpiece->setProjectionEvaluator(
            std::make_shared<base::RealVectorOrthogonalProjectionEvaluator>(
                si->getStateSpace(), cdim, projection));

        return kpiece;
    }
Ejemplo n.º 6
0
    base::PlannerPtr newPlanner(const base::SpaceInformationPtr &si) override
    {
        auto sbl(std::make_shared<geometric::pSBL>(si));
        sbl->setRange(10.0);
        sbl->setThreadCount(std::min(4u, std::thread::hardware_concurrency()));

        std::vector<unsigned int> projection = {0, 1};
        std::vector<double> cdim = {1, 1};

        sbl->setProjectionEvaluator(
            std::make_shared<base::RealVectorOrthogonalProjectionEvaluator>(
                si->getStateSpace(), cdim, projection));

        return sbl;
    }
Ejemplo n.º 7
0
ompl::geometric::FOSVFRRT::FOSVFRRT(const base::SpaceInformationPtr &si,
                                    omplplanner::omplPlanner *pl,
                                    SynergyTree *st, double exploration,
                                    double initial_lambda, unsigned int update_freq)
    : RRT(si), st_(st), efficientCount_(0), inefficientCount_(0), explorationInefficiency_(0.),
      explorationSetting_(exploration), initialLambda_(initial_lambda),
      nth_step_(update_freq), step_(0), meanNorm_(0.), vfdim_(0), originalExtend_(true), inefficiencyThreshold_(1.) {
    setName("FOSVFRRT");
    maxDistance_ = si->getStateValidityCheckingResolution();

    Planner::declareParam<double>("exploration", this, &FOSVFRRT::setExploration,
                                  &FOSVFRRT::getExploration, "0.:1.");
    Planner::declareParam<double>("initial_lambda", this, &FOSVFRRT::setInitialLambda,
                                  &FOSVFRRT::getInitialLambda, "0.:1.e100");
    Planner::declareParam<double>("update_freq", this, &FOSVFRRT::setUpdateFrequency,
                                  &FOSVFRRT::getUpdateFrequency, "0.:1.e100");
    Planner::declareParam<bool>("use_original_extend", this, &FOSVFRRT::setOriginalExtend,
                                &FOSVFRRT::getOriginalExtend);


    ///dofs of base orientation are supposed to be disabled for all the robots!

    //Get the total number of joints in the workspace
    unsigned int totalNumJoints = 0;
    for (unsigned int r = 0; r < pl->wkSpace()->getNumRobots(); ++r) {
        if (pl->wkSpace()->getRobot(r)->isSE3Enabled()) totalNumJoints += 3;
        totalNumJoints += pl->wkSpace()->getRobot(r)->getNumJoints();
    }

    //For each robot
    for (unsigned int r = 0; r < pl->wkSpace()->getNumRobots(); ++r) {
        //Get robot
        Robot *robot = pl->wkSpace()->getRobot(r);

        //SE3
        if (robot->isSE3Enabled()) {
            for (unsigned int i = 0; i < 3; ++i) {
                //Check if the joint is actuated, i.e. there exists a component different from 0
                //in the i-th row of the robot map matrix
                bool actuated = false;
                for (unsigned int j = 0; j < pl->wkSpace()->getNumRobControls() && !actuated; ++j) {
                    if (fabs(robot->getMapMatrix()[i][j]) > FLT_EPSILON) {//Different from 0
                        actuated = true;
                        //Add joint
                        robotJoint[r].first.insert(i);
                        vfdim_++;
                    }
                }
            }
        }

        //Rn
        for (unsigned int i = 0; i < robot->getNumJoints(); ++i) {
            //Check if the joint is actuated, i.e. there exists a component different from 0
            //in the (i+6)-th row of the robot map matrix (first 6 rows are related with the SE3, not used)
            bool actuated = false;
            for (unsigned int j = 0; j < pl->wkSpace()->getNumRobControls() && !actuated; ++j) {
                if (fabs(robot->getMapMatrix()[i+6][j]) > FLT_EPSILON) {//Different from 0
                    actuated = true;
                    //Add joint
                    robotJoint[r].second.insert(i);
                    vfdim_++;
                }
            }
        }
    }

    assert(vfdim_ > 0);
}