void ompl::control::KPIECE1::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
Grid::CellArray cells;
tree_.grid.getCells(cells);
if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
{
double delta = siC_->getPropagationStepSize();
for (unsigned int i = 0 ; i < cells.size() ; ++i)
for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
{
const Motion* m = cells[i]->data->motions[j];
if (m->parent)
cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
else
cpd->recordEdge(NULL, m->state, NULL, 0.);
cpd->tagState(m->state, cells[i]->border ? 2 : 1);
}
}
else
{
for (unsigned int i = 0 ; i < cells.size() ; ++i)
for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
{
const Motion* m = cells[i]->data->motions[j];
data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
data.tagState(m->state, cells[i]->border ? 2 : 1);
}
}
}
void ompl::geometric::CForest::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
for (std::size_t i = 0; i < planners_.size(); ++i)
{
base::PlannerData pd(si_);
planners_[i]->getPlannerData(pd);
for (unsigned int j = 0; j < pd.numVertices(); ++j)
{
base::PlannerDataVertex &v = pd.getVertex(j);
v.setTag(i);
std::vector<unsigned int> edgeList;
unsigned int numEdges = pd.getIncomingEdges(j, edgeList);
for (unsigned int k = 0; k < numEdges; ++k)
{
base::Cost edgeWeight;
base::PlannerDataVertex &w = pd.getVertex(edgeList[k]);
w.setTag(i);
pd.getEdgeWeight(j, k, &edgeWeight);
data.addEdge(v, w, pd.getEdge(j, k), edgeWeight);
}
}
for (unsigned int j = 0; j < pd.numGoalVertices(); ++j)
data.markGoalState(pd.getGoalVertex(j).getState());
for (unsigned int j = 0; j < pd.numStartVertices(); ++j)
data.markStartState(pd.getStartVertex(j).getState());
}
}
void ompl::control::EST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motionInfo;
tree_.grid.getContent(motionInfo);
double stepSize = siC_->getPropagationStepSize();
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (auto &mi : motionInfo)
for (auto &motion : mi.motions_)
{
if (motion->parent)
{
if (data.hasControls())
data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state),
PlannerDataEdgeControl(motion->control, motion->steps * stepSize));
else
data.addEdge(base::PlannerDataVertex(motion->parent->state),
base::PlannerDataVertex(motion->state));
}
else
data.addStartVertex(base::PlannerDataVertex(motion->state));
}
}
void ompl::control::SyclopRRT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion *> motions;
if (nn_)
nn_->list(motions);
double delta = siC_->getPropagationStepSize();
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (auto &motion : motions)
{
if (motion->parent)
{
if (data.hasControls())
data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state),
control::PlannerDataEdgeControl(motion->control, motion->steps * delta));
else
data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
}
else
data.addStartVertex(base::PlannerDataVertex(motion->state));
}
}
void ompl::control::RRT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
double delta = siC_->getPropagationStepSize();
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
const Motion* m = motions[i];
if (m->parent)
{
if (data.hasControls())
data.addEdge(base::PlannerDataVertex(m->parent->state),
base::PlannerDataVertex(m->state),
control::PlannerDataEdgeControl(m->control, m->steps * delta));
else
data.addEdge(base::PlannerDataVertex(m->parent->state),
base::PlannerDataVertex(m->state));
}
else
data.addStartVertex(base::PlannerDataVertex(m->state));
}
}
void ompl::geometric::RRTsharp::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (std::size_t i = 0 ; i < motions.size() ; ++i)
{
if (motions[i]->parent == NULL)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
base::PlannerDataVertex(motions[i]->state));
// if (visitedMotions.count(motions[i])!=0){
// data.tagState(motions[i]->state,1);
// }
// if (toVisitMotions.count(motions[i])!=0){
// data.tagState(motions[i]->state,2);
// }
}
}
void ompl::control::SyclopEST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
double delta = siC_->getPropagationStepSize();
if (lastGoalMotion_)
data.addGoalVertex(lastGoalMotion_->state);
for (size_t i = 0; i < motions_.size(); ++i)
{
if (motions_[i]->parent)
{
if (data.hasControls())
data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state),
base::PlannerDataVertex(motions_[i]->state),
control::PlannerDataEdgeControl (motions_[i]->control, motions_[i]->steps * delta));
else
data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state),
base::PlannerDataVertex(motions_[i]->state));
}
else
data.addStartVertex (base::PlannerDataVertex(motions_[i]->state));
}
}
void ompl::geometric::LazyPRM::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
// Explicitly add start and goal states. Tag all states known to be valid as 1.
// Unchecked states are tagged as 0.
for (size_t i = 0; i < startM_.size(); ++i)
data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]], 1));
for (size_t i = 0; i < goalM_.size(); ++i)
data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]], 1));
// Adding edges and all other vertices simultaneously
foreach(const Edge e, boost::edges(g_))
{
const Vertex v1 = boost::source(e, g_);
const Vertex v2 = boost::target(e, g_);
data.addEdge(base::PlannerDataVertex(stateProperty_[v1]),
base::PlannerDataVertex(stateProperty_[v2]));
// Add the reverse edge, since we're constructing an undirected roadmap
data.addEdge(base::PlannerDataVertex(stateProperty_[v2]),
base::PlannerDataVertex(stateProperty_[v1]));
// Add tags for the newly added vertices
data.tagState(stateProperty_[v1], (vertexValidityProperty_[v1] & VALIDITY_TRUE) == 0 ? 0 : 1);
data.tagState(stateProperty_[v2], (vertexValidityProperty_[v2] & VALIDITY_TRUE) == 0 ? 0 : 1);
}
}
void ompl::control::EST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motions;
tree_.grid.getContent(motions);
double stepSize = siC_->getPropagationStepSize();
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
{
if (motions[i][j]->parent)
{
if (data.hasControls())
data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
base::PlannerDataVertex (motions[i][j]->state),
PlannerDataEdgeControl(motions[i][j]->control, motions[i][j]->steps * stepSize));
else
data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
base::PlannerDataVertex (motions[i][j]->state));
}
else
data.addStartVertex (base::PlannerDataVertex (motions[i][j]->state));
}
}
void ompl::geometric::LBKPIECE1::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
dStart_.getPlannerData(data, 1, true, NULL);
dGoal_.getPlannerData(data, 2, false, NULL);
// Insert the edge connecting the two trees
data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
}
void ompl::geometric::LazyRRT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
if (motions[i]->valid)
data.tagState(motions[i]->state, 1);
}
}
void ompl::control::SyclopEST::getPlannerData(base::PlannerData& data) const
{
Planner::getPlannerData(data);
if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
{
const double delta = siC_->getPropagationStepSize();
for (std::vector<Motion*>::const_iterator i = motions_.begin(); i != motions_.end(); ++i)
{
const Motion* m = *i;
if (m->parent)
cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
else
cpd->recordEdge(NULL, m->state, NULL, 0.);
}
}
else
{
for (std::vector<Motion*>::const_iterator i = motions_.begin(); i != motions_.end(); ++i)
{
const Motion* m = *i;
data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
}
}
}
void ompl::control::SST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
std::vector<Motion*> allMotions;
if (nn_)
nn_->list(motions);
for(unsigned i=0;i<motions.size();i++)
{
if(motions[i]->numChildren_==0)
{
allMotions.push_back(motions[i]);
}
}
for(unsigned i=0;i<allMotions.size();i++)
{
if(allMotions[i]->parent_!=NULL)
{
allMotions.push_back(allMotions[i]->parent_);
}
}
double delta = siC_->getPropagationStepSize();
if (prevSolution_.size()!=0)
data.addGoalVertex(base::PlannerDataVertex(prevSolution_[0]));
for (unsigned int i = 0 ; i < allMotions.size() ; ++i)
{
const Motion *m = allMotions[i];
if (m->parent_)
{
if (data.hasControls())
data.addEdge(base::PlannerDataVertex(m->parent_->state_),
base::PlannerDataVertex(m->state_),
control::PlannerDataEdgeControl(m->control_, m->steps_ * delta));
else
data.addEdge(base::PlannerDataVertex(m->parent_->state_),
base::PlannerDataVertex(m->state_));
}
else
data.addStartVertex(base::PlannerDataVertex(m->state_));
}
}
void ompl::geometric::BasicPRMmodif::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
for (unsigned int i = 0 ; i < milestones_.size() ; ++i)
for (unsigned int j = 0 ; j < milestones_[i]->adjacent.size() ; ++j)
data.recordEdge(milestones_[i]->state, milestones_[i]->adjacent[j]->state);
}
void ompl::control::PID::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
double delta = siC_->getPropagationStepSize();
if (motions.back())
{
data.addGoalVertex(base::PlannerDataVertex(motions.back()->state));
}
for (int i = motions.size()-1 ; i >= 1 ; --i)
{
const Motion* m = motions[i];
data.addEdge(base::PlannerDataVertex(motions[i-1]->state), base::PlannerDataVertex(m->state), control::PlannerDataEdgeControl(m->control, delta));
}
data.addStartVertex(base::PlannerDataVertex(motions.front()->state));
}
void ompl::geometric::pSBL::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motions;
tStart_.grid.getContent(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
if (motions[i][j]->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state, 1));
else
data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state, 1),
base::PlannerDataVertex(motions[i][j]->state, 1));
motions.clear();
tGoal_.grid.getContent(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
if (motions[i][j]->parent == nullptr)
data.addGoalVertex(base::PlannerDataVertex(motions[i][j]->state, 2));
else
// The edges in the goal tree are reversed so that they are in the same direction as start tree
data.addEdge(base::PlannerDataVertex(motions[i][j]->state, 2),
base::PlannerDataVertex(motions[i][j]->parent->state, 2));
data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
}
void ompl::geometric::TRRTConnect::getPlannerData(base::PlannerData &data) const {
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (tStart_) tStart_->list(motions);
for (std::size_t i(0); i < motions.size(); ++i) {
if (!motions[i]->parent) {
data.addStartVertex(base::PlannerDataVertex(motions[i]->state,1));
} else {
data.addEdge(base::PlannerDataVertex(motions[i]->parent->state,1),
base::PlannerDataVertex(motions[i]->state,1));
}
}
motions.clear();
if (tGoal_) tGoal_->list(motions);
for (std::size_t i(0); i < motions.size(); ++i) {
if (!motions[i]->parent) {
data.addGoalVertex(base::PlannerDataVertex(motions[i]->state,2));
} else {
//The edges in the goal tree are reversed to be consistent with start tree
data.addEdge(base::PlannerDataVertex(motions[i]->state,2),
base::PlannerDataVertex(motions[i]->parent->state,2));
}
}
//Add the edge connecting the two trees
data.addEdge(data.vertexIndex(connectionPoint_.first),
data.vertexIndex(connectionPoint_.second));
}
void ompl::geometric::SBL::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motionInfo;
tStart_.grid.getContent(motionInfo);
for (auto &m : motionInfo)
for (auto &motion : m.motions_)
if (motion->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motion->state, 1));
else
data.addEdge(base::PlannerDataVertex(motion->parent->state, 1),
base::PlannerDataVertex(motion->state, 1));
motionInfo.clear();
tGoal_.grid.getContent(motionInfo);
for (auto &m : motionInfo)
for (auto &motion : m.motions_)
if (motion->parent == nullptr)
data.addGoalVertex(base::PlannerDataVertex(motion->state, 2));
else
// The edges in the goal tree are reversed so that they are in the same direction as start tree
data.addEdge(base::PlannerDataVertex(motion->state, 2),
base::PlannerDataVertex(motion->parent->state, 2));
data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
}
void ompl::geometric::pRRT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion *> motions;
if (nn_)
nn_->list(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (auto &motion : motions)
{
if (motion->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motion->state));
else
data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
}
}
void ompl::geometric::FMT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
nn_->list(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->getState()));
unsigned int size = motions.size();
for (unsigned int i = 0; i < size; ++i)
{
if (motions[i]->getParent() == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->getState()));
else
data.addEdge(base::PlannerDataVertex(motions[i]->getParent()->getState()),
base::PlannerDataVertex(motions[i]->getState()));
}
}
void ompl::geometric::ProjEST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motionInfo;
tree_.grid.getContent(motionInfo);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (auto &m : motionInfo)
for (auto &motion : m.motions_)
{
if (motion->parent == nullptr)
data.addStartVertex(base::PlannerDataVertex(motion->state));
else
data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
}
}
void ompl::geometric::EST::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<MotionInfo> motions;
tree_.grid.getContent(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (unsigned int i = 0 ; i < motions.size() ; ++i)
for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
{
if (motions[i][j]->parent == NULL)
data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state),
base::PlannerDataVertex(motions[i][j]->state));
}
}
void ompl::geometric::RRTstarAR::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (std::size_t i = 0 ; i < motions.size() ; ++i)
{
if (motions[i]->parent == NULL)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
base::PlannerDataVertex(motions[i]->state));
}
}
void ompl::geometric::BallTreeRRTstar::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
for (unsigned int i = 0 ; i < motions.size() ; ++i)
data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
}
void ompl::geometric::LBTRRT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state_));
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
if (motions[i]->parentApx_ == nullptr)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state_));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parentApx_->state_),
base::PlannerDataVertex(motions[i]->state_));
}
}
void ompl::control::KPIECE1::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
Grid::CellArray cells;
tree_.grid.getCells(cells);
double delta = siC_->getPropagationStepSize();
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (unsigned int i = 0 ; i < cells.size() ; ++i)
{
for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
{
const Motion* m = cells[i]->data->motions[j];
if (m->parent)
{
if (data.hasControls())
data.addEdge(base::PlannerDataVertex (m->parent->state),
base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1),
control::PlannerDataEdgeControl (m->control, m->steps * delta));
else
data.addEdge(base::PlannerDataVertex (m->parent->state),
base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1));
}
else
data.addStartVertex(base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1));
// A state created as a parent first may have an improper tag variable
data.tagState(m->state, cells[i]->border ? 2 : 1);
}
}
}
void ompl::geometric::LazyRRT::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state, 1));
for (unsigned int i = 0 ; i < motions.size() ; ++i)
{
if (motions[i]->parent == NULL)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parent ? motions[i]->parent->state : NULL),
base::PlannerDataVertex(motions[i]->state));
data.tagState(motions[i]->state, motions[i]->valid ? 1 : 0);
}
}
void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
{
Planner::getPlannerData(data);
std::vector<Motion*> motions;
if (nn_)
nn_->list(motions);
if (lastGoalMotion_)
data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
for (std::size_t i = 0 ; i < motions.size() ; ++i)
{
if (motions[i]->parent == NULL)
data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
else
data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
base::PlannerDataVertex(motions[i]->state));
}
data.properties["iterations INTEGER"] = boost::lexical_cast<std::string>(iterations_);
data.properties["collision_checks INTEGER"] =
boost::lexical_cast<std::string>(collisionChecks_);
}
void ompl::geometric::LightningRetrieveRepair::getPlannerData(base::PlannerData &data) const
{
OMPL_INFORM("LightningRetrieveRepair: including %d similar paths", nearestPaths_.size());
// Visualize the n candidate paths that we recalled from the database
for (std::size_t i = 0 ; i < nearestPaths_.size() ; ++i)
{
ompl::base::PlannerDataPtr pd = nearestPaths_[i];
for (std::size_t j = 1; j < pd->numVertices(); ++j)
{
data.addEdge(
base::PlannerDataVertex(pd->getVertex(j - 1).getState()),
base::PlannerDataVertex(pd->getVertex(j).getState()));
}
}
}
void ompl::geometric::SimpleSetup::getPlannerData(base::PlannerData &pd) const
{
pd.clear();
if (planner_)
planner_->getPlannerData(pd);
}
