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_);
}
}
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;
}
}
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_);
}
}
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;
}
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;
}
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;
}
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);
}