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

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

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!
>
>