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

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

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!