[Graceful Controller] Additional Controller Techniques - Experimentation

[SteveMacenski]

Right now the Graceful Controller computes a smooth spiral curve from the control law from the robot's current state to a pose on the path to track. The path pose is based on a lookahead distance that is shortened up if there is a collision in the trajectory's spiral curve on the way. Getting out some ideas / thoughts so we can collaborate on this as a community instead of staying in some notes document

Linking @ajtudela @mikeferguson for visibility (feel free to mute & sorry to ping you if you don't care 😉 )

There are quite a few other ways, I think that this control law could be utilized that are worth experimenting with for improvements or different behaviors:

Current controller improvements

• Interpolate the path point for the distance requested by the lookahead, to smooth out the trajectories from frame to frame. We do this in the RPP controller, so this should be a relatively straight forward improvement that improves the controller as-is. Other RPP-based bells-and-whistles improvements?
• Consider committing to a given trajectory for a period of time before computing a new spiral. Each iteration, we compute a new spiral and perhaps its worth committing to the current spiral for a couple of seconds (except if collision or large change in cost) so we commit to actions more fully, before then computing a new curve. Compute a new curve after moving some distance or collision now in the spiral arc
• Consider sampling a set of spiral trajectories with different spiral parameters instead of them as static parameters. Sample within some range of values and score them for the best trajectory. Score on integrated path distance, obstacles, smoothness, length, etc. See [Graceful Controller] Additional Controller Techniques - Experimentation #4865 (comment) for insights.
• Get rid of the slowdown radius and do the forward simulation using proper acceleration limits. This same thing could be done with docking - where it might actually be a very big improvement (especially if people know their acceleration limits well, they can be certain the robot won't hit the dock real hard)
• "The collision checking in the iterative loop assumes you are starting at max speed (not at your current speed). This is probably not that big of a deal, but if you're going very slowly in a tight area, the controller might wobble a bit more." I think this is actually easier to do with the nav2 controller API than what existing in nav1 where we had to separately subscribe to odom and whatnot.
• Footprint expansion - as you go faster, your footprint is enlarged to account for the uncertainty in how well your controls will actually work. This is especially helpful if you have an environment which is quite varied in how open space it is (like, when you want to go 1.5m/s in wide open hallways but automatically slow down when entering a cubicle farm that is quite a bit less open).

---

New Graceful motion model controller - probably good

• Consider the above, with relaxation on the exact goal pose so we compute (collision valid) spirals towards or along the path, but no so exact. Further, we could sample based on some time duration into the future from the robot's starting pose + spiral parameters. Part of the scoring could be how close / near the path or it is (so not even considering the path's poses in the spiral constraints)
• Perhaps rather than randomly sampling (like RRT) or sampling in fixed intervals (like DWA) of the the spiral parameters, attempt to use convex optimization on some cost functions to select the best trajectory and goal. It looks like we'd be in good company in this direction and I'm reading between the lines that this is what the Amazon Astro does given Jong Jin Park's current employment and the relatedness of the graphics on these papers to some Amazon Astro blog posts Unconstrained Model Predictive Control for Robot Navigation under
  Uncertainty Robot Navigation with MPEPC in Dynamic and Uncertain
  Environments: From Theory to Practice Robot Navigation with Model Predictive Equilibrium Point Control dissertation

Some things we can score:

• Path smoothness
• Collision
• How well it follows the path // gets close to the desired intermediate goal pose on the path
• Adjust scoring weighting based on distance from obstacle (follow goal better when far from objects; avoid objects better when close to them)

---

New Graceful motion model controller - probably dumb?

• Consider the aforementioned, but in multiple timesteps. If we sample / optimize for a trajectory that is some time in the future, then we can do so again at that time to compute a new trajectory to hand it off to. For example: we compute trajectories that are 1s into the future via sampling or optimization. Then we do so again for another 1s and so forth to compute more sophisticated trajectories. This is somewhat similar to MPPI, but more coarsely and the trajectories themselves are known to be smooth and feasibly connect to each other. This would be quite computational and not sure its an improvement over MPPI, so perhaps this is better a thought experiment to pull ideas from for simpler modifications. I think Graceful as a simple, smooth trajectory planner is where we should keep it -- rather than being highly reactive or dealing with multiple time-step issues, where MPC/MPPI shine best. https://www.cs.cmu.edu/~iwan/papers/vfhstar.pdf

[mikeferguson]

A couple notes on the above ideas:

• I've actually never tried "interpolating path points" - but my gut says that would probably work pretty well
• I'm not sure about the "committing to a spiral" - my sense is that isn't causing any real issues (especially if you add some interpolation stuff). This is especially true if you have the continuous replanning turned on (since your whole plan disappears and gets replaced).
• For sampling different spirals, you might only need to sample across k1 parameter - to basically change the curvature (maybe also k2, but I feel like they are inter-related enough you might only have to vary one to get a pretty good set of spirals). That said, I think any sort of exhaustive search will get computationally nasty and negate some of the use cases for a lightweight controller.

Some other notes that I had sitting around:

• Biggest bang for our buck would probably be to get rid of the slowdown radius and do the forward simulation using proper acceleration limits. This same thing could be done with docking - where it might actually be a very big improvement (especially if people know their acceleration limits well, they can be certain the robot won't hit the dock real hard)
• I have a note in the previous PR that The collision checking in the iterative loop assumes you are starting at max speed (not at your current speed). This is probably not that big of a deal, but if you're going very slowly in a tight area, the controller might wobble a bit more. I think this is actually easier to do with the nav2 controller API than what existing in nav1 where we had to separately subscribe to odom and whatnot.

A few remaining features that exist in the older graceful controller, but not the nav2:

• Footprint expansion - as you go faster, your footprint is enlarged to account for the uncertainty in how well your controls will actually work. This is especially helpful if you have an environment which is quite varied in how open space it is (like, when you want to go 1.5m/s in wide open hallways but automatically slow down when entering a cubicle farm that is quite a bit less open). This can of course be achieved through speed limit maps, but that's a lot more manual to setup per installation.

[VArt3m]

Great ideas above, good to see improvements to Graceful controller which is my favorite so far.
Tacking on some possible improvements:

• Not just interpolating but extrapolating (extending) original paths near endpoint (goal). As far as I understand when the robot approaches the goal, the lookahead distance effectively shrinks because there are no further points. This leads to change of behavior of the controller (in my experience, for the worse) - it becomes much eager to rotate so if there's any CTE it will stop at a very awkward angle near the goal. In some cases final rotation in place can fix this, though this is undesirable in the first place, but some robots have Ackermann kinematics. I did improve this behavior in my project by adding a continuous path pre-processor extending path past original goalpoint (coupled with my own goal checker).
• Somewhat similar kind of pre-processing with cusps (forward-backward path reverse points) - in my project I prune the path beyond nearest cusp and extend trajectory like outlined above. When the cusp point is reached, the path beyond current cusp is fed to the controller. This helps to follow intended trajectory more precisely, allows for gradual slowdowns, and prevents the robot from "shortcutting" the trajectory - very good for three-point turns and Reeds-Shepp trajectories.
• Pruning the path to limit path direction change relative to the current point in path. I haven't tried this yet, but if we have - for example - a path with 180° turn with radius significantly smaller than lookahead then the robot will likely make a significant "shortcut" because the controller will find the next lookahead point father than the turn is. Pruning the path to the point that deviates for example no more than 90° or 60° in its orientation than current closest point on path would help to follow such turns more exactly.

[mikeferguson]

Not just interpolating but extrapolating (extending) original paths near endpoint (goal). As far as I understand when the robot approaches the goal, the lookahead distance effectively shrinks because there are no further points.

We actually do this in the docking stuff.. but for some reason I've never actually experimented with doing it for general navigation.

[SteveMacenski]

I'm not sure about the "committing to a spiral" - my sense is that isn't causing any real issues

From my extremely superficial testing of the controller, this seemed to cause some uncommitted action/wobbling, but also I was testing when the orientations weren't being computed correctly, so this may be moot now. I could easily also see with path point interpolation this being discarded too. Its worth toying with, but I agree with you, it could unnecessary / not particularly useful.

For sampling different spirals, you might only need to sample across k1 parameter - to basically change the curvature

This is a great insight, thanks!

I think any sort of exhaustive search will get computationally nasty and negate some of the use cases for a lightweight controller.

I don't think this has to be only a lightweight controller, I think we can offer a heavier-weight option that leverages the smooth control law to do more sampling or optimization-based solutions. Kind of like the Smac Planner sharing a common-A*, the Graceful Controller package could be a set of Controllers based on the shared spiral smooth control law. The current controller would always remain, but have some other additional options.

Get rid of the slowdown radius
Footprint expansion

Agreed and added both!

The collision checking in the iterative loop assumes you are starting at max speed (not at your current speed). This is probably not that big of a deal, but if you're going very slowly in a tight area, the controller might wobble a bit more. I think this is actually easier to do with the nav2 controller API than what existing in nav1 where we had to separately subscribe to odom and whatnot.

Is the action item to use the current velocity instead? Yeah, that would be very easy, I think we even provide that Twist to the controller when it computes its velocity. That's a good-first-issue kind of contribution too (I'm looking for more of those right now too).

[SteveMacenski]

Not just interpolating but extrapolating (extending) original paths near endpoint (goal). As far as I understand when the robot approaches the goal, the lookahead distance effectively shrinks because there are no further points. This leads to change of behavior of the controller (in my experience, for the worse) - it becomes much eager to rotate so if there's any CTE it will stop at a very awkward angle near the goal. In some cases final rotation in place can fix this, though this is undesirable in the first place, but some robots have Ackermann kinematics. I did improve this behavior in my project by adding a continuous path pre-processor extending path past original goalpoint (coupled with my own goal checker).

@VArt3m see the RPP controller, we actually do that already for these exact same reasons. I'd love if you contributed that here!

Somewhat similar kind of pre-processing with cusps (forward-backward path reverse points) - in my project I prune the path beyond nearest cusp and extend trajectory like outlined above. When the cusp point is reached, the path beyond current cusp is fed to the controller. This helps to follow intended trajectory more precisely, allows for gradual slowdowns, and prevents the robot from "shortcutting" the trajectory - very good for three-point turns and Reeds-Shepp trajectories.

@VArt3m see the RPP controller, we actually do that already for these exact same reasons. I'd love if you contributed that here! (wow, copy paste 😆 )

Pruning the path to limit path direction change relative to the current point in path. I haven't tried this yet, but if we have - for example - a path with 180° turn with radius significantly smaller than lookahead then the robot will likely make a significant "shortcut" because the controller will find the next lookahead point father than the turn is. Pruning the path to the point that deviates for example no more than 90° or 60° in its orientation than current closest point on path would help to follow such turns more exactly.

This is worth some testing. I would counter by saying that this controller really doesn't follow paths very precisely since it creates its own curve of its own design based on its own selection of a lookahead point on the path. I'm not sure this is the best controller algorithm if you want accurate path tracking in turns. However, if the behavior of that 180 turn is really bad, then I agree we should make some special arrangements. We could look at the orientations of the path points and use those as part of the calculation of the distance along the path (not just X,Y, but X,Y,Yaw distances with some yaw scale). I think that would probably work smashingly and also reduce the lookahead when doing normal turns to be more conservative as well. Either way, we'd make this a parameterized option so folks could turn it off/on as they saw fit.

@VArt3m open to some contributions? These sound like they're all pretty analog to other things we have, so that should make the implementation and review much more straight forward.