[OMPL] Nav2 OMPL planner

[shivaang12]

This ticket is for progress tracking and for the discussion on nav2 OMPL planner.

Initial implementation and it's feedback, refer to open-navigation/navigation2_tutorials#2

Link to my branch: https://github.com/shivaang12/navigation2/tree/feature/nav2_ompl_planner

[ardabbour]

I would definitely be interested in working on this

[shivaang12]

@ardabbour Sure look out for this PR #1647. If you want to involve deeply than we could chat over email.

[SteveMacenski]

We could chat in the working group about it next week. @ardabbour are you in the working group calendar?

We also have a Matrix / Riot chat for navigation (+navigation2:matrix.org) that would be good to use so that any of the other maintainers or interested parties can read along.

Its great to see you guys collaborating on this!

[ardabbour]

That sounds great, how do I join the working group calendar and the riot details?

[SteveMacenski]

You can join in the calendar here, next meeting is Thursday. The Riot all you need is that address, but I'm thinking right now we might want to start a slack or use the ROS discord navigation channel. Though I think for a bunch of the technical discussion, just using this ticket thread should be OK.

[SteveMacenski]

https://github.com/ompl/ompl/blob/master/demos/GeometricCarPlanning.cpp
https://github.com/ompl/ompl/blob/master/demos/Point2DPlanning.cpp
https://arxiv.org/pdf/1905.04762.pdf

These are simple demo code, but may be helpful.

[ardabbour]

What kind of state space should we be aiming for? There seems to be several choices:

• 2D real vector space
• SE2 space
• SE2 + 2D/3D kinematic constraint space
• 2D real vector + 2D/3D kinematic constraint space
• SE2 + 2D/3D kinematic constraint + 2D/3D dynamic constraint space
• 2D real vector + 2D/3D kinematic constraint + 2D/3D dynamic constraint space

The way I see it, the costmap reduces the 2.5D planning problem to a 2D planning problem, and if there are kinematic/dynamic constraints for the movement of the robot (eg, car-like robots), these are usually overcome using the local planner. This is the ROS navigation philosophy, to my understanding.

This does not work all the time of course, as the global planner may send the robot on a path where no solution exists for the local planner (for eg., car-like robot having to pull a very tight u-turn). This is usually solved by re-planning, but this can be avoided altogether if the global planner was aware of the kinematic/dynamic motion constraints.

My 2 cents is that it would be best if the planner would allow switching between simple point planning and sophisticated kinematic planning with a parameter, with support for (at least) differential-drive & skid-steer kinematic constraints.

[SteveMacenski]

The way I see it, the costmap reduces the 2.5D planning problem to a 2D planning problem, and if there are kinematic/dynamic constraints for the movement of the robot (eg, car-like robots), these are usually overcome using the local planner.

There's little to no value in using a sampling based planner if you're only going to operate in XY space. Search based methods will be faster and smoother. The value of sampling is being able to take into account the full state of the robot to make sure its feasible that a search method may be too computational to afford. You should take into account the robot footprint so that it works with noncircular robots and perhaps have a simplification if the footprint happens to be circular to just look at the costmap cell cost.

My 2 cents is that it would be best if the planner would allow switching between simple point planning and sophisticated kinematic planning with a parameter, with support for (at least) differential-drive & skid-steer kinematic constraints.

I don't think even supporting particle planning has value, only doing state planning. You bring up whether you should randomly sample in the the XY-theta space or in the velocity space so you can propogate your states forward based on some kinematics model. I'm not sure what the best answer is there, it should be pretty simple to try them both out and see what works best / is feasible computationally / other metrics or goals. You can also just set velocity limits when sampling so that the XY-theta space sampling is within some propogation window. There are many ways to try to do this.

[ardabbour]

Sounds like a good case for an SE2 space planner. Especially if we use optimizing planners like RRT*, I would expect better path smoothness and kinematic compatibility.

As a first step, I think we can use a Reed-Shepp space (or a Dubin space if reverse driving is disabled) for car-like robots, since those are readily available. For differential drive robots, we could implement a Balkcom-Mason state, or perhaps something simpler. I'm no expert in this topic, so insert proverbial salt pinch :)

[SteveMacenski]

What are your thoughts on the kinematic / dynamic constraints?

The curves are usually good for building paths from primitives but wouldn't we rather do random sampling via a typical planner like RRT* or PRM and have the validity checker throw out invalid states? That's how I think about things in sampling planners, I don't know the correct way to use something like special classes of curves in this way.

