Ompl. Wrong result with flag haveExactSolutionPath()=true
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;
}
}