[WIP] Improve testability to explore velocity command generation for an Ackermann Steered Polaris

[aosmw]

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4217
Primary OS tested on	Ubuntu 22.04 - humble
Robotic platform tested on	test environment targeting Polaris ATV
Does this PR contain AI generated software?	No

---

Description of contribution in a few bullet points

Improve testability/reset-ability of nav2_mppi_controller state and create a velocity test to help explore parameters and attempt to build intuition around how max velocity is reached.

Added and documented the following new parameters

• dump_noise - dump noise to file for inpection

• noise_seed - control noise generation with a seed for reproducability

• visualize_generated_trajectories - visualise ALL generated trajectories

• visualize_optimal_trajectory - visualise just the optimal trajectory

• The last few commits of this PR are me trying to explore the problem space and are not intended to be merged in their current form.

Description of documentation updates required from your changes

• If the new parameters are acceptable they still need to be added them into the main default config.

---

Future work that may be required in bullet points

• I developed these changes on the humble branch and have rebased them onto mppi_opt. I attempted to build and test on rolling using the Dockerfile but note that that is not currently operational.
• The velocity_test is still very specialized to my needs and I have not yet found the secret to getting a cmd_vel to approach my desired vx_max of 3.5.
• The dummy factory structures should probably have all available parameters.

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

[mergify]

@aosmw, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@aosmw, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@aosmw, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[aosmw]

Here is a sample of the velocity_test parameter sweep report. I am forcing angular velocity z feedback to 0. Its getting higher speeds. AT LAST. Just going full speed backward. Could be the dummy path I am feeding it is behind the vehicle.

85   // Explore parameters for the Polaris ATV Ackermann
86   optimizer_settings.vx_max = 3.5;
87   optimizer_settings.vx_max = 0.5;
88   optimizer_settings.vx_min = -4.5;  // WTF we get a max velocity, just backwards!!!
89   optimizer_settings.vx_bias = 0.0;
90   optimizer_settings.wz_max = 0.75;
91   optimizer_settings.wz_std = 0.1;

#k,j,i,vx_max,vx_std,vx_bias,wz_max,wz_std,vx_in,wz_in,cmd_vel_vx,cmd_vel_wz
2,1,0,0.5,0.2,0,0.75,0.1,0,0,-3.48499,0
2,1,1,0.5,0.2,0,0.75,0.1,-0.174249,0,-4.26381,0
2,1,2,0.5,0.2,0,0.75,0.1,-0.378727,0,-4.78661,0
2,1,3,0.5,0.2,0,0.75,0.1,-0.599121,0,-4.71394,0
2,1,4,0.5,0.2,0,0.75,0.1,-0.804862,0,-4.61619,0
2,1,5,0.5,0.2,0,0.75,0.1,-0.995429,0,-4.5304,0
2,1,6,0.5,0.2,0,0.75,0.1,-1.17218,0,-4.49591,0
2,1,7,0.5,0.2,0,0.75,0.1,-1.33836,0,-4.48867,0
2,1,8,0.5,0.2,0,0.75,0.1,-1.49588,0,-4.49226,0
2,1,9,0.5,0.2,0,0.75,0.1,-1.6457,0,-4.4968,0
2,1,10,0.5,0.2,0,0.75,0.1,-1.78825,0,-4.49947,0
2,1,11,0.5,0.2,0,0.75,0.1,-1.92381,0,-4.5004,0
2,1,12,0.5,0.2,0,0.75,0.1,-2.05264,0,-4.50044,0
2,1,13,0.5,0.2,0,0.75,0.1,-2.17503,0,-4.50025,0
2,1,14,0.5,0.2,0,0.75,0.1,-2.29129,0,-4.50008,0
2,1,15,0.5,0.2,0,0.75,0.1,-2.40173,0,-4.5,0
2,1,16,0.5,0.2,0,0.75,0.1,-2.50665,0,-4.49998,0
2,1,17,0.5,0.2,0,0.75,0.1,-2.60631,0,-4.49998,0
2,1,18,0.5,0.2,0,0.75,0.1,-2.701,0,-4.49999,0
2,1,19,0.5,0.2,0,0.75,0.1,-2.79095,0,-4.5,0
2,1,20,0.5,0.2,0,0.75,0.1,-2.8764,0,-4.5,0
2,1,21,0.5,0.2,0,0.75,0.1,-2.95758,0,-4.5,0
2,1,22,0.5,0.2,0,0.75,0.1,-3.0347,0,-4.5,0
2,1,23,0.5,0.2,0,0.75,0.1,-3.10797,0,-4.5,0
2,1,24,0.5,0.2,0,0.75,0.1,-3.17757,0,-4.5,0
2,1,25,0.5,0.2,0,0.75,0.1,-3.24369,0,-4.5,0
2,1,26,0.5,0.2,0,0.75,0.1,-3.3065,0,-4.5,0
2,1,27,0.5,0.2,0,0.75,0.1,-3.36618,0,-4.5,0

[SteveMacenski]

@aosmw I'd suggest closing this PR until you have something ready for review or have any particular questions that need address. Changing the defaults in Nav2 Bringup for all users is not a contribution which we'd accept.