[ardabbour]

My understanding is that these curves implement the car-like kinematic constraints through a maximum turning radius parameter, but with no dynamic constraints.

If we were to use a base SE2 space, the state validity checker would be quite complex, because we have to take into account the previous state as well as the current for rotations. For example, planning for a parallel park by checking for validity of each state individually can result in two consecutive and valid states within the parking spot where the car is rotated 180 degrees (an extreme case would be in-place rotation); this is obviously impossible. There is also the advantage of being able to optimize using the curves, since they provide the distance metric we need.

[SteveMacenski]

My understanding is that these curves implement the car-like kinematic constraints through a maximum turning radius parameter, but with no dynamic constraints.

That's my understanding as well, but we're not dealing with only car-like robots (omni and differential). Many robots can have a zero-radius turn. The geometric car demo I linked to above looks like it could be made an option (e.g. the space could be SE2 or using a geometric car space). We don't want robots to be generally failing because there's no path based on an artificial constraint. If included, that should be a param option. I'd rather focus potentially on setting up the OMPL problem so its propogating forward in time by some valid set of turning angles. Or we could optionally model the kinematics for each major robot base type to limit the sampling into valid space.

If we were to use a base SE2 space, the state validity checker would be quite complex, because we have to take into account the previous state as well as the current for rotations.

I don't think you need to take into account the prior state, why would you? You have XY-theta space (which is by definition SE(2), so you can just check the footprint cost at a given pose. I don't think there's any requirement to use curves to accomplish this, that's the point of sampling based planners, though geometric method may be faster. But its unclear to me what OMPL is doing with those curves, that also may still be sampling based.

We don't need to over-optimize here early on. Creating a demo planner with OMPL takes at most an afternoon. As far as I see it, the options are planning in SE2 space just directly with XY-theta states (smoothness?), I've seen propagating the states by some valid velocity with the velocity space involved (probably much smoother and also time-variant which is interesting for including dynamic obstacles, potentially), using OMPL with curves, and probably another few methods. Lets figure out / test a bunch of these and just see what works out well or if it makes sense to support multiple options.

[ardabbour]

We don't want robots to be generally failing because there's no path based on an artificial constraint. If included, that should be a param option.

Agreed, this makes a lot of sense. Also, I can't help but wonder what happens if we simply set the max radius to 0...

I've seen propagating the states by some valid velocity with the velocity space

This is a very nice way of doing it, we can definitely use propagators for kinematic and dynamic constraints.

@shivaang12 thankfully finished some boilerplate so it certainly shouldn't take long, I think I can get a basic demo done in the weekend

[SteveMacenski]

Agreed, this makes a lot of sense. Also, I can't help but wonder what happens if we simply set the max radius to 0...

There's a geometric car demo in the ompl.app, I say try it. Its not going to blow up your computer 😉. The propagation method could also be used for car-like robots I believe as long as the footprint and axes are setup correctly.

Which version are you thinking of doing over the weekend? Including the velocity propagation? I was wondering if we should (could, would, might, pick one) also do the acceleration space so that the velocities sampled are smooth. OMPL also has a b spline smoother so maybe that can take into account a bit of it?

Either way, I look forward to screen videos of is. You may want to work with @shivaang12 as well and coordinate development.

[ardabbour]

I'm thinking I'll implement the velocity propagation model for a diff drive robot, and if all goes well, we can add the acceleration constraints as well. It would of course be good to have hands on board if @shivaang12 is available.

[SteveMacenski]

Sounds good. Lets get some demo going and see what it looks like then. On my 6th gen i7 running the full simulation, I can generally get NavFn to plan a 60 meter trajectory in the Willow garage map (@shivaang12 can send you) in ~ 0.017 seconds. I'm not saying you need to beat that, just a number to compare to. You can test this too by using a chrono timer on the beginning and end of the planning action callback in the plugin and printing to screen or something for a quick order of magnitude check.

