Implement Critic for Velocity Deadband Hardware Constraints

[perchess]

---

Basic Info

Some platforms have deadband velocity constraints and cannot move slowly. For instance, the legged robot Unitree Go2. This PR adds a critic that penalizes trajectory samples below a deadband value.
A robot with hardware limitations cannot reach the goal with a high tolerance (0.03m in my Gazebo simulation). I have attached videos comparing my feature with the 'out of the box' MPPI.
Original MPPI
With deadband critic

Info	Please fill out this column
Ticket(s) this addresses	discussion in slack
Primary OS tested on	(Ubuntu 22.04)
Robotic platform tested on	Gazebo: omni robot linorobot2; Real: Unitree GO2
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

• Add new critic that penalizes trajectory samples lower than a deadband value

Description of documentation updates required from your changes

• Added new parameter, so need to add that to default configs and documentation page

Sample to copy paste into MPPI readme:

Velocity Deadband Critic

Parameter	Type	Definition
cost_weight	double	Default 100.0. Weight to apply to critic term. (100.0 fits for turtlebot sim, 35.0 for omni robot)
cost_power	int	Default 1. Power order to apply to term.
deadband_velocities	double[]	Default [0.05, 0.05, 0.05]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action.

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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

[SteveMacenski]

Still a couple of linting issues it seems. What’s a good tuned value for this critic? You should make sure to update the packages readme with this critic and navigation.ros.org’s MPPI config guide

[perchess]

navigation.ros.org’s MPPI config guide updated. I've opened the PR link

[SteveMacenski]

Generally seems good to me! Only things:

• Remove from default config file, that was tuned for general purpose users and this is a more special-purpose critic (like Twirling)
• Is 100 a sane default? I know it'll need to be high to make an impact considering the small numbers we're talking about, but just checking that this isn't over-penalizing from your tuning

[perchess]

Yes, 100 is a lot. I recommend users to tune the weight for their specific deadband velocity. Small weight is suitable for omni platforms, for diff-drive need to add extra weight.

[SteveMacenski]

Lets change to 35 to match the docs -- folks obviously need to tune to their situations, but I'd prefer lower than higher in general to not overconstrain the optimizer out of the box. DWB has that problem and has caused alot of people issues in the past. I prefer a looser out of the box tuning that works well but leaves some room to up numbers without making the system fall apart

Not saying that 100 would, but if you're saying 35 is passable for some to start, that sounds about right to me

[SteveMacenski]

/opt/overlay_ws/src/navigation2/nav2_mppi_controller/test/critics_tests.cpp:658
The difference between costs(1) and 56.7 is 36.854998779296878, which exceeds 0.01, where
costs(1) evaluates to 19.845001220703125,
56.7 evaluates to 56.700000000000003, and
0.01 evaluates to 0.01.

😄 One last thing

[codecov]

Codecov Report

Attention: Patch coverage is 52.27273% with 21 lines in your changes are missing coverage. Please review.

Project coverage is 89.82%. Comparing base (8af70ab) to head (9d66e3a).
Report is 3 commits behind head on main.

Files	Patch %	Lines
...ontroller/src/critics/velocity_deadband_critic.cpp	52.27%	21 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #4256      +/-   ##
==========================================
+ Coverage   89.75%   89.82%   +0.06%     
==========================================
  Files         431      432       +1     
  Lines       19574    19618      +44     
==========================================
+ Hits        17569    17621      +52     
+ Misses       2005     1997       -8     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.