Revision history [back]

I understand how to do it!

       og::PathGeometric path( dynamic_cast< const og::PathGeometric& >( *pdef->getSolutionPath()));
       const std::vector< ob::State* > &states = path.getStates();
       ob::State *state;
       for( size_t i = 0 ; i < states.size( ) ; ++i )
       {
        state = states[ i ]->as< ob::State >( );

        rob_ang temp_rob_ang;
        temp_rob_ang.a1 = state->as<ob::RealVectorStateSpace::StateType>()->values[0];
        temp_rob_ang.a2 = state->as<ob::RealVectorStateSpace::StateType>()->values[1];
        temp_rob_ang.a3 = state->as<ob::RealVectorStateSpace::StateType>()->values[2];
        temp_rob_ang.a4 = state->as<ob::RealVectorStateSpace::StateType>()->values[3];
        temp_rob_ang.a5 = state->as<ob::RealVectorStateSpace::StateType>()->values[4];
        temp_rob_ang.a6 = state->as<ob::RealVectorStateSpace::StateType>()->values[5];
        res.push_back(temp_rob_ang);
        }