Re: [ompl-users] Problem with path planning using multiple goal state with RRTconnect in Dubin's state space

[Aditya G. <ag...@vt...>]

Here is one more update, and possible solution. I looked through RRTConnect
code, and think that the if condition on line 240 of RRTConnect.cpp that is
used to sample goal states is what is causing failure to sample all goal
states.

`if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() /
2)`

[RRTConnect.cpp](
https://github.com/ompl/ompl/blob/9c3a20faaddfcd7f58fce235495f043ebee3e735/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
)

That if condition means that unless the very first sampled goal state can
be connected to some other state, no other goal states will be sampled. In
the example I am working with, the first goal state is sitting extremely
close to a large obstacle. Thus the likelihood of sampling a state between
this goal state and the obstacle that can be connected to the the goal
state is extremely low. As such, a tree cannot grow from the first goal
state and no other goal states get sampled, and thus no solution is
generated.

I changed the if condition to the if condition used in RRTstar and that
made RRTConnect work. This is how my change looks like

`if (pis_.getSampledGoalsCount() < goal->maxSampleCount()) {`

I would appreciate any input on this possible solution. Thank you for your
help!

On Sat, Sep 24, 2022 at 1:29 PM Aditya Gadre <ag...@vt...> wrote:

> I am providing more details about the problem I am having with multiple
> goal states. Following is the output that shows that simple setup indeed
> has three goal states. As I had mentioned in my previous email,
> samplePosition_ in GoalStates goes from 0 to 1 and then remains at 1. The
> code used is attached.
>
> ====== PROGRAM OUTPUT START ======
> 3 goal states, threshold = 2.22045e-16, memory address = 0x7ff58d706e08
> Compound state [
> RealVectorState [50 50]
> SO2State [0]
> ]
>
> Compound state [
> RealVectorState [50 50]
> SO2State [1.0472]
> ]
>
> Compound state [
> RealVectorState [50 50]
> SO2State [1.5708]
> ]
>
> Debug:   RRTConnect: Planner range detected to be 28.598431
> Info:    RRTConnect: Starting planning with 1 states already in
> datastructure
> Info:    RRTConnect: Created 9347 states (9346 start + 1 goal)
> Info:    ProblemDefinition: Adding approximate solution from planner
> RRTConnect
> Info:    Solution found in 1.005351 seconds
> Here is the code I used for testing planning with multiple goal states.
> ====== PROGRAM OUTPUT End ======
>
> Thank you for your help!
>
>
> On Fri, Sep 23, 2022 at 11:45 PM Aditya Gadre <ag...@vt...> wrote:
>
>> Hello,
>>
>> I am relatively new to OMPL. I am experimenting with planning paths using
>> RRTconnect in Dubin's state space, and have been able to plan paths from a
>> given starting state to the goal in the presence of obstacles. Now I am
>> trying to use the GoalStates class to specify multiple goals, but somehow
>> it is not working as expected.
>>
>> State-space: Dubin's
>> Planner: RRTconnect
>> Start: 1 start state
>> Goal: 3 goal states in GoalStates
>>
>> This is how I have set up the problem. The first goal state is (x1, y1,
>> yaw1), and there is an obstacle very near to (x1, y1) position such that
>> approach at yaw1 is blocked. The two other states are (x1, y1, yaw2) and
>> (x1, y1, yaw3). Thus all the goal states have the same x and y coordinates,
>> but different yaw angles. The two yaw angles, yaw2 and yaw3, are such that
>> it is possible to compute paths from the start state to (x1, y1, yaw2) and
>> (x1, y1, yaw3). I use the GoalStates object to add the three goal states.
>> All the setup is done using SimpleSetup.
>>
>> auto goals = std::make_shared<ob::GoalStates>(si);
>> goals->addState(goal_state);     // infeasible goal state
>> goals->addState(goal_state1);    // feasible
>> goals->addState(goal_state2);    // feasible
>> simple_setup.setGoal(goals);
>>
>> I thought that the RRTconnect planner would sample all the three goal
>> states and would be able to plan a path to one of the two feasible goal
>> states. However, it seems that only the first goal state was ever sampled.
>> In the debugger I checked the goal details in the SimpleSetup object. It
>> showed three goal states added, but the samplePosition_ remained unchanged
>> at 1, indicating that only the first goal state was sampled. The console
>> output also shows that there ever was one goal state used in planning,
>> which leads to the planner not being able to compute a path as the yaw
>> angle of the first goal state makes any approach to its x-y possible
>> impossible. In the console output below, we can see the tree growing only
>> from the start state but not from the goal as only the infeasible goal
>> state is sampled. The type of solution after each iteration remains
>> APPROXIMATE solution.
>>
>> Info:    RRTConnect: Starting planning with 1 states already in
>> datastructure
>> Info:    RRTConnect: Created 15246 states (15245 start + 1 goal)
>> Info:    ProblemDefinition: Adding approximate solution from planner
>> RRTConnect
>> Info:    Solution found in 1.010254 seconds
>>
>> Info:    RRTConnect: Starting planning with 15246 states already in
>> datastructure
>> Info:    RRTConnect: Created 26487 states (26486 start + 1 goal)
>> Info:    ProblemDefinition: Adding approximate solution from planner
>> RRTConnect
>> Info:    Solution found in 1.009778 seconds
>>
>> Info:    RRTConnect: Starting planning with 26487 states already in
>> datastructure
>> Info:    RRTConnect: Created 36780 states (36779 start + 1 goal)
>> Info:    ProblemDefinition: Adding approximate solution from planner
>> RRTConnect
>> Info:    Solution found in 1.007346 seconds
>>
>> I am most likely missing something important in the setup, but cannot
>> really determine what it is. I will greatly appreciate any help in this
>> regard.
>>
>> Thanks!
>>
>>