Apply first order lag low-pass filter to MPPI controller #6

chanhhoang99 · 2025-06-06T03:01:04Z

Drafting mppi model lag compensation

Signed-off-by: chanhhoang99 <[email protected]>

SteveMacenski · 2025-06-06T23:14:16Z

nav2_mppi_controller/src/critics/constraint_critic.cpp

@@ -57,6 +57,18 @@ void ConstraintCritic::score(CriticData & data)
    return;
  }

+  auto adapt = dynamic_cast<AdaptiveMotionModel *>(data.motion_model.get());


I think we can just add the lag compensation to the controller's motion models by default so we don't need to add this override

SteveMacenski · 2025-06-06T23:15:30Z

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

+  explicit AdaptiveMotionModel(bool is_holonomic, ParametersHandler * param_handler, const std::string & name)
+    : is_holonomic_(is_holonomic)
+  {
+    auto getParam = param_handler->getParamGetter(name + ".AdaptiveConstraints");


Lets just add this to the default motion model base class. I think we just need to make the default effectively disabled (or have an enable_lag_compensation parameter that's default off so we can keep these parameters as reasonably tuned defaults.

Also: are these reasonably tuned defaults for a broad range of platforms?

SteveMacenski · 2025-06-06T23:16:05Z

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

+      }
+    }
+
+    void predict(models::State & state) override


I'm going to refine from reviewing this function until its integrated into the main motion model plugin so I only look at your additions or changes :-)

SteveMacenski · 2025-06-06T23:16:40Z

nav2_mppi_controller/src/optimizer.cpp

@@ -27,7 +27,7 @@

 namespace mppi
 {
-
+geometry_msgs::msg::Twist last_cmd_vel_;


? make this a class member please not a global

SteveMacenski · 2025-06-06T23:17:00Z

nav2_mppi_controller/src/optimizer.cpp

@@ -137,6 +137,9 @@ void Optimizer::reset()
  generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);

  noise_generator_.reset(settings_, isHolonomic());
+  last_cmd_vel_.linear.x = 0.0;


You can just do last_cmd_vel_ = Twist() to reinitialize as empty

SteveMacenski · 2025-06-06T23:17:55Z

nav2_mppi_controller/src/optimizer.cpp

