예제 #1
0
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);
            }
    }
}
예제 #2
0
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);
        }
    }
}