[ompl-users] OMPL-1.4.0 Validity check is not enforced with certain planners

[Steven M. <sm...@em...>]

Hello,

It appears that my validity checker does not work with certain planners,
such as RRT*. I've tested this by creating a validity checker that rules
out my start state. Using RRT*, I still get a solution, but using something
like SBL I get an error along the lines of "no valid start state".

I use SimpleSetup like so:

auto constraint = std::make_shared<TetherConstraint>();
auto const_space = std::make_shared<ob::ProjectedStateSpace>(space,
constraint);
auto const_space_info =
std::make_shared<ob::ConstrainedSpaceInformation>(const_space);vv
auto state_space =
std::make_shared<ompl::geometric::SimpleSetup>(const_space_info);
state_space->setStateValidityChecker(check_state);
...

state_space->setPlanner(std::make_shared<og::RRTStar>(const_space_info));

state_space->solve(10);


Check state just measures distance to an obstacle:

bool check_state(const ob::State *state)
{
    double* pos = state->as<ob::RealVectorStateSpace::StateType>()->values;
    Eigen::Vector2d bot0(pos[0], pos[1]);
    Eigen::Vector2d bot1(pos[2], pos[3]);
    Eigen::Vector2d bot2(pos[4], pos[5]);

    // add obstacles
    double clearance = 2;
    std::vector<Eigen::Vector2d> obstacles = {
        Eigen::Vector2d(5,5),
        Eigen::Vector2d(5,6),
        Eigen::Vector2d(4,6),
        Eigen::Vector2d(2,2),
    };

    for (auto obstacle : obstacles) {
        if ((bot0 - obstacle).norm() < clearance){
            return false;
        }
        if ((bot1 - obstacle).norm() < clearance){
            return false;
        }
        if ((bot2 - obstacle).norm() < clearance){
            return false;
        }
    }
    return true;
}


The planner appears to only pass states containing all zero (not exact, but
around 6.92615e-310) to the validity checker. The output paths appear to go
where I want them, but they ignore obstacles defined in the validity check.

Using LBKPIECE, there are nonzero states being passed to the check.

Does anybody have any idea what might be causing this?

Thanks,
Steven
-- 
Steven Morad
PhD Student
Space and Terrestrial Robotic Exploration Laboratory
University of Arizona
www.dangersteve.com/home