PathGeometric::checkAndRepair problem with path composed of two states

[krakjack5]

PathGeometric::checkAndRepair problem with path composed of two states

[zkingston]

We should add the special case to make sure the two state case gets handled appropriately. Feel free to submit a PR to address this.

[ChamzasKonstantinos]

Hi @krakjack5 . The obstacle that you are inserting makes either the start and goal invalid or is an obstacle that is in between?

If either your start, goal is invalid then this function would not work as these are always assumed to be valid and part of the problem definition.

In the other case, which I guess is what you are asking, the object make the edge invalid, but not the endpoints.
Also in this case, I am not sure the path can be repaired since it only has 2 states, and you cannot change the start and goal as this would be a different motion planning problem.

Would calling interplolate() before you call checkAndRepair address your usecase?

[krakjack5]

Hi @ChamzasKonstantinos The way I have set up the test, the obstacle makes the edge invalid and both endpoints are valid. The checkAndRepair method computes a temp state to uniformly sample states around for repair. In the case of an invalid edge (path with only two states), if the for loop on line 234 used the stopping condition of "i <= n1", wouldn't it sample a new state around the goal with radius equal to the length of the edge? That will be able to repair the path as long as the obstacle is small enough.

Calling interpolate does work for my use case, but I was wondering why an edge is effectively treated as a special case.

Thanks!

[ChamzasKonstantinos]

Yeah, I don't think it is possible to repair a single edge, as the endpoints would be start, and goal and you cannot change them since that would be a different motion planning problem. I think what you are alluding to would be adding an extra waypoint(state) that goes away from the obstacle thus the repaired path would have 2 edges, but I think an underlying assumption of this function is that the number of waypoints(states) cannot change, thus it never add waypoints(states), it only substitutes them, see line 275.

[krakjack5]

@ChamzasKonstantinos Ok, that makes sense; I missed the fact that no new additional states are added during repair. Thank you for that clarification!

I have another sort of related question if you don't mind. I used interpolate and it works fine. However, I think my understanding of interpolate is not clear. I had set up the validity check such that interpolate added 42 states, thus making the total number of states 44. Just to try, I called interpolate again, and that made the total number of states equal to 224. I thought interpolate worked such that total number of states will be approximately similar to that used for validity checking. I did not understand why after calling interpolate the second time, the number of states jumped from 44 to 224.

Thanks!

[krakjack5]

Never mind my last comment about the interpolate method. The discrepancy was of my doing.

Coming back to the checkAndRepair method, I think the behavior in the case of a path with only two states is not intuitive in terms of the output returned by the method. The comments in PathGeometric.h state

"\brief Check if the path is valid. If it is not, attempts are made to fix the path by sampling around invalid states."

Clearly, for the case of a path with only two states, the path does not get checked for validity. I was wondering if it will be useful to update the comments accordingly. Another option may be to handle this as a separate case, similar to how the case of a path with only a single state is handled. In that case, the method will return a pair where pair.first = path.check() and pair.second = false.

I will appreciate any input!