If this is good, we should consider making a nav2_motion_models package. I also need some motion models for Hybrid-A* so we can share that (also AMCL, my guess is they'll come in handy in a bunch of places). This will help us have headers for the motion characteristics of the different major base types. Don't worry about this now, just getting you thinking about it.

If we can get something respectable with good looking plans and performance, I'd love to poke Mark that made OMPL to give us some critical feedback on how to make it awesome so that's its not demo-quality but production quality.

Maybe its worth it if Shivang can make a counterpart to yours that's without the velocity space (like you PRed in the tutorials repo), but trying to make the paths smooth (either through the way it samples, didnt smooth with the bspline, or other OMPL things I don't know about off the top of my head). The major reason I like the velocity space is because the paths that Shivang's earlier demo showed were really sharp and not great for production use. Also, maybe using the costmap information would help that too? Maybe its possible to make this better. Then we can circle back with both options and assess next steps. Maybe Shivang can make it way better and we don't need to use the velocity space which will undoubtedly be more computational. Or perhaps the velocity space is the way to go, but some of the nuggets Shivang finds to try to make those paths smoother / using costmap / messing with sampling characteristics can then be applied to the velocity method to make that even better.

Sound good @ardabbour @shivaang12 ?

[shivaang12]

Update on my Personal testing

I have been testing OMPL (with the settings in my PR) and found some interesting stuff.

• I am getting good curves (without using smoothing) when using Cost-aware planner such as RRT*, PRM* (need to test others but I think it won't differ much).
• Now the problem part is its taking whole 1 second to do it (for 14 meters). This is in RealVectorStateSpace and my projection is this number might change if we use SE2StateSpace
• I am happy with using cost aware planners because it almost generates same time when plan multiple times (lilke we do in our stack, plan every second or so).

Future with my testing

• I will test with SE2StateSpace as soon as I update the footprint collision checking. And that will be soon (before weekend so @ardabbour can work during weekend).

In reply to @SteveMacenski last comment

• I am experimenting without velocity because I think that will require more time (in terms of generating a plan) to be perfect for our use.
• One question I have is that some "process params" like our current behavior tree replans every second, do we want to make that thing constant and create our planner OR we want to create almost optimal planner while changing that re-planning window.

For @ardabbour

• If you want to setup plugin you can use my PR or branch from my fork. If you want to make from scratch, you can follow my tutorial here.
• If you want to use my branch as starting plan for your implementation, PM me on slack and I will guide how to setup everything.
• And regardless of everything, PM me if you need help with anything.

[ardabbour]

@SteveMacenski sounds good to me!
@shivaang12 I will probably ask for your testing methodology / benchmarking so we are sure we can have meaningful comparisons.

[ardabbour]

I've finished a first draft of the kinematically constrained OMPL Planner for differential drive robots, but I have not been able to run it yet. I'm getting the following cryptic error when trying to run it on the simple turtlebot3 demo:

[planner_server-9] [INFO] [planner_server]: Created global planner plugin SamplingBased of type nav2_ompl_planner/OMPLPlanner
[rviz2-3] [INFO] [rviz2]: Message Filter dropping message: frame 'base_scan' at time 6.801 for reason 'Unknown'
[ERROR] [planner_server-9]: process has died [pid 13250, exit code -11, cmd '/home/ardabbour/bin/nav2_ws/install/nav2_planner/lib/nav2_planner/planner_server --ros-args -r __node:=planner_server --params-file /tmp/tmpaot6a9sz        '].

I'm wondering if it's something you might have come across, and what the problem could be. There are no building problems, and pluginlib isn't throwing any linking issues either. My implementation is here.

[SteveMacenski]

https://github.com/leggedrobotics/se2_navigation

This may also be a useful reference, they have an OMPL SE2 planner (using reed shepp) in here. I didn't look much at it yet but might be something to pull ideas from if they do something interesting.

[SteveMacenski]

@ardabbour that probably means your plugin is crashing. That log statement is the last INFO before we try to instantiate the plugin.

[SteveMacenski]

@ardabbour any update on your problem?

@shivaang12 any update on your planner work?

[shivaang12]

I will post my new results this weekend
Before that I have to work on Footprint cost thing, I got side tracked recently. Anyways, they will be up soon, sorry for the delay.

[ardabbour]

It was a simple mistake of calling an unassigned node pointer (to log and debug, no less :))

There are some other bugs I am ironing out now, and then will start measuring performance

[SteveMacenski]

to log and debug, no less :)

Those are the worst. "I was trying to be a good software citizen" backfires. Also please share a link to this fork / code when you can so we can all look through it. I'd like it if you and Shivang looked over each others' work (obviously I will too) and see where we can make things good. Video / gifs of paths I think are necessary to get a feeling of smoothness / continuity. I'm hoping that Shivang's project of this will help us optimize things and effectively use the costmap and that your work will let us rule out or embrace use of additional constraints.

[ardabbour]

