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