AITstar hangs when goal is infinitely-sampleable

[werner291]

Hi,

so, if we look at this method here: https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/aitstar/src/ImplicitGraph.cpp#L159 if we have a goal that can be sampled infinitely many times (as would be the case for most continuous goal regions), this method will keep sampling more and more goal states forever (see loop condition here: https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/aitstar/src/ImplicitGraph.cpp#L181), even violating the planner termination condition.

Oddly enough, I see this method being called during the iteration of AITstar (https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/src/AITstar.cpp#L689), which seems to suggest there's an intent to sample more goal states as the planning session progresses, but I don't see that happening with the current implementation.

A quick fix would be to just set the maximum number of goal states to some fixed number, but that would seem to break asymptotic optimality.

[gammell]

Thanks for the bug report @werner291

To confirm, are you saying that the do-while loop is ignoring the second half of the condition which ends when the number of sampled goals (goalVertices_.size()) is more than the specified maxNumGoals_? https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/aitstar/src/ImplicitGraph.cpp#L181

Can you provide a MWE or more details of the situation where you are experiencing this?

[werner291]

Well, no, I do imagine that it wouldn't outright ignore that condition; I never specified a maximum there.

Perhaps I'm misunderstanding how the algorithm is supposed to work... Given this line here (https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/src/AITstar.cpp#L689) and how I understand asymptotically-optimal planning to work, the algorithm would presumably take additional goal samples over time in order to grow the chances of finding a better goal sample.

Is that incorrect? Is AIT* meant to only work with a finite, pre-defined number of goal states, rather than goals it can take samples from as required?

[gammell]

Hi @werner291 , So does the line I highlighted, https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/aitstar/src/ImplicitGraph.cpp#L181 then not address your original comment?

A quick fix would be to just set the maximum number of goal states to some fixed number

Can you confirm that this is now a conceptual question and not a bug you have experienced?

I think there may be a slight confusion about the OMPL APIs. OMPL provides both a method to get a goal state, ompl::base::PlannerInputStates::nextGoal(), and a method to test whether a state satisfies the goal, ompl::base::Goal::isSatisfied(). The nextGoal function can be quite complex, for example I believe MoveIt adds new goals in parallel to the planner running, which necessitates the ability to check for new goals, i.e., ompl::base::PlannerInputStates::haveMoreGoalStates().

AITstar uses nextGoal to get a goals to start the search, as necessary for its asymmetric bidirectional search. It then adds more goals when they become known (e.g., immediately if the goal is uncountable/samplable or as they are defined by some other parallel process, e.g., MoveIt) up to a user-defined limit. This limit is a practical implementation necessity. AITstar then stops explicitly sampling the goal but does still check every newly sampled state to see if it meets the goal definition https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/aitstar/src/ImplicitGraph.cpp#L342 and adds those samples that do to the list of goal states.

This means that, as the number of samples goes to infinity, AITstar will sample a non-zero measure goal infinitely often as necessary to maintain theoretical almost-sure asymptotic optimality.

Hope that clears things up. @marlinstrub can hopefully confirm that my summary of his implementation is correct.

[marlinstrub]

That's exactly right, @gammell.

The loop that samples goals calls registerGoalState which adds goal states to goalVertices_. The second part of the condition of the loop, goalVertices_.size() <= maxNumGoals_, therefore prevents AIT* from sampling infinite goals.

The relevant lines are https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/aitstar/src/ImplicitGraph.cpp#L177 https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/aitstar/src/ImplicitGraph.cpp#L145 https://github.com/ompl/ompl/blob/e1faf6da1067d1d708f40867c7fe9a18366630af/src/ompl/geometric/planners/informedtrees/aitstar/src/ImplicitGraph.cpp#L181

[werner291]

Yes, I think that makes this more of a conceptual question than really a bug, indeed. At worst, it's probably an API inconsistency.

I suppose that I'm drawing the comparison, here, with a planner like PRM(*) or RRT(*) where the planner is provided with a sampleable goal, and a certain percentage (about 5% by default, I think) of all samples drawn will be samples of the goal. So, the number of goal states in the graph will slowly grow as time goes on, to ensure that an increasingly dense set of potential goal states is accounted for.

MoveIt adds new goals in parallel to the planner running, which necessitates the ability to check for new goals

So, I'd have to run a goal sampler in parallel to get the sort of asymptotic behavior I'm looking for? (Assuming a fixed number samples proves inadequate somehow.)

[gammell]

So, I'd have to run a goal sampler in parallel to get the sort of asymptotic behavior I'm looking for? (Assuming a fixed number samples proves inadequate somehow.)

No, this is not necessary. AIT provides a.s.a.o. behaviour for a sampleable, non-zero-measure goal region as written. The number of goals known to AIT will increase asymptotically and without bound as the number of samples goes to infinity since these samples are drawn uniformly at random from the entire search space (which by definition includes the goal). There is no need to explicitly sample goals for a.s.a.o. in this situation.

[werner291]

Oh... That's a very different way of working than I expected. Indeed, it makes sense that uniform samples would include goal samples, so the number of goal samples does go to infinity.

Do I understand correctly, then, that this wouldn't work if the goal region formed was "thin" (don't know the exact word) compared to the configuration space? The example here would be that of a robotic arm, where goal samples are valid if the end-effector is at an exact position in space, but the planner is free to choose any joint values as long as that constraint is satisfied... In that case, the chances of a uniform sampler sampling a goal state are effectively zero, and AIT* would not find more goal samples... is that right?

Now, that's admittedly quite a contrived example, and not really the case in my research, so it doesn't form an obstacle to me.

Given the way that AIT* constructs reverse trees... Do I understand correctly that having a large number of explicit goal samples (and thus reverse tree roots) would be problematic?

[marlinstrub]

If by "thin" you mean zero measure, then you are correct in that AIT would almost never (i.e., with zero probability) sample more goal samples than aitstar::ImplicitGraph::maxNumGoals_. The same is true for BIT, ABIT, and EIT. If you want AIT to explicitly sample more goals as time goes on, then you can incrementally increase the above variable through AITstar::setMaxNumGoals(unsigned). There are no conceptual reasons for why AIT could not be implemented with a similar goal-sampling behavior as RRT* though and it should be really easy to do. If that turns out to be an issue for you, I can put it on my queue (but my queue is pretty full as it is, I won't be able to work on this for a while).

Why do you think that having a large number of explicit goal samples would be problematic for AIT*?