I managed to get a working prototype available here. There's still some work to be done, like defining optimization goals (path length, energy, maybe a mix of both?) and integrate the footprint into the state validity checking. Here a GIF sample of the path planning.

I should mention that it is quite slow currently; takes about 0.5-1 s for a collision-free plan to be generated, but the upside of using these constraints is that the path is always smooth. Perhaps we can have a controller that simply uses the control values used in state propagation (they should be compliant) and only intervenes when there is a dynamic obstacle detected.

[SteveMacenski]

That gif showed a really long period of time without a new plan - do you know why?

Suggestions below for improving speed or things to try out / mention here for posterity:

• Did you take a look where the compute time is coming from? (e.g. is it all OMPL solve() or is there a bunch of time from other things like copies and setup) Just sanity checking that that high compute time is really just from solving the problem.
• Did you test with a few planners and see if there's a subset of them that are particularly fast compared to others?
• There are also a few different ODE solvers (ODEAdaptiveSolver, ODEBasicSolver, ODEErrorSolver, and ODESolver), did you try other ones to see if any of those are better suited?
• I'd also suggest taking a look at this section: https://github.com/windelbouwman/move-base-ompl/blob/master/src/ompl_global_planner.cpp#L263-L318. Their propagation doesn't an ODE solver, I think that will substantially boost performance. I would recommend trying that out. The dynamics for Ackermann/differential/omni are trivial to program.

