Hi, we have a use case where the robot goal has two possible orientations (yaw & yaw+180°) because the robot can be loaded from both sides. We want to find the fastest path so we compute it twice (one for each yaw) and execute the path we the smallest number of points.

That is a bit of a different problem, but falls into the same category of solutions. The same strategy would work, just using mirrored quantization rather than all quantizations

I would love to take this task. I would more detail on this if possible so I can start working on it.

@stevedanomodolor this would be a great contribution! I think the description above more or less gives the details. We want the ability to not only plan to an exact pose (X, Y, Yaw) but also perhaps the inverse of that (X, Y, Yaw-180) or at any angle (X, Y, 0-360).

To do that, we need to change setGoal to something like setGoals or setGoal to include some flag for how to interpret the orientation. Probably the latter makes sense to be more generalized, otherwise if it was just only Yaw and Yaw-180, then setting 2 goals would be EZ-PZ.

Then in A* we need to change the check for isGoal to be OK with any Goal orientation that meets the requests spec. That gets us half the way there.

Then we need to update the analytic expansion module with similar information about the orientation flexibility to try analytic expansions to the different orientations. If we find any that pass, we return it. If we find multiple that pass, we return the shortest.

That should make it so that we can (1) user specify the flexibility (2) A* is aware of it to terminate search when met and (3) the analytic expansions attempt at various angles. The additional overhead of the analytic expansions should be benchmarked so we know how much compute that adds, but it will certainly be smaller than having to replan multiple times with the different settings

@SteveMacenski have a bit of questions on how users would be able to use this feature.

1. Would this be done every time the global planner is called?(computepath). In this case the interface used as input to the createplan function in the global planner would have to change also affecting other planners too. Not a big deal though but more changes all over the code.
2. The user only configured this once. In this case it will not influence the create plan function in the global planner but only the setgoal and just include some configuration values.
   Just to be clear before any implementation.

Have new parameters in hybrid-A* / State Lattice which allows for bidirectional heading for the X or X-180 options or any_heading for any. The user could change with dynamic parameters later if they want. That would be similar to how we handle tolerance now

I have some doubts concerning how to adapt to the create_path. In step 2.1, I try the analytical expansion for all the goals and return the one with the shortest accumulated cost. In step 3.1, the isgoal() function is adapted to check if the current node is in any of the goal nodes. The problem lies in the getHeuristicCost function. How do I decide which goal to check?

The two components of the heuristic is the getObstacleHeuristic and getDistanceHeuristic. Generally, the obstacle heuristic is going to be the prevailing one and that is not dependent on orientation, so that should be good to go. The distance heuristic does add a bit of a quandary.

I suppose in this case, we could look up each theta for a given XY cell and use the lowest. If this were a global setting though (so its using orientation-non-specificity for all requests), then we could adapt precomputeDistanceHeuristic to find the shortest for each XY cell one time at initialization and then the subsequent lookups in getDistanceHeuristic would only be done once like now. That seems like the easiest solution :-) That could be done by keeping the X,Y,Theta lookup table, but setting all Theta for an XY cell to the same lowest value. Or, you could store the data in only an X,Y grid, since quantizing by Theta doesn't matter in this case. But then that setting would need to be changed for how its looked up in that condition (i.e. store that state and have conditional about which to use).

The former is probably the least invasive, but the latter would make the non-orientation goal requests slightly faster by having less data to crawl through. Overall, I leave it to you, but I'd do the former to keep things as simple as possible (even if it means that the non-goal orientation requests don't have a slight speed it up it could otherwise have)

Its handwavey to start, but I think you can do prototypes without this feature just to check that things are working. Like I said, the obstacle heuristic is the one used most commonly in search. However, finding the complete solution to this would be necessary before merging, you're right! Good catch

I suppose in this case, we could look up each theta for a given XY cell and use the lowest. If this were a global setting though (so its using orientation-non-specificity for all requests), then we could adapt precomputeDistanceHeuristic to find the shortest for each XY cell one time at initialization and then the subsequent lookups in getDistanceHeuristic would only be done once like now. That seems like the easiest solution :-) That could be done by keeping the X,Y,Theta lookup table, but setting all Theta for an XY cell to the same lowest value. Or, you could store the data in only an X,Y grid, since quantizing by Theta doesn't matter in this case. But then that setting would need to be changed for how its looked up in that condition (i.e. store that state and have conditional about which to use).

@SteveMacenski Now that we are prunning the path when we check whether the goal is invalid, there is a possibility that the minimum found using the first option can be invalid. Can you explain more about the second option? wouldn't removing the theta affect the accuracy of the computation?
Also, without theta, we assume that heading is zero during the computation of the distance heuristic?

to[0] = goal_coords.x;
 to[1] = goal_coords.y;
 to[2] = goal_coords.theta * motion_table.num_angle_quantization;
 from[0] = node_coords.x;
 from[1] = node_coords.y;
 from[2] = node_coords.theta * motion_table.num_angle_quantization;

What about having a lookup table based on the heading?, this would increase the loopup time but not affect the accuracy of the results?
Adding @jwallace42 for visibility.

Now that we are prunning the path when we check whether the goal is invalid, there is a possibility that the minimum found using the first option can be invalid. Can you explain more about the second option? wouldn't removing the theta affect the accuracy of the computation?

Heuristics are not perfect, if they were then the number of search expansions would be precisely the number of path points to get from the start to goal 😄 Its to aid search with the best we can know about the situation in general.

Lets not consider the second option. It is a memory optimization that complicates things in a way I don't think we need to introduce here. Maybe after we have a working PR we can revisit it if you like. It would be a nice puzzle & involve some architectural changes to be clean (which you may or may not enjoy doing).

Also, without theta, we assume that heading is zero during the computation of the distance heuristic?

We still compute it for the given start heading, since the goal is what is 'we don't care about heading', not the currently expanded search node. If both where orientation independent, then we could just use the L2 distance, since the shortest path would always be a straight line 😉