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

Bắt đầu với việc tái hiện planWithSimpleSetup và so sánh hai bộ lập kế hoạch SST, bộ cấp phát valid-state sampler của chúng và thiết lập KinematicCarPropagator. Đọc phần tích hợp MBPIValidStateSampler và kiểm tra cùng nhau các bán kính selection/pruning, thời lượng điều khiển, kích thước bước propagation và yêu cầu Benchmark. Hoàn thành có nghĩa là xác định được một cấu hình có thể tái hiện hoặc nguyên nhân tích hợp gây ra việc lấy mẫu thưa, đồng thời ghi lại một cách giải quyết đã được xác minh.

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.