Its also possible with more constraints like the ones you mention / costmap it speeds up from being more constrained (though that's more of a 'search' thing than a 'sampling' thing).

Perhaps we can have a controller that simply uses the control values used in state propagation

So that would then be a controller, which is something I've thought about (having an OMPL controller option) but out of the scope I think of the planner. We wouldn't want the controller naively following the control efforts of the planner, that would be similar to a carrot follower. Except the carrot can only be generated once a second right now and if it strays off the path, there's a long time before there's feedback to correct it. I think it would be unstable. An OMPL controller is something maybe we should make another ticket for and look at. That would have a relatively short path its trying to follow so the compute is alot lower and we can add constraints like dynamic obstacles and costmap info.

[SteveMacenski]

@mamoll we're not really as interested in narrowing into specifically a kinodynamic planner right now as much as we're interested in some viable OMPL based planner. Viable in terms of being sufficiently smooth, computationally practical, and costmap aware. Generic SE2 planning with OMPL resulted in extremely jagged paths that were not practical to give to a robot base. @shivaang12 was exploring the SE2 planning while @ardabbour was exploring kinodynamic planning option to see which, if either, we could get a working MVP.

The use of the velocity space was to bound the paths to valid velocity constraints with the goal of yielding a more practical path to follow in terms of smoothness. So far, we haven't found a combination of items that yield either something computationally practical or smooth. We haven't yet explored costmaps because if the other 2 aren't possible / found, then that's not worth addressing.

I don't expect without smoothing that the output of OMPL will be incredibly smooth, but the outputs I've seen so far were so bad that I didn't see anywhere we could go from there. Perhaps adding in the costmap information at that point would help with smoothness since its operating in gradient field and nudge it to follow lower-cost areas.

[mamoll]

Generic SE2 planning with OMPL resulted in extremely jagged paths that were not practical to give to a robot base.

@shivaang12 did you run path simplification after a planner found a solution? This takes almost no time, but is essential to getting higher quality paths.

[shivaang12]

@SteveMacenski This is my code snippet of state validator in which I am using footprint for collision checking using which sometimes I get inf cost. When using just cells for collision checking, never got into this problem.

bool OMPLPlanner::isStateValid(const ompl::base::State * state)
{
  if (!ss_->getSpaceInformation()->satisfiesBounds(state)) {
    return false;
  }

  ompl::base::ScopedState<> ss(ompl_state_space_);
  ss = state;

  double x, y, th;
  x = ss[0];
  y = ss[1];
  th = ss[2];

  auto cost = collision_checker_.footprintCostAtPose(x, y, th, costmap_ros_->getRobotFootprint());
  if (cost > 252)
  {
    return false;
  }

  return true;
}

[shivaang12]

@mamoll I am using simplifySolution() method for simplification. Also, I observe less jagged path when using cost aware planner such as RRTstar + simplification. Like this for e.g.

@SteveMacenski Is this acceptable?

[SteveMacenski]

@shivaang12 on inf: Find who's producing the Inf. That snippet of code isn't much help by itself, I think you need to either set up break points, setup some prints, and/or periodic inf/nan checks to figure out where those are coming from (and making sure that your collision checker class changes didn't introduce changes, particularly, I'm thinking about the orient/unorient not being correct and you have an inversion issue).

@shivaang12 on map: woah, that's much more reasonable than anything I've seen from your work so far! What did you change, just the costmap info & simplification? What's the speed? Last I saw from you, the TB3 local map were really not great.

Try also adding the inflate unknown / inflate around unknown so that you have a full potential field since the borders of this map are just uknowns not obstacles. Maybe also mess with the inflation parameters so make a smoother gradient field. I see an unexpected amount of navigating through purple zones given there's zero cost space all around them (some assigned weight parameter need tuning?)

---

An aside from our current woes, but in the future we may be looking at kinodynamic planning as we complete some of this radar standardization so we can have the planner take into account a set of dynamic tracked obstacles. This I don't expect to run nearly as fast. We can revisit that at a later date though. I'm having some students work this summer on the Chinese eq. of GSoC on integrating 3D detection AI on top of the radar for dynamic obstacle tracking and once that's underway, it makes alot of sense to start talking about non-naive ways of using that information without just smeering the costmap in the velocity direction.

[mamoll]

@shivaang12 by default RRT* can sometimes be slow to converge. It helps to turn pruning on. In the latest version of OMPL there is also a CostConvergenceTerminationCondition, which may be helpful in getting RRT* to terminate before using up all available time. Other planners that might work well in practice with cost maps include TRRT and BiTRRT.

[shivaang12]

@SteveMacenski I think cost aware algorithm has made difference (RRTstar instead of RRT). I need to check the exact time but I can say that it is << 1 second. I will trying testing with the suggested settings and get back to this issue.

@mamoll Thanks for the info! I was thinking if the inf cost thing is due to slow convergence or is it purely due to error in state validator. Thanks!

[mamoll]

@shivaang12 I don't know to what extent you have explored the OptimizationObjective infrastructure, but independent from the particular planning algorithm you can express the cost of states, motions, and how they should be aggregated into an overall path cost. The default objective is to minimize path length, but it might be worth playing around with this if the "optimal" path doesn't look all that optimal to you.

[shivaang12]

@mamoll I tried playing around with TRRT and BiTRRT planners but I am not satisfied with their performance. TRRT does converge but spits out jagged path much more similar to RRT, and BiTRRT couldn't able to find a feasible path in 1 second.

I tried using CostConvergenceTerminationCondition but couldn't able to make it work yet. It would be helpful if you can link some examples.

[mamoll]

This unit test has some basic examples. Once you have created a CostConvergenceTerminationCondition instance, you should be able to pass it as the only argument to Planner::solve(). Can you be more specific in what doesn't work?

[ardabbour]

So apparently the root of my memory issues was that I was asking the costmap to give me the cost of a point that did not exist. Now, I ran some tests and it seems that the plans are mostly valid and 1-2 Hz but very far from optimal. Here are some results:

• RRT
• EST
• KPIECE

I don't think we can get any faster than this without implementing a steering function for the controls or implementing a Balkcom-Mason curve and using it, as @mamoll said earlier.

The navigation server continuously asks for plans and that is not a good idea for this planner. Is there a parameter that switches this behaviour off?

Also I have moved my implementations to nav2_ompl_planner to make life easier for me. There is also a geometric branch where I am implementing the planner using the Reeds-Shepp curve.

[SteveMacenski]

https://github.com/ardabbour/nav2_ompl_planner/blob/master/CMakeLists.txt can you try adding optimization flags & build in release mode? Maybe there's some to be gained that way.

Those are hilariously bad plans, is that due to a mis-set maximum turning radius (looks like the turns are incredibly wide) maybe? Its not great if these take 0.5-1s when those paths are also very, very short (< 10 m).

I wonder how @shivaang12 is doing without the velocity space. That should in concept be much faster since its doing alot less. I would hope then it could be at least 300ms bounded...

[shivaang12]

As I said earlier, if we want 100ms - 300ms bounded performance it can be only possible using non-cost aware algorithms such as RRT etc. Cost aware planners takes nearly one second to get a single feasible path, some times beyond that. Going with non-cost ware planners, means settling for jagged path.

[SteveMacenski]

Maybe we should look into the curves then. This seems silly to me that its so slow.

[mamoll]

Cost aware planners takes nearly one second to get a single feasible path, some times beyond that.

Even with an appropriate CostConvergenceTerminationCondition set? What kind of state space are you planning in: SE(2), Dubins, Reeds-Shepp? What planner(s) are you using? What happens if you call OptimizationObjective::setCostThreshold with a very large cost?

[shivaang12]

I am using SE(2) state space (I got suggestion from Abdul to use Reeds-Shepp State Space). I am using RRTStar and planning to test with RRTSharp. Higher OptimizationObjective::setCostThreshold introduces more jaggedness into the path. I'll try with RRTSharp and will let you know the results.

[ardabbour]

@SteveMacenski I always ask colcon to compile with -o3 and release, but I added the optimizations to the cmake file anyway for others to test. Unless anyone has any other ideas, we should consider writing a steering function. Maybe @mamoll can guide us to a simple example or unit test to get started with.

Now moving on to the geometric methods, I combined @shivaang12's code with mine and used a Reeds-Shepp state space. On the example environment, it works at around 10-20 Hz but a different Nav2 replanning paradigm needs to be used for this planner and this video shows why. I don't know what the Nav2 condition for replanning is, but it is not suitable here because it seems to assume that generated plans will complement one another, which is not always the case for sampling-based planners, unless they are constrained to return highly optimal plans.

I understand that replanning is needed when a plan fails for whatever reason, but Nav2 seems to continuously request plans regardless? I understand this is achieved with the default behavior tree of Nav2, but I'm just trying to understand why creating global plans at 20 Hz is needed when we have a robust controller (dwb) attempting to follow the path...

@shivaang12 I think if you use the lazy bidirectional KPIECE, with path simplification and smoothing (as I have done here), you can get fairly fast results (for me this was around 0.05-0.1 seconds for the entire createPlan function, but any talk of bounding to a certain time doesn't make sense with sampling-based planners because of their nature.

[SteveMacenski]

Well that could also be a little bit on the fault of the trajectory planner, but yeah, that clearly is a problem. Current replanning is just at a constant rate (1hz), but soon we're discussing making it proportionate to speed. Plenty of other replanning behaviors could be implemented too using the behavior trees.

Those plans also seem really sub-optimal. If we can plan at 10-20hz now, could we back that down to 5-10hz if the quality is much closer to optimal? I'd also try on a realistic sized map to see what the planning rate is. That small distance isn't representative of an actual application.

Are you storing the different methods using OMPL that you're using in different branches? It would be good to make sure we have a history of attempts to work off of if needed. Is this one cost aware?

[mamoll]

For replanning, you'd typically seed a planner with the remainder of the trajectory the robot is executing. One trick to do this in OMPL is to create a special sampler that you feed the states along the remainder of the trajectory during construction. You configure planners to use this sampler. When generating "random" samples, the first samples will be the states that were passed in. After it has generated those, it falls back on the default random sampling behavior. There is no guarantee that every planner will exactly reconstruct the trajectory the robot was on, but you are more likely to stick with a similar solution path.

[ardabbour]

I will be trying it on a larger map soon, but from my experience we can expect good performance for each path but expecting all the paths to be consistent with one another is a bit of a stretch. I do like the idea of seeding the planner with previous states. All SLAM implementations of OMPL I have seen completely throw away all the information after each plan is made because the state space is recreated each time a plan is requested. If we can somehow keep the relevant information, it would be a great boost.

Also regarding the different methods, all the planners and their basic settings are exposed as ROS parameters. There are two different implementations right now: control-based on the master branch, and geometric-based on the geometry branch.

[soldierofhell]

The kinodynamic planners treat the vehicle dynamics by default as a black box, just probing the system with random controls to see what gets the system closer to the goal. Without spending a lot of time on a steering function you probably won't get the performance you want.

Hi @mamoll, are you aware of any example of such steering? I've searched a bit and haven't found any implementation of steer() function except some trivial ones

[SteveMacenski]

One of the ultimate aims of this work was to support other classes of robots that didn't fit well in the dubin / reeds-shepp motion models that are used in a hybrid-A* planner - which we've found to be substantially faster. Given it appears that the only want to make OMPL planners feasible is to use these motion models, I suggest perhaps we change our focus onto a state lattice node for the new smac planner that can take in the motion primitives for an arbitrary vehicle and use the optimized search framework to allow for arbitrary robots with arbitrary motion models.

That would involve simply:

• A new state lattice node template that reads in the projections from those motion models
• Scripts or documentation about how to generate these lattices

Any objections to closing this ticket and changing the focus on this?

[ardabbour]

None from me

[SteveMacenski]

Closing from comment in October after no other objections