SST Planner Produces Insufficient Sampling in Kinematic Car Problem Despite Valid Path Existence

Inizia con la riproduzione di planWithSimpleSetup e confronta i due planner SST, il loro allocator del valid-state sampler e la configurazione di KinematicCarPropagator. Leggi l’integrazione di MBPIValidStateSampler e ispeziona insieme i raggi di selezione/pruning, le durate dei controlli, la dimensione del passo di propagazione e la richiesta Benchmark. Il lavoro è completato quando è stata identificata una configurazione riproducibile o una causa di integrazione per il campionamento sparso e ne è stata documentata una risoluzione verificata.

I'm experiencing issues with the SST planner in a kinematic car motion planning scenario. Despite verifying that valid paths exist (confirmed with PRM), SST with both uniform and custom samplers generates very few samples and fails to find solutions. The problem persists across different sampler configurations.

Key observations:

• Identical environment works with PRM
• Both MBPI (our sampler) and uniform samplers show minimal sampling
• No collision-free path found even when manually verifying path existence
• Visualizations show sparse sampling around start state

Relevant Code Excerpt (planWithSimpleSetup function)

`void planWithSimpleSetup(const BenchmarkMap &bmap, const std::pair<double, double>& start_point, const std::pair<double, double>& goal_point) {
auto space(std::make_sharedob::SE2StateSpace());

ob::RealVectorBounds bounds(2);
bounds.setLow(0, 0);
bounds.setLow(1, 0);
bounds.setHigh(0, bmap.getOccupancyGrid()[0].size() * bmap.getResolution());
bounds.setHigh(1, bmap.getOccupancyGrid().size() * bmap.getResolution());
space->setBounds(bounds);

auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
ob::RealVectorBounds cbounds(2);
cbounds.setLow(0, -20.0); 
cbounds.setHigh(0, 20.0); 
cbounds.setLow(1, -20.6); 
cbounds.setHigh(1, 20.6);
cspace->setBounds(cbounds);

auto space_info = std::make_shared<oc::SpaceInformation>(space, cspace);
space_info->setStateValidityChecker([bmap](const ob::State *state){ return isStateValid(state, bmap); });
space_info->getStateSpace()->setLongestValidSegmentFraction((double)1/(double)bmap.getOccupancyGrid().size());
double carLength = 1.0; // Aracın dingil mesafesi
auto propagator = std::make_shared<KinematicCarPropagator>(space_info, carLength);

oc::SimpleSetup ss(space_info); 

auto rrtMBPI = std::make_shared<oc::SST>(ss.getSpaceInformation());
rrtMBPI->setName("MBPI");
auto rrtUniform = std::make_shared<oc::SST>(ss.getSpaceInformation());
rrtUniform->setName("Uniform");

space_info->setStatePropagator(propagator);
space_info->setPropagationStepSize(0.05);
space_info->setMinMaxControlDuration(1, 1);
space_info->setup();

rrtMBPI->setSelectionRadius(0.2);
rrtMBPI->setPruningRadius(0.1); 
rrtUniform->setSelectionRadius(0.002);
rrtUniform->setPruningRadius(0.01);

ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(start_point.first);
start->setY(start_point.second);
start->setYaw(M_PI/2);

ob::ScopedState<ob::SE2StateSpace> goal(space);
goal->setX(goal_point.first);
goal->setY(goal_point.second);
goal->setYaw(M_PI/2);

ss.setStartAndGoalStates(start, goal, 0.1);

// Benchmark setup
ompl::tools::Benchmark benchmark(ss, "NarrowPassage");

std::shared_ptr<ob::MBPIValidStateSampler> mbpiSampler = std::make_shared<ob::MBPIValidStateSampler>(
    ss.getSpaceInformation().get(), 
    bmap.occupancyGrid, 
    bmap.resolution, 
    bmap.origin
);
benchmark.addPlanner(rrtMBPI);
benchmark.addPlanner(rrtUniform);

benchmark.setPlannerSwitchEvent([mbpiSampler, ss](const ob::PlannerPtr &planner) {
    if (planner->getName() == "MBPI") {
        ss.getSpaceInformation()->setValidStateSamplerAllocator([mbpiSampler](const ob::SpaceInformation* si) {
            return mbpiSampler;
        });
        ss.getSpaceInformation()->setup();
    }
});

benchmark.setPostRunEvent([&](const ompl::base::PlannerPtr &planner, ompl::tools::Benchmark::RunProperties &runProperties)
{
    std::string plannerName = planner->getName();
    std::cout << "Processing results for planner: " << plannerName << std::endl;
    
    ob::PlannerData planner_data(planner->getSpaceInformation());
    planner->getPlannerData(planner_data);
  
    if (planner->getProblemDefinition()->hasSolution())
    {
        auto path = planner->getProblemDefinition()->getSolutionPath()->as<oc::PathControl>();
        if (path) {
            std::cout << "Path size: " << path->getStateCount() << std::endl;
            sf::Color pathColor = (plannerName == "MBPI") ? sf::Color::Green : sf::Color::Blue;
            
            for (size_t i = 1; i < path->getStateCount(); ++i)
            {
                auto* state = path->getState(i)->as<ob::SE2StateSpace::StateType>();
                unsigned int mx, my;
                worldToMap(bmap, state->getX(), state->getY(), mx, my);

                auto* state_prev = path->getState(i-1)->as<ob::SE2StateSpace::StateType>();
                unsigned int mx_prev, my_prev;
                worldToMap(bmap, state_prev->getX(), state_prev->getY(), mx_prev, my_prev);
                
                Renderer::instance().drawMatch({mx_prev, my_prev}, {mx, my}, pathColor);
            }
        }
    }

    std::cout << "Planner data size: " << planner_data.numVertices() << std::endl;
    sf::Color sampleColor = (plannerName == "MBPI") ? sf::Color::Yellow : sf::Color::Magenta;
    
    for (unsigned int i = 0; i < planner_data.numVertices(); ++i) {
        const auto* state = planner_data.getVertex(i).getState()->as<ob::SE2StateSpace::StateType>();
        unsigned int mx, my;
        worldToMap(bmap, state->getX(), state->getY(), mx, my);
        Renderer::instance().drawPoints({{(int)mx, (int)my}}, sampleColor);
    }
});

ompl::tools::Benchmark::Request req;
req.saveConsoleOutput = false;
req.simplify = true;
req.maxTime = 5.0; 
req.runCount = 1;
req.displayProgress = true;

benchmark.benchmark(req);
benchmark.saveResultsToFile();

unsigned int mgx, mgy;
worldToMap(bmap, goal_point.first, goal_point.second, mgx, mgy);

unsigned int msx, msy;
worldToMap(bmap, start_point.first, start_point.second, msx, msy);

Renderer::instance().drawPoints({{(int)mgx, (int)mgy}}, sf::Color::Blue);
Renderer::instance().drawPoints({{(int)msx, (int)msy}}, sf::Color::Blue );

}`

Question

What might be causing this insufficient sampling behavior in SST? Are there specific configuration requirements for:

1. SST pruning/selection radii relative to state space bounds?
2. Control duration/propagation step size settings?
3. Sampler integration with optimal control planners?
4. Benchmark parameters for kinodynamic systems?

Here is my renderer app's results:

In the image above; white area is free space, black pixels are obstacles and other points are samples generated from uniform and mbpi samplers. Blue ones are points that are passes from the StateValidityChecker function while red ones are caused return false from it. Pink and yellow points are successive samples that generated from mbpi and uniform samplers respectively. And lastly, the dark blue points are start and goal positions. As you can see from the output, both samplers could not even gets close to the goal point.

Any guidance on proper SST configuration for vehicle models would be appreciated. The complete reproduction code is available if needed.