OMPL plugin bug for multi query (e.g. PRM)

Report crashes, strange behaviour, or apparent bugs
Post Reply
Uli_v
Posts: 35
Joined: 14 Dec 2018, 18:07

OMPL plugin bug for multi query (e.g. PRM)

Post by Uli_v »

Hi,

For multiquery I had to clear the solutions from a previous query, or otherwise it returns the solution for previous start config if previous solution is better (lower cost). I also had to add PRMstar ( || task->algorithm == sim_ompl_algorithm_PRMstar). See function below:

Code: Select all

void setStartState(SScriptCallBack *p, const char *cmd, setStartState_in *in, setStartState_out *out)
{
    TaskDef *task = getTask(in->taskHandle);
    validateStateSize(task, in->state);

    task->startState.clear();
    for(size_t i = 0; i < in->state.size(); i++)
        task->startState.push_back(in->state[i]);

    // for multi-query PRM, if the OMPL's ProblemDefinition has already been set,
    // we want only to clear the query and add the new start state:
    if(task->problemDefinitionPtr && task->planner && task->algorithm == sim_ompl_algorithm_PRM || task->algorithm == sim_ompl_algorithm_PRMstar))
    {
        task->planner->as<og::PRM>()->clearQuery();
        ob::ScopedState<> startState(task->stateSpacePtr);
        for(size_t i = 0; i < task->startState.size(); i++)
            startState[i] = task->startState[i];
        std::cout << "plugin.cpp (setStartState): clearing start states" << std::endl;
        // Do I also need to clear other stuff, like "solutions"/best pathCost?
        //std::cout << "SolutionCount " << task->problemDefinitionPtr->getSolutionCount() << std::endl;
        task->problemDefinitionPtr->clearSolutionPaths(); //Uli, clear the solution(s) of previous query
        //std::cout << "SolutionCount " << task->problemDefinitionPtr->getSolutionCount() << std::endl;
        task->problemDefinitionPtr->clearStartStates();
        task->problemDefinitionPtr->addStartState(startState);
    }
}
Best,
Uli

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: OMPL plugin bug for multi query (e.g. PRM)

Post by fferri »

Thanks!

Now fixed in GitHub repo.

Post Reply