Its also hard for me to tell what changes you made considering you're including another unrelated branch in this PR. If nothing else, you definitely need to remove that / rebase on main so that this PR represents only your changes.

[aosmw]

The unrelated commits in this PR is the result of me living on the bleeding edge of your mppi_opt branch while I figured out the code base.

I will rebase it back on main now that you have merged your mppi_opt branch.

The parameter changes above are isolated to the internals of a new nav2_mppi_controller/test/velocity_test.cpp. The test factories remain as default.

The ultimate point of the changes were to be able to run loops over optimizer->evalControl(); while changing parameters and having optimiser->reset() actually reset all its internal state, and have the critics actually take on new values.

I will contribute some separate bug fix PRs for the internal state resetting and some changes to the test factories to allow parameters to be changed between calls to optimiser->reset().

I fell back to this low level of isolated testing because I could not understand the reasons for why our robot was not getting commands to drive at its vx_max when the path to its goal was straight ahead. Even when the only critics activated are the PathFollowCritic and PreferForwardCritic. (Yes we have a big offroad test area/field and multiple independent emergency stop systems.)

I also fell back to this low level test to isolate from noisy sensors, noisy localisation, rough terrain and purely concentrate on what values the controller was coming up with.

I would love a comment on the following. Is the following correct thinking about parameter selection?

SCENARIO

The time taken to manually drive a full circle at a reasonable speed(comfortable and safe) with the steering wheel at full lock is ~12 seconds.

This equates to a linear distance of $2\pi r$ => $2\pi 2.75 = 17.3m$, and linear speed of $s=d/t$ => $17.3/12 = 1.44m/s$, and an angular velocity of $w = v/r$ => $1.44/2.75 = 0.52 rad/s$

HYPOTHESIS

wz_max should be 0.52

A smaller than default wx_std(0.4) is more appropriate for a larger(than a turtlebot) Ackermann steered vehicle(2.75m turning radius).

BECAUSE

The trajectories generated by a large wx_std have a larger proportion of larger steering angles. Trajectories with larger steering angles lead to control predictions that if applied(integrated) at higher speeds, would see an Ackermann vehicle steer off course at a higher rate. The critics don't score these higher speeds with higher steering inputs favourably. Therefore lower linear speeds end up being favoured.

CONCLUSION

vx_max may not be reached in an Ackerman steered vehicle as it is coupled/affected by wx_max and wz_std.

An Ackermann Steered vehicle's angular velocity is coupled to its linear velocity. mppi noise parameter selection needs to take account of this coupling. To get higher cmd_vel.twist.linear.x control outputs, more predictions of smaller steering inputs need to be scored by the critics.

QUESTION

Is this coupling between vx_max and wx_std a fact of life?

[mergify]

@aosmw, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@aosmw, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[aosmw]

Here is a screenshot plotting the output of VelocityTest.ParameterSweep

python3 src/navigation2/nav2_mppi_controller/test/velocity_test_plot.py build/nav2_mppi_controller/nav2_mppi_controller_velocity_test.csv

The sequential colormap attempts to show the grouping of vx_max, vx_std

[SteveMacenski]

The unrelated commits in this PR is the result of me living on the bleeding edge of your mppi_opt branch while I figured out the code base.

Ah ok, a few changes I didn't immediately ack as my own. I knew some where, but sometimes how your brain pattern matches is context sensitive 😆

There is definitely a coupling of the ackermann constraint - to save you some time, its computed here: https://github.com/ros-planning/navigation2/blob/main/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp#L125-L126. In it, we set the angular velocity to be scaled back so that we meet the turning radius constraint for the given translational velocity. Though, it is just as valid to consider the opposite case where we reduce the vx to allow for a given wz.

--

On your chart, I suppose it doesn't surprise me that if you have higher speeds, higher std values might be valuable to have quicker responses on acceleration since it can effectively search more of the parameter space for the critics to evaluate. Your wz seems low at 0.1, but that depends on how extreme of turns you're interested in exploring.

It doesn't grokk with me immediately that having a lower std would intrinsically result in a lower velocity cap. I would just expect that it would take more iterations to explore the parameter space fully to get up to speed (i.e. so running faster would help). I'm thinking that the critics that are being evaluated in this situation have a local minima that these are settled in instead.

I'd be curious if you ran that again with the speed starting high if for those situations that it approaches the non-max-speed limit, if it goes back down to those values or keeps at the max speed to understand if it is a local minima or a global minima to go at that speed.

Do you see this same behavior for the differential drive motion model? That can also help us identify if this is unique to the ackermann introduced constraint or a system behavior

[mergify]

@aosmw, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[aosmw]

Here are some screenshots showing improvements to plotting the output of velocity_test. The plots are of a reduced sweep of vx_std, eliminating the unstable responses. There is a subplot of angular velocity (cmd_vel_wz), and one showing how the current velocity fed back into optimiser->evalControl() is varied. The plots indicate that even with high speed initial conditions the control returns to the lower than expected cmd_vel.

The first plot is has a simple low pass filter to feedback the output of optimiser->evalControl() back into itself.

The second plot shows instant feedback.

I will make the adjustments to run it for the differential drive motion model.

[SteveMacenski]

Closing due to lack of reponses