graceful_controller: implement iterative selection of control points

[mikeferguson]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4115
Primary OS tested on	Ubuntu
Robotic platform tested on	UBR-1
Does this PR contain AI generated software?	No, only real intelligence was used

---

Description of contribution in a few bullet points

• Iteratively selects a valid target pose to use with the control law. This is done by forward simulating the control law towards the target pose and rejecting the target pose if there is a collision detected.
• Adds collision checking to the "final_rotation". If this fails, we will attempt to roll out further movement via the control law (this is especially important for non-circular robots!). With this in mind, the parameter final_rotation has been renamed to prefer_final_rotation.
• Modifies how final_rotation is done a bit - this is closer to how the graceful_controller did it. When prefer_final_rotation is true, we ignore the rotation of the final pose in the path and drive straight towards (and then do an in-place rotation). Previously, this code was simply using final_rotation as "do we latch and rotate", which is only half the battle if you aren't using a kinematically aware planner.

Description of documentation updates required from your changes

• Replaces motion_target_dist parameter with two new parameters: min_lookahead and max_lookahead.
• Renames final_rotation to prefer_final_rotation (to match graceful_controller and also indicate that this doesn't actually ensure there is a final rotation, just that the controller will prefer to generate one if it is collision free)
• Replace boolean initial_rotation and double initial_rotation_min_angle with initial_rotation_tolerance which is a better name for what this parameter does (and avoids needing two parameters to do one thing). This also involved a number of tests updates to ensure that initial_rotation_tolerance cannot conflict with allow_backward.
• Adds parameter v_angular_min_in_place to make sure that rotation commands actually work.
• Fixed a test that was erroneously looking at twist.angular.x

Future work that may be required in bullet points

These will be done before this converts from DRAFT:

• This still needs to be tested on robot (I'm simply opening for visibility and to see where we are in terms of code coverage)
• There is a TODO around returning cmd_vel when TF fails - this seems like the wrong thing to do, need to look at it closer - updated this to throw, similar to no command found
• Review parameter max_robot_pose_search_dist - seems to be a potentially VERY large number? This is consistent with other controllers in nav2
• Port orientation filter (at least NavfnPlanner doesn't set orientations)

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mikeferguson]

A few other things noted when porting to UBR-1:

• need to add a "min_in_place_vel_theta" equivalent - most robots can't generate arbitrarily low angular rotations
• Got my answer - yeah, not all planners output headings - 100% incompatible with the NavfnPlanner - I'm going to port over the orientation filter stuff as well then.

[codecov]

Codecov Report

Attention: Patch coverage is 95.14563% with 5 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...v2_graceful_controller/src/graceful_controller.cpp	93.24%	5 Missing ⚠️

Files with missing lines	Coverage Δ
...e/nav2_graceful_controller/graceful_controller.hpp	100.00% <ø> (ø)
...ude/nav2_graceful_controller/parameter_handler.hpp	100.00% <ø> (ø)
nav2_graceful_controller/src/parameter_handler.cpp	100.00% <100.00%> (ø)
...v2_graceful_controller/src/graceful_controller.cpp	89.69% <93.24%> (-0.82%)	⬇️

... and 5 files with indirect coverage changes

[mikeferguson]

This was the updated config for testing this on the UBR-1 (moving from the version of graceful controller in my repo): mikeferguson/ubr_reloaded@a24ad85

[mikeferguson]

Note for posterity: I did also test "allow_backward" - and it still works

[mikeferguson]

Notes on things that are still different versus my controller (that maybe we'd want to do in the future):

• In the control law - the slowdown_radius is a poor approximation for actual deceleration limits. Really, you want to cap the velocity based on knowing you can slow to a stop by the time you arrive at the pose.
• 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.
• add_orientations just adds the orientations but doesn't do any more sophisticated filtering - I'm not actually convinced that additional filtering is really needed though (and, it always had a few odd edge cases - if we were to implement something here, it should really be a bit more sophisticated than what was in my controller).

[SteveMacenski]

I'll admit, this was reasonably hard for me to review with the diffs and reordering of the main code, so I'm not 100% confident in not having missed something on an initial look. I'll probably need another complete read through after changes to sanity check.

@ajtudela since this touches the docking code and I know you're thinking about these areas - want to review / test?

[SteveMacenski]

Why isn't this considering if we want to rotate to the goal using the prefer_final_rotation parameter?

[mikeferguson]

So, there are really two different features at work here:

1. prefer_final_rotation is actually the feature down on L212 - when we are close to the goal (less than max lookahead) and prefer final rotation, we will force the target_pose heading to be the "heading to target pose" (imagine robot is at 0,0,0 and the target_pose is 0.1,0,1.57 - if you feed this into the control law, you take a massive sweeping arc to try and approach that 1.57 heading straight on. Instead, we use atan2(0.0, 0.1) and will end up doing no sweeping turn and then just turning in place at the end). I've found this to be probably the most important feature for actually successfully following the majority of paths regardless of the planner than generated them (and especially true for planners that are not kinematics aware).

2. When you've reached the XY goal, if prefer_final_rotation is true you probably have a huge heading error - but even if you aren't preferring final rotation you might have a small heading error (usually, we satisfy the heading requirement before we satisfy XY - but, if your localization is a bit unstable, that may not be the case). In either case, the control law gets quite unstable when the XY error is so small, and it can generate huge swooping motions to correct the heading, so you really ALWAYS want to do that final bit of rotation when you are already sitting on top of your goal.

In some ways, this second part is like the latch_xy_goal_tolerance behavior that other controllers have - in my controller, I actually had that parameter, and goal_reached_ (as this code calls it) wouldn't be set to true, but rather to latch_xy_goal_tolerance. Regardless, in the existing graceful_controller outside nav2, you always entered this rotation behavior even if latch and prefer were set to false - you just wouldn't have it be sticky unless latch was true.

[SteveMacenski]

I'm curious about your thoughts from the ticket brainstorming #4115 (comment) (or is the wobbling here more or less gone with this method recomputing at the full rate?)

[mikeferguson]

I'm curious about your thoughts from the ticket brainstorming #4115 (comment) (or is the wobbling here more or less gone with this method recomputing at the full rate?)

I expect most of the wobbling will be gone since our motion_target can be so much farther away in most cases

[mikeferguson]

@ajtudela since this touches the docking code and I know you're thinking about these areas - want to review / test?

I don't think this affects the docking code at all, because that is only using the SmoothControlLaw - which is unmodified

[mikeferguson]

@SteveMacenski all review comments have been addressed - this is ready for another look when you get a chance

[ajtudela]

I'll admit, this was reasonably hard for me to review with the diffs and reordering of the main code, so I'm not 100% confident in not having missed something on an initial look. I'll probably need another complete read through after changes to sanity check.

@ajtudela since this touches the docking code and I know you're thinking about these areas - want to review / test?

Of course I'll test it on our robots on Monday morning.

This part doesn't change the controller of the docking but this is exactly the behaviour I need for the new following server. I'll have to figure out how to integrate this part into the server reusing most of the code.

[ajtudela]

I tested the improvements with the UBR-1 this morning. Although I don't really like the new order of the "steps" (rotation before movement, control, final rotation, ...) because it's harder (for me) to read, I have to admit that these changes take care of the wobbling much better than before.

Tomorrow I'll test all the combinations for the new parameters to make sure I haven't missed anything.

I recorded a little video if you want to see the changes in motion:

new_graceful.mov

[mikeferguson]

I have to admit that these changes take care of the wobbling much better than before.

What are you running for the k_phi/k_delta parameters with the controller? I found that the defaults in the controller right now are pretty aggressive on heading correction (which causes both wobbling, and divergence from the path - which appears to be also causing lots of replanning for you).

I went with lower values that were actually the defaults in https://github.com/mikeferguson/graceful_controller:

GracefulController:
      plugin: "nav2_graceful_controller::GracefulController"
      v_linear_min: 0.1
      v_linear_max: 1.0
      v_angular_max: 2.8
      v_angular_min_in_place: 0.6
      max_lookahead: 1.25
      initial_rotation: true
      initial_rotation_tolerance: 0.25
      prefer_final_rotation: true
      # The defaults aren't as good as this (more wiggling)
      k_phi: 2.0
      k_delta: 1.0
      beta: 0.4
      lambda: 2.0
      # This isn't quite comparable to using actual acceleration limits
      # (but works well in practice)
      slowdown_radius: 0.75

I didn't update these defaults - at first thinking it might impact some people's docking servers - but upon further review, it looks like that isn't the case (the docking server has it's own parameter set). @SteveMacenski thoughts? Should we update the defaults here?

[ajtudela]

Yes, they might be a bit high. I used these:

    Graceful:
      plugin: nav2_graceful_controller::GracefulController
      transform_tolerance: 0.5
      motion_target_dist: 1.2
      initial_rotation: true
      initial_rotation_min_angle: 0.75
      final_rotation: true
      allow_backward: false
      k_phi: 3.0
      k_delta: 2.0
      beta: 0.4
      lambda: 2.0
      v_linear_min: 0.1
      v_linear_max: 1.0
      v_angular_max: 5.0
      slowdown_radius: 0.5

But I agree with you. I had to lower k_phi/k_delta for the docking server. I'd try with these settings tomorrow.

[mikeferguson]

I had to lower k_phi/k_delta for the docking server

I think I might have actually been the one who chose those higher values for the docking server initially - that is an application where you want the robot to come at the charge dock as straight-on as possible, so the values make sense for docking. I think they are too high for general navigation controlling (but it wasn't something I caught back when we originally added the controller here)

[ajtudela]

I made a new video this morning. Lowered k_phi/k_delta reduces the wobble more.

new_graceful_02.mov

[mikeferguson]

@SteveMacenski bumping this - wasn't sure if you maybe missed my comment that this was ready for another review

[SteveMacenski]

I found that the defaults in the controller right now are pretty aggressive on heading correction (which causes both wobbling, and divergence from the path - which appears to be also causing lots of replanning for you).

but upon further review, it looks like that isn't the case (the docking server has it's own parameter set). @SteveMacenski thoughts? Should we update the defaults here?

I think I might have actually been the one who chose those higher values for the docking server initially - that is an application where you want the robot to come at the charge dock as straight-on as possible, so the values make sense for docking. I think they are too high for general navigation controlling (but it wasn't something I caught back when we originally added the controller here)

We can change those, definitely! I want all defaults to have a really nice out of the box experience to showcase algorithm's values without the need of heavy tuning for 'most' applications.

[SteveMacenski]

That's pretty tight - we'd stop whenever the angle is > 14 deg off. I don't think that's a sensible default behavior to have the robot stop that much. Is that what's causing all the halt and rotates in the video that @ajtudela posted?

Maybe the value before of ~43 deg was too high? But I think this is definitely too low

[mikeferguson]

An initial rotation only happens when a new plan arrives - I suppose if you have continuous replanning running, this would be a problem - but is that the default out of the box?

[SteveMacenski]

Or also that there is no path further away than params_->min_lookahead. I don't see how that is gracefully handled for short paths (or very long lookaheadsv for larger robots like tractors)

[mikeferguson]

Once the goal is < max_lookahead away, we stop enforcing the requirement that target distance > min_lookahead