Ompl. Wrong result with flag haveExactSolutionPath()=true

edit

Hi all!

I am planning motion of KUKA with OMPL (without MoveIt).

It works great but sometimes it writes to me:

Warning: Solution path may slightly touch on an invalid region of the state space at line 386 in /tmp/buildd/ros-indigo-ompl-1.0.0003094-0trusty-20141029-0351/src/ompl/geometric/src/PathSimplifier.cpp

I check a flag ss.haveExactSolutionPath() but it is true and my robot crashes.

Can I check a validity of path with some flag in OMPL?

My code:

bool plan_ompl(cut_data & work,rob_ang start_ang,rob_ang finish_ang,vector <rob_ang> & res,double & length)
{
ob::StateSpacePtr space(new ob::RealVectorStateSpace(6));
ob::RealVectorBounds bounds(6);

bounds.setHigh(0,work.kuka.a1_max);
bounds.setHigh(1,work.kuka.a2_max);
bounds.setHigh(2,work.kuka.a3_max);
bounds.setHigh(3,work.kuka.a4_max);
bounds.setHigh(4,work.kuka.a5_max);
bounds.setHigh(5,work.kuka.a6_max);

bounds.setLow(0,work.kuka.a1_min);
bounds.setLow(1,work.kuka.a2_min);
bounds.setLow(2,work.kuka.a3_min);
bounds.setLow(3,work.kuka.a4_min);
bounds.setLow(4,work.kuka.a5_min);
bounds.setLow(5,work.kuka.a6_min);

space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
space->setLongestValidSegmentFraction(0.001);
space->setup();

og::SimpleSetup ss(space);
ss.setStateValidityChecker(boost::bind(&isStateValid, _1,boost::ref(work)));

ob::ScopedState<> start(space);
start[0] = start_ang.a1;
start[1] = start_ang.a2;
start[2] = start_ang.a3;
start[3] = start_ang.a4;
start[4] = start_ang.a5;
start[5] = start_ang.a6;

ob::ScopedState<> goal(space);
goal[0] = finish_ang.a1;
goal[1] = finish_ang.a2;
goal[2] = finish_ang.a3;
goal[3] = finish_ang.a4;
goal[4] = finish_ang.a5;
goal[5] = finish_ang.a6;

ss.setStartAndGoalStates(start, goal);
og::RRTConnect *rrt_con = new og::RRTConnect(ss.getSpaceInformation());
rrt_con->setRange(range_ompl);
ob::PlannerPtr planner(rrt_con);

ss.setPlanner(planner);
ss.setOptimizationObjective(getPathLengthObjective(ss.getSpaceInformation()));
ss.setup();

ob::PlannerStatus solved = ss.solve(20);

if (ss.haveExactSolutionPath())
{
    ss.simplifySolution();

    og::PathGeometric path( dynamic_cast< const og::PathGeometric& >( ss.getSolutionPath() ) );
    const std::vector< ob::State* > &states = path.getStates();
    ob::State *state;
    cout << "Length " << path.length() << endl;
    length = path.length();

    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);
    }
    return true;
}
else
{
    cout << "No solution found" << endl;
    return false;
}

}