@@ -209,6 +220,11 @@ void Optimizer::prepare(
  costs_.setZero();
  goal_ = goal;

+  auto adaptive_model = std::dynamic_pointer_cast<AdaptiveMotionModel>(motion_model_);
+  if (adaptive_model) {
+      adaptive_model->updateTau(robot_speed, last_cmd_vel_, settings_.model_dt);


If its the motion model base class, you wouldn't need to do this and could always just run this function

chanhhoang99 · 2025-06-11T08:10:03Z

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

+
+        // float vx_new = use_adaptive_lag_ ?
+        //   vx_last + model_dt_ * (cvx_feasible - vx_last) / tau_vx_ :
+        //   cvx_feasible;


This is our initial thought that would improve the predict function

If that's how the theory does it, then that looks reasonable to me!

chanhhoang99 · 2025-06-11T08:35:46Z

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

+        float & cvx_cmd = state.cvx(j, i - 1);
+        float min_vx_acc = vx_last + min_delta_vx;
+        float max_vx_acc = vx_last + max_delta_vx;
+        float cvx_feasible = utils::clamp(min_vx_acc, max_vx_acc, cvx_cmd);


Hi @SteveMacenski ,
Our intention at first is to limit robot's acceleration at 0.5m/s^2 (We clamp acceleration to 0.5m/s^2 in our driver controller).Hence, our initial thought is to use FirstOrder lag model in MPPI to compensate the acceleration above. But when I tried as your suggestion to apply FirstOrder lag model in MotionModel class, I saw that even when enable or disable the first oder lag model, nothing change much but robot can still do pretty good job.

I think above is the change that makes our observation looks good.

I wonder that why did we apply constrain to state.cvx even though we already applied constrain acceleration to tracjectory scoring.
Let's now focus on acceleration constrain part and maybe ignore the firstoder lag solution.

I have carried out some experiments with the original implementation

float & cvx_curr = state.cvx(j, i - 1); cvx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, cvx_curr); state.vx(j, i) = cvx_curr; // wz and cwz ....

Below is my experiment base on the above original snipped code scenarios:

From the snipped code above, I did not clamp both state.vx and state.cvx (state.wz and state.cwz) with acceleration constrain. In my driver controller, we set acceleration clamping of az and ax to 0.5m/s^2. The result is that robot oscillated along the path like a snake slithering.

From the snipped code above, I did not change any code and keep it same as original. I change az max, ax max 0.5m/s^2 and ax min to -0.5m/s^2 and set the driver controller's az and ax max to 5.0m/s^2. The result is that the robot could not able to maneuver towards the path. I witnessed that the samples produced by MPPI does not stay on the path when robot is rotating to it path heading.

From the snipped code above, I only apply acceleration constrain to state.vx(state.wz) and do not touch state.cvx. Set az max, ax max as 0.5m/s^2 and ax min as -0.5/m^s and and set the driver controller's az and ax max to 0.5m/s^2.
This setup works pretty well, samples produce by MPPI stay on the path, robot is able to stick to the path even with very low acceleration 0.5m/s^2, we also tried to test with acceleration of 0.1m/s^2 and MPPI still can control the robot stick to its path (which is really hard to just control by teleop keyboard).

In short:
In case 1, we tested if MPPI is able to predict without knowing acceleration clamping in the driver controller.
In case 2, the acceleration constrain seems not to work even if acceleration clamping in the driver controller is bigger than MPPI acceleration constrain.
In case 3, we only apply acceleration constrain to state.vx and state.wz for trajectory scoring and clamp acceleration in our driver controller.

How do you think about this ?
Do you need video and plotting of 3 cases ?

But when I tried as your suggestion to apply FirstOrder lag model in MotionModel class, I saw that even when enable or disable the first oder lag model, nothing change much but robot can still do pretty good job.

I don't understand, isn't this the same as your custom adaptive class but just implemented within the base motion model? Why would moving it change behavior if implemented the same?

I did not clamp both state.vx and state.cvx

I think you did, the cvx_curr is a reference.

I'm not sure I understand the question. I think you tested a solution already as working - I only asked that those modification be made in the MotionModel predict function so all motion models can benefit from your improvements :-)

I don't understand, isn't this the same as your custom adaptive class but just implemented within the base motion model? Why would moving it change behavior if implemented the same?

Sorry for confusing you. During following your suggestion to implement the model into motion model class. I tested the flag to make sure that if the solution or theory above is working or not. By enable and disable the flag, I witnessed there is no difference at all, robot is still operating fine even without the first order lag flag enable.

This leads me into look at 1 other change that I made in this review. It is the change of below that makes difference in our observation (which I thought that first order lag would solve)

- float & cvx_curr = state.cvx(j, i - 1); - cvx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, cvx_curr); - state.vx(j, i) = cvx_curr; + float & cvx_cmd = state.cvx(j, i - 1); + float min_vx_acc = vx_last + min_delta_vx; + float max_vx_acc = vx_last + max_delta_vx; + float cvx_feasible = utils::clamp(min_vx_acc, max_vx_acc, cvx_cmd);

