Technical questions for adding Omnidrive support in SmacPlanner

Navigation Stack
,
1

I am going to add Omnidrive planning in SmacPlanner, so I opened this topic to discuss.

Technical Questions:
1- How to choose motion primitives for an Omni robot? I find this discussion helpful, but shall we support all of them and user choose between those?

2- Obviously the Distance Huristic is not applicable for the Omni drive robots. Shall we just use the Obstacle Huristic?

3- Analytic Expansions are not applicable here as well. am I right?

Is there anything else I must be aware of?

1 Like
2

There’s some current work right now on adding a state lattice planner to the SmacPlanner framework to enable omnidirectional / differential drive control sets. You bring up legitimate points of discussion but we haven’t made a decision why on them as we’re still getting the pieces assembled to do some initial testing. So there’s no clean answer I can give you that would likely be right in a few months – I’d be guessing.

In general though:

  • That link is our current best guess and the route we plan to support at the moment. Adding rotate in places and move lateral left/right is our current plan for omni
  • Probably not the best heuristic, but we need to try and see if its a good enough approximation. This could be replaced with a precomputed lookup table, similar to what exists there now, but as generated from BFS in the search field and then stored in the primitives file for use at runtime.
  • This is the biggest unknown. Generally, I would like to keep this since it adds a serious speed up, but it doesn’t quite fit the motion model well. Basically I think for “easier” paths it will help, for “harder” paths it will take up some compute but won’t probably find the terminal solution for omni robots. The compute it takes up is non-trivial but also not massive. My intuition is to keep it, but we could disable it if we find that it is actively bad.

The problem of generating omnidirectional control sets is actually also an area of a bit of unknown. I’ve found almost no literature about how to general minimum control sets for omnidirectional robots that both utilize the full range of motion capabilities of an omni platform and has a practical branching factor for even moderately real-time path planning. The discussion you point to is my best estimate of how to support omnidirectional robots in a state lattice / search motion primitive based solution, but just tacking on “move left/right” isn’t a very satisfying solution when we’re able to auto-generate actual viable minimum control sets for ackermann and differential robot types.

If you know something we don’t, an area of collaboration would be to create a generator for omni control sets that meets your needs and then have that as an option along side our ackermann and differential drive control set generators to use in the state lattice planner implementation. Frankly, its not a huge priority to us support really great omni robots mostly because the few manufacturers we’ve found using omni robots requirements basically could work fine with the other control sets or were able to use just naive grid search. I’m not really satisfied with either of those answers, but my experience and opinions on how to best handle omni robots is limited. I’d be happy to have your collaboration to make sure omni robots have great support here, but it would involve a bit of leg work on your side – but you could build off our work.

2 areas of collaboration here:

    1. Help on control sets generators for omni robots to make best or better use of the drivetrain features
    1. Help replace the distance heuristic with a precomputed BFS using an arbitrary control set (such that it can be used for omni or any other model or a custom omni model from another maker)
1 Like
3

Absolutely agree, almost nothing in literature. I went through a few papers related to UAVs cause those are kinematically close to the Omni drive robots, but neither something special nor a vast analysis on how to choose motion primitives.
Maybe we have to compare different motion primitive sets and find the best fits.

That makes sense, I will continue on SmacPlanner, after getting results, I will be happy to merge with lattice planner. It will be my first step.

It would be my second step.

4

In terms of order of ease / impact, I would actually recommend reversing those tasks. It is still going to take some time for us to even open a PR with the state lattice work in it in order to modify. But the distance heuristic (based on Reeds-Shepp and Dubin models) being replaced with a precomputed BFS of a given control set within a given window size is something that can be largely done even without that code and could plugged in once ready. The tool to build that lookup table is going to take the most time and is independent of the run-time code of the planner.

Then once we have a PR open with state lattice and we start working on integration of the different components, you’d have a decent jumping off point to see how we did our generators to start working on a different one.

5

Honestly, I have to have a demo for our Omni drive robot in early September, so I have to move it at first without caring runtime :slight_smile:

6

That’s super fair. If you join the Nav2 slack, I can add you to the thread with the people working on state lattice planner and we can take it from there.

7

That would be great. I am already in Nav2 Slack @Hossein, please add me to the thread.

8

Here is an update:

  • Upgrade the Dingo Omni robot to support ROS2 (simplified)
  • Add an “OMNI” option to SmacPlanner. The motion primitives are ReedsShepp + Turn in place + Lateral
  • Change DWB parameters to support holonomic robots

@smac :

  • What is the way to analyze if the generated path is near-optimal? How to analyze the effect of adding more motion primitives.

  • Now I am going to add the BFS heuristic you mentioned earlier, but am not sure you mean Best-First or Breadth-First Search.

  • I was thinking to replace the current Analytic Expansion with Rotate-Translate-Rotate or Translate-Rotate expansion for the Omni robot (check if there is a straight and collision-free path from robot to goal) what do you think.

9

Here is the initOmni function.

I may better redesign some parts of the SmacPlanner, maybe a new class for motions will cause a cleaner code.
@smac would you please take a look.

10

I have no doubt that this works at a 30,000 ft view level, but would you consider that to be a complete or representative planner for an omnidirectional robot? That motion doesn’t look overly clean and none of the heuristics / cost estimates are estimating those primitives. There’s a reason we have work going on for a state lattice planner implementation within the smac planner framework to allow people to specify control sets for their robot’s unique drive trains of any sort to use in planning.