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