[Smac planner] Ignore goal orientation option

[doisyg]

Feature request

Feature description

I would like to ask the planner to generate a path without taking into account the orientation of the goal pose. In other word, to arrive to the goal position as naturaly as possible and without final rotation or added constraint on the trajectory.
Is it already possible and I missed something ?
If not, what would be the place to add this feature ? A hacky/nasty way with the 2D planner is to replace the final orientation by the orientation of the point of the path before.

[SteveMacenski]

Which Smac Planner do you mean?

The 2D planner is just a 2D grid search A*, it doesn't know anything about orientations, so I think that is already true. You can verify yourself that we don't even input the orientation information to the A* algorithm in the 2D case.

The Hybrid-A* planner's selling point is kinematic feasibility, so if that were to be offered in Hybrid-A*, it would have to be a unique parameter or only leveraged by the analytic expansion. I'm not sure realistically how to get that done without actually costing more CPU than just specifying an orientation

[doisyg]

Which Smac Planner do you mean?
Ideally both but I am currently focused on the 2D planner

The 2D planner is just a 2D grid search A*, it doesn't know anything about orientations, so I think that is already true. You can verify yourself that we don't even input the orientation information to the A* algorithm in the 2D case.

I just realize that more or less: a path computed with the 2D planner outputs a end pose with a 0 orientation, no matter which orientation was sent in the goal! This is, I believe a major issue to use it as such as a standard planner! Opening a separated issue: #2482

[SteveMacenski]

O..k... closing this then?

[gomandr]

Hi,

I'm confronted with the case where I don't have any requirement on the goal orientation and specifying one beforehand should be avoided (large turning radius w.r.t cell size). While I don't see how to have a "pure" orientation free goal implemented easily in Hybrid-A*, I tried the following modifications:

• Add an ignore_goal_orientation parameter in search_info and motion_table,
• In NodeHybrid::getDistanceHeuristic, recompute the goal orientation as being the "bearing" from the current node, e.g something along the line of to[2] = std::atan2(to[1] - from[1], to[0] - from[0]);,
• In the AStarAlgorithm<NodeT>::getAnalyticPath, same as above.

It's working as expected, but maybe I'm missing something important here. Do you see any pitfalls in doing that ?

If you think it's a viable solution, I could open a PR with this.

[yassiezar]

Hi @gomandr , we're facing a similar problem. In our use case we have a reasonably large robot operating within constrained environments. We only care about the goals' 2D positions and the goal orientations often lead to plans that are impossible to execute (e.g. large turning radii). I was wondering if you've made any progress with your solution and if you'd mind sharing it?