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); } } }
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::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::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::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 (auto &motion : motions) { if (motion->parent == nullptr) data.addStartVertex(base::PlannerDataVertex(motion->state)); else data.addEdge(base::PlannerDataVertex(motion->parent ? motion->parent->state : nullptr), base::PlannerDataVertex(motion->state)); data.tagState(motion->state, motion->valid ? 1 : 0); } }