search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.63k stars 1.31k forks source link

[OMPL] Nav2 OMPL planner #1646

Closed shivaang12 closed 3 years ago

shivaang12 commented 4 years ago

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

Initial implementation and it's feedback, refer to ros-planning/navigation2_tutorials#2

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

ardabbour commented 4 years ago

I would definitely be interested in working on this

shivaang12 commented 4 years ago

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

SteveMacenski commented 4 years ago

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 commented 4 years ago

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

SteveMacenski commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 :wink:. 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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

Update on my Personal testing

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

Future with my testing

In reply to @SteveMacenski last comment

For @ardabbour

ardabbour commented 4 years ago

@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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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

SteveMacenski commented 4 years ago

@ardabbour any update on your problem?

@shivaang12 any update on your planner work?

shivaang12 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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:

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.

ardabbour commented 4 years ago
  • 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.

This is definetely the next step here; I think the solving should be much faster at the cost of optimality when reducing the timeout, but I haven't observed much of a difference.

  • Did you test with a few planners and see if there's a subset of them that are particularly fast compared to others?

This should be trivial, I'll look into it but I doubt there will be a great difference. By default, OMPL assigns KPIECE, because " This planner has been shown to work well consistently across many real-world motion planning problems".

  • 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.

This actually makes a lot of sense; perhaps something to pursue after this planner

SteveMacenski commented 4 years ago

This is definetely the next step here; I think the solving should be much faster at the cost of optimality when reducing the timeout, but I haven't observed much of a difference.

I just meant literally profiling the call or adding a timer around the call to make sure that the time you're seeing for the planner to return a plan is due to the planner or due to other things in the software that might be taking up a bunch of time. I suspect it is, but I don't want us to enter a rabbit hole of optimizing OMPL to find out the issue is in the planner plugin using OMPL.

Lets do some first-order testing on the bullets I listed above (maybe other ones too I missed?) to see if we can improve performance. You also bring up a good point in changing the timeout to see if the difference in a path with 0.1 1 or 10 seconds is all that different. I assumed that timeout as a maximum time, not plan until expired type of thing (e.g. if you set to 100, would still return in 100 ms if it got a plan), maybe I'm dead wrong.

SteveMacenski commented 4 years ago

Hi just checking in here @shivaang12 @ardabbour . I think you have the new collision checker to use Shivang so I don't know if you got a chance to play around, or Abdul if you looked over the different options? I remember you mentioned that there were some defaults / settings that were really fine we might be able to make more coarse to drop compute time.

ardabbour commented 4 years ago

I added a propagation that doesn't use the ODESolver, and I added an optimization criteria that is a weighted combo of the costs in the costmap and the path length. The propagation is now parametric, and the user can choose between using the ODESolver (and which between Basic, Adaptive, and Error) and the more rudementary propagation. I couldn't see much difference in performance with regards to the propagation methodology/solver type used.

I also profiled the main planning loop, and it shows that the planning takes pretty much the exact amount of time we specify as a timeout. So specifying a 0.1 s timeout will result in a plan produced in 0.1 s.

This is where things get interesting: if the timeout is too small (< ~1 s), the planner says that the problem has been solved, but the path returned by the planner does not end up in the goal (it goes towards the goal but stops abruptly in the middle of the way - path is basically incomplete) and gets rejected automatically by the planning server. I suspect this might be because the plan is complete and valid in the control space but not in the geometric space of the problem. I would be happy if someone more experienced with OMPL can drop their two cents on this.

PS: The AdaptiveODESolver throws a strange compile-time error regarding odeint. It might be related to this issue.

In file included from /usr/local/include/ompl/control/ODESolver.h:50:0,
                 from /home/ardabbour/bin/nav2_ws/src/nav2_ompl_planner/include/nav2_ompl_planner/ompl_planner.hpp:32,
                 from /home/ardabbour/bin/nav2_ws/src/nav2_ompl_planner/src/ompl_planner.cpp:18:
