[ompl-users] Multiple goal states with RRTconnect and Dubin's state space

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

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
given starting state to the goal in presence of obstacles. Now I am trying
to use 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 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 will be able to plan 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 the its x-y possible
impossible. In the console output below, we can see tree growning only from
start state but not from the goal as only the infeasible goal state is
sampled. The type of solution after each itaration remains APPROXIMATE
soluton.


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 mostly likely missing something important in the setup, but cannot really
determine what it is. I will greatly appreciate any help in thie regard.


Thanks!