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