/usr/include/boost/numeric/odeint/stepper/generation/make_controlled.hpp: In instantiation of ‘struct boost::numeric::odeint::result_of::make_controlled<boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double> > >’:
/usr/local/include/ompl/control/ODESolver.h:321:91:   required from ‘void ompl::control::ODEAdaptiveSolver<Solver>::solve(ompl::control::ODESolver::StateType&, const ompl::control::Control*, double) const [with Solver = boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double> >; ompl::control::ODESolver::StateType = std::vector<double>]’
/home/ardabbour/bin/nav2_ws/src/nav2_ompl_planner/src/ompl_planner.cpp:330:1:   required from here
/usr/include/boost/numeric/odeint/stepper/generation/make_controlled.hpp:65:58: error: no type named ‘type’ in ‘struct boost::numeric::odeint::get_controller<boost::numeric::odeint::runge_kutta_cash_karp54<std::vector<double> > >’
         typedef typename get_controller< Stepper >::type type;

Also, I often get a runtime error when a State is converted into a ScopedState, I still haven't gotten to the bottom of it, but it could be a pointer issue. I checked the OMPL.app codebase and it seems that I more or less did what they did here, so I can't figure out exactly where I'm going wrong.

shivaang12 commented 4 years ago

Currently experimenting with different OMPL provided planners. I have yet to play with footprint collision checking but will be done before weekend, I think.

SteveMacenski commented 4 years ago

I couldn't see much difference in performance with regards to the propagation methodology/solver type used.

That's disappointing but thanks for verifying. Any of them even slightly (5%) better? Nickel and diming performance can be helpful.

So specifying a 0.1 s timeout will result in a plan produced in 0.1 s.

I'm really not a fan of that behavior, but nothing we can do about it. Any performance changes between using different planning options? Some may be better suited for this type of planning problem to others. I thought in some channel of communication you mentioned some OMPL parameters you found that we could change for resolution or something that we could make more coarse to speed up? How do the paths change using the costmap info you mentioned?

Maybe file a ticket for the runtime error and type issue?

This is where things get interesting: if the timeout is too small (< ~1 s), the planner says that the problem has been solved, but the path returned by the planner does not end up in the goal (it goes towards the goal but stops abruptly in the middle of the way - path is basically incomplete) and gets rejected automatically by the planning server. I suspect this might be because the plan is complete and valid in the control space but not in the geometric space of the problem. I would be happy if someone more experienced with OMPL can drop their two cents on this.

@mamoll any suggestions? I have to imagine we can plan for very small spaces well below 1s using OMPL. The examples they're running are plans <5m for a particle. The benchmark I would have thought easily possible would be 100ms for that size of space. A greedy search approach is <<12ms.

mamoll commented 4 years ago

This is where things get interesting: if the timeout is too small (< ~1 s), the planner says that the problem has been solved, but the path returned by the planner does not end up in the goal (it goes towards the goal but stops abruptly in the middle of the way - path is basically incomplete) and gets rejected automatically by the planning server. I suspect this might be because the plan is complete and valid in the control space but not in the geometric space of the problem. I would be happy if someone more experienced with OMPL can drop their two cents on this.

@mamoll any suggestions? I have to imagine we can plan for very small spaces well below 1s using OMPL. The examples they're running are plans <5m for a particle. The benchmark I would have thought easily possible would be 100ms for that size of space. A greedy search approach is <<12ms.

OMPL planners' solve method returns a PlannerStatus type that supports casts to bool. Both exact and approximate solutions convert to true. So just check whether the returned value by solve is EXACT_SOLUTION.

Numerical integration + kinodynamic planning won't give you great performance out of the box. You really need to have a steering function for your dynamical system, because otherwise OMPL is just going to sample random controls. Note that not all kinodynamic planners (planners in the ompl::control namespace) use the steering function, but you can modify them to do so. I'm actually playing with that myself now. Creating a steering function means implementing a StatePropagator with a steer() implementation and have canSteer()n return true.

The geometric approach taken by ReedsSheppStateSpace and DubinsStateSpace (where you have analytic solutions for the optimal trajectories) will almost always be faster than kinodynamic planning. If curvature-constrained curves are all you need, and you can quickly time-parametrize them, then that's the way to go. With the appropriate cost function you can also penalize driving backwards with ReedsSheppStateSpace, so that the motions look more "natural" and minimize the number of direction reversals. Balkcom-Mason curves (and variations of this) can easily be added (PRs welcome 😃).

SteveMacenski commented 4 years ago

