[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 Deemed unhelpful in [Graceful Controller] Additional Controller Techniques - Experimentation #4865 (comment)
• 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)
• 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.
• "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.

[SakshayMahna]

Hi, is this still under progress?
Can I work on this?

[SteveMacenski]

Currently no one is working on this, but a few people have expressed interest recently! If you'd like to work on part of this, I'd love the help and just mention here what you plan to do and a timeline so that others can know what is taken and when it could be expected for them to build upon!

[SakshayMahna]

Sure,
I'll start with this:

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

I can provide some updates on this by next week.

[SakshayMahna]

The current implementation of the Graceful Controller involves finding a point on the given trajectory that is within a look ahead threshold. It uses that point to construct a local trajectory and follows through it.

In my implementation, I tried to interpolate a point that falls within a lookahead distance on the given trajectory. And as well extrapolate a point not on the trajectory when it is about to reach the goal (Just like it is done in RPP). However, there is one slight thing to take care, Graceful Controller makes use of the position and orientation of the robot (RPP only uses position), therefore we need to interpolate/extrapolate an orientation as well. For now I took it as the angle from the interpolated point to the point that is closest to interpolated point on the trajectory.

Following are the results:

Current Implementation:
https://github.com/user-attachments/assets/6e1a501e-9456-4b16-8e79-4c4456d5a412

Proposed Implementation:
https://github.com/user-attachments/assets/62dadc00-eb49-4cae-b7ea-dda80409db7a

I had two questions:

• What exact improvements should we be expecting from this?
• Any ideas how we can exactly monitor/observe them?

[SteveMacenski]

Can you highlight the key differences you'd like us to note in the videos representing changes you made (just for clarity's sake)? I see that it appears to be smoother frame-to-frame when computing the spiral curves - which is very nice indeed! Does that come mostly from the interpolation?

What exact improvements should we be expecting from this?

I think that's up to some testing and engineering analysis to decide. If this is in response to my first improvement bullet in this ticket, I think this should create much smoother trajectories from frame-to-frame since there is much less jumping due to discretization. This was an important step for RPP for high speed vehicles, but given how sensitive Graceful is to changes in position and orientation, I would think it would be doubly as important here.

In the first video, you showed it short cutting the corner path - do you still see that? The second video didn't show a similar turn.

Any ideas how we can exactly monitor/observe them?

This visualization of the trajectories and the robot's actual motion thereby should be evident enough for this task, I think. It should be clear that (a) the trajectories look much more smooth and continuous w.r.t. the previous iterations rather than jumping around. (b) Additionally, if you ignore the trajectories and plot only the robot base frame, I would hope that you could see the robot's behavior itself is much more smooth as a result (RQT plot with the X/Y/Theta pose might also help there).

Let me know when you're ready for a code review! I'd love to get this in :-)

[SakshayMahna]

I conducted a few more tests, and have some observations.

1. Visually I don't see much difference between the performance. Both the controllers (current and new) seem to behave almost the same.
2. The difference can be observed in the plots of linear and angular velocities. Especially the linear velocity seems quite smoother because of this change. Plots shown below:
3. For the end path interpolation (extrapolation), it seems the original Graceful Controller implementation is better than the changed one. Visually I observe that in the original implementation, the robot slows down when it is about to reach the end of the trajectory. But because of the extrapolation I don't see that happening for the new implementation.

Linear Velocity Plots:

Original Implementation

New Implementation

Note that the waypoints/trajectory provided in the start and towards the end are same. (In between the goal poses provided are different).

However, the jitters in the original implementation velocity are quite evident as compared to the new implementation.

Angular Velocity Plots:

Original Implementation

New Implementation

Note that the waypoints/trajectory provided in the start and towards the end are same. (In between the goal poses provided are different).

Nothing major to focus here, but overall the angular velocity for new implementation seems slightly smoother compared to the current implementation.

If all seems fine with this improvement I can start with the PR and code review then.

[SteveMacenski]

The difference can be observed in the plots of linear and angular velocities. Especially the linear velocity seems quite smoother because of this change

Oh very nice, that is not only smoother (especially in angular jerk), but we spend more time moving at our maximum velocity, meaning we're also executing tasks faster!

For the end path interpolation (extrapolation), it seems the original Graceful Controller implementation is better than the changed one. Visually I observe that in the original implementation, the robot slows down when it is about to reach the end of the trajectory. But because of the extrapolation I don't see that happening for the new implementation.

I think it would be good to open 2 PRs. One that is the interpolation while tracking the path and another that does the end-of-path interpolation. We can have some folks review the end of path interpolation one separately that I think would be interested in that work and testing it out for their applications and wouldn't block the path-tracking part of this. We could also spot some errors about why its not slowing down to help resolve as well so it could be included :-)

Please open the PRs! I can mark this off on the TODO list for the graceful controller then 😄 I think the next point on committing to a given spiral would be the next good experiment to test out. I'm less certain that this will be a meaningful improvement (or an improvement), but I think it is worthwhile to investigate!