Without clamping reference to state.cvx, those sample spawned by MPPI stay on the path so the robot maneuver correctly to the path heading. Otherwise, the robot likely overshoot when rotating to the path heading.(Robot's initial heading is inverse to path heading)

Ah ok, please update the PR with all that - I think that's OK but I want to think about that a little more once I can look at the final form. That is the role of the control cvx versus command vx separation. However, the cvxs are what are used to populate the control sequence https://github.com/ros-navigation/navigation2/blob/main/nav2_mppi_controller/src/optimizer.cpp#L469 which means that the vxs that are integrated for the critic scoring wouldn't be representative of the control sequence output.

The cvx used to populate the control sequence could violate the acceleration constraints. applyControlSequenceConstraints would apply the constraint on the final trajectory so it would be valid, but I'm not sure why you would see the difference based on not updating vs updating cvx. Any thoughts on that?

Can you open a PR against the main Nav2 repository? That would help discoverability for this from me

While I was drafting predict() function. I thought that cvx should not be clamped because:
https://github.com/ros-navigation/navigation2/blob/2a80675ff2bd94eb8e3427d48dd673157103552f/nav2_mppi_controller/src/noise_generator.cpp#L66. So noises velocity has control sequence as foundation that will be used in next prediction. At https://github.com/ros-navigation/navigation2/blob/2a80675ff2bd94eb8e3427d48dd673157103552f/nav2_mppi_controller/src/optimizer.cpp#L278. , control sequence has been clamped with accel constrain.
Then, it is again clamped in the predict function in the next iteration.Is this intended ? It's my guess, please correct me if i am wrong.

Can you open a PR against the main Nav2 repository? That would help discoverability for this from me

Sure! Along with that I just sent you an email with videos of 2 test cases and bags.
New PR link: ros-navigation#5266

So the workflow now is to:

Clamp the controls to match the velocities with the acceleration limits

Forward project the velocities to create trajectories (that match the controls) and score them with critics

Use the scores to create the next optimal control by combining the scores with the controls (that are clamped to be valid)

Put hard constraints on the acceleration validity of the output, changing the shape (but only slightly, if at all, since the controls should be valid)

The new one would be:

Clamp the velocities to with acceleration limits, the controls remain invalid

Forward project the velocities to create trajectories (that no longer match the controls) and score them with critics. So the trajectories being scored are not the same as the controls as inputs

Use the scores to create the next optimal control by combining the scores with the controls. These controls are not the same as the trajectories that were used to score.

Put hard constraints on the acceleration validity of the output, changing the shape which may be more extreme

I thought about this for a bit and I think the original method might break the theoretical foundations, but practically keeps the controls and velocities from diverging - which makes sure the optimal trajectory that is later constrained by the applying control sequence constraints should be not or barely infeasible, so we don't reshape the trajectory much. If we let them diverge, the shape of the optimal trajectory itself will change more from the final acceleration clamping since there are going to be samples included that are out of the acceleration bounds.

I want to say that is accounted for by the theory, but does make my eyebrow raise. For that reason, I want to get some folks to test this for us to make sure it doesn't have any unintended consequences.

SteveMacenski · 2025-06-11T17:44:11Z

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

-        cvx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, cvx_curr);
-        state.vx(j, i) = cvx_curr;
+        float & cvx_cmd = state.cvx(j, i - 1);
+        float min_vx_acc = vx_last + min_delta_vx;


Nit: If these aren't going to be used outside of the clamp, I think that can be reverted for cleanliness

SteveMacenski · 2025-06-11T17:46:52Z

nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp

+        // float vx_new = use_adaptive_lag_ ?
+        //   vx_last + model_dt_ * (cvx_feasible - vx_last) / tau_vx_ :
+        //   cvx_feasible;
+        state.vx(j, i) = utils::clamp(control_constraints_.vx_min, control_constraints_.vx_max, cvx_feasible);


Remove these, this is important not to include in the predict model. The role of this function is only to predict what the robot would do if this command was requested. If you add this, there are inverse side effects not allowing the robot to get to full speed since the limited speed maximum is the limit. When doing a softmax, there's some averaging of the best scores so it'll approach but never achieve full speed. This is a lessons learned I've had on writing MPPI

tasteRobotic and others added 2 commits June 5, 2025 10:14

Apply first order lag low-pass filter to MPPI controller

c81a06e

Update nav2_params.yaml

fc4c214

Signed-off-by: chanhhoang99 <[email protected]>

SteveMacenski reviewed Jun 6, 2025

View reviewed changes

tasteRobotic and others added 3 commits June 11, 2025 15:06

Migrate lag compensattion feature to MotionModel base class

7d1df11

Adjust default tau value

3cc0fe0

Disable tau, only apply accel constrain to vx

b1acedf

chanhhoang99 commented Jun 11, 2025

View reviewed changes

SteveMacenski reviewed Jun 11, 2025

View reviewed changes

SteveMacenski mentioned this pull request Jun 16, 2025

Unclamp noise velocity. ros-navigation/navigation2#5266

Merged

7 tasks

Apply first order lag low-pass filter to MPPI controller #6

Are you sure you want to change the base?

Apply first order lag low-pass filter to MPPI controller #6

Uh oh!

Conversation

chanhhoang99 commented Jun 6, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

chanhhoang99 Jun 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jun 12, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

chanhhoang99 Jun 13, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jun 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

chanhhoang99 Jun 11, 2025 •

edited

Loading

SteveMacenski Jun 12, 2025 •

edited

Loading

chanhhoang99 Jun 13, 2025 •

edited

Loading

SteveMacenski Jun 11, 2025 •

edited

Loading