I believe that @ardabbour implemented a propagator function, unless I'm mistaken on understanding on what he was doing here. I believe what's that is doing is the transition function between the to and from state analytically - which is the same as the steering function's role?

My concern with Reeds-Shepp or Dublin curves is that they're typically used to model ackermann vehicles. While that is certainly a class of robot we want to support, many robots can turn in place or are omni-directional. We don't want the planner to be either failing to compute a valid path because it doesn't know the robot can turn in place or not taking advantage of its kinematics. Do you think it would be a good approach to use them with a minimum radius of 0? With your comment on curve-constrained curves, that's not really what we need or want. We're really looking to make a valid costmap-aware smooth(ish) sampling planner alternative to the Navigation Potential Function.

mamoll commented 4 years ago

@SteveMacenski state propagation specifies how the system evolves given a starting state and after applying some control input for some time duration. The resulting state is the state reached at the end of that duration. With steering you try to compute the control and duration that will take the system from an initial state to a desired final state (or least close to it). This is typically hard to compute.

Another, more complex solution is to have planning at multiple levels:

SteveMacenski commented 4 years ago

@shivaang12 @ardabbour any progress?

ardabbour commented 4 years ago

Wish I had better news, but I haven't had time to look at it yet

SteveMacenski commented 4 years ago

Got it - thanks. From your experience here, do you think we'll get the kind of performance we're looking for (>3 hz, smooth, costmap respecting)? I'm starting to wonder from what we're seeing here if OMPL is just the wrong technology set for low-dimensional planning needing speed like what we're dealing with. These seem to be unrealistically slow. I'm not saying we should stop working on this if we think there's potential - but asking that we maybe re-evaluate if there is potential.

shivaang12 commented 4 years ago

I have done some rigorous testing, lets talk tomorrow during wg meet.

SteveMacenski commented 4 years ago

Awesome!

ardabbour commented 4 years ago

do you think we'll get the kind of performance we're looking for (>3 hz, smooth, costmap respecting)?

I think it's fair enough to say that a geometric approach would result in more hz with smooth paths, but it involve will involve implementing Balkcom-Mason curves (which I remember reading somewhere that Reeds Shepp curves reduce to - meaning they can be optimal kinematically - I will look for that paper again) if we want to include diff-drive robots. With a control space planner, 3-5 hz seems realistic on medium-sized maps (maybe half the willow garage map?) on a laptop even with dynamics constraints, but something more restricted like a Raspberry Pi might be challenging. I have generated plans with many more DOFs in tighter time constraints for arms through OMPL on MoveIt, so I don't think that we are going in the wrong direction, just that I haven't implemented it properly yet :)

SteveMacenski commented 4 years ago

My concern with the curves is that it would limit the generality of the plugin. I'd ideally like it to work with the major base models (diff, omni, ackermann) and allow people to select the OMPL planner of choice. I'm really surprised that we can't compute a plan from A->B on a modest sized map in 100ms. Its between 5 and 50x slower than a search based method. I expected it to be a bit slower, but not slower than 100ms for a reasonable-ish path.

mamoll commented 4 years ago

@ardabbour if you plan to look into Balkcom-Mason curves, you may also want to look into the generalization presented in:

A. A. Furtuna and D. J. Balkcom, “Generalizing Dubins curves: Minimum-time sequences of body-fixed rotations and translations in the plane,” The International Journal of Robotics Research, vol. 29, no. 6, pp. 703–726, 2010.

@SteveMacenski For optimal planning performance you should really use analytic solutions for (locally) optimal trajectories such as those given by the Dubins curves, Reeds-Sheep curves, etc. The analytic solutions are only available for certain vehicle types, but that doesn't seem like a major limitation to me. There aren't too many models other than the ones you list, so having a special state space for each seems acceptable. Presumably the search-based methods you mention use some sort of state-space lattice with carefully selected motion primitives that are vehicle-type specific, no?

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. Are there scenarios where the decoupled planning approach of generating a path first and time parametrization second (as is done for the ompl::geometric::* planners) has serious limitations in practical scenarios?

SteveMacenski commented 4 years ago

@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 commented 4 years ago

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 commented 4 years ago

@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 commented 4 years ago

@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. rrtstar

@SteveMacenski Is this acceptable?

SteveMacenski commented 4 years ago

@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 commented 4 years ago

@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.