add open loop option for MPPI #5617

chanhhoang99 · 2025-10-16T08:12:57Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #5214)
Primary OS tested on	(Ubuntu)
Robotic platform tested on	(Personal real platform)
Does this PR contain AI generated software?	(No)
Was this PR description generated by AI software?	No

Description of contribution in a few bullet points

It is unavoidable that it takes time for motors to reach a specific setpoint. Espescially, in case where acceleration and model_dt are small. The velocity steps (example 0.0 -> 0.025 -> 0.05 -> 0.075 ...) are also tight. Hence, torque generated by motor is very little and it's hard to move the motor. For instance, MPPI is running with model_dt=0.05, this means MPPI will get odometry every 50ms .But at 50ms, the motor has not yet reach it setpoint (at that time the torque generated my motor is not strong enough).
When close loop mode is used, MPPI will get the current odometry and add small velocity step for next setpoint every control iteration. Because of that, motor still could not generate enough torque.
When open_loop is used, MPPI uses last command velocity instead of odometry, this helps small velocity steps accumulate every control iteration and at some point, it will be enough to populate the motor.

Description of documentation updates required from your changes

Description of how this change was tested

With

      ax_max: 0.1
      ax_min: -0.1
      az_max: 0.1

Before the change

Screencast.from.2025-10-16.11-15-26.mp4

After the change
https://www.youtube.com/watch?v=HbLMPBcHRgI

Future work that may be required in bullet points

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
Should this be backported to current distributions? If so, tag with backport-*.

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

nav2_mppi_controller/README.md

nav2_mppi_controller/src/optimizer.cpp

SteveMacenski · 2025-10-16T15:12:22Z

nav2_mppi_controller/src/optimizer.cpp

-  state.wz.col(0) = static_cast<float>(state.speed.angular.z);
+  const bool open = settings_.open_loop;
+
+  const float vx0 = open ? control_sequence_.vx(0)


Is this correct considering we pop the initial one when we send it?

These lines of code, the purpose is to take the last command velocity sent to the motor, give it to the MPPI predict() steps.
I think that control_sequence_.vx(0) is the last cmd_vel that sent to the motor.

But control_sequence_.vx(0) is not the last command sent to the motors. After we send a command to the motors, the trajectory is slid over to initialize for the next iteration

navigation2/nav2_mppi_controller/src/optimizer.cpp

Lines 273 to 285 in 47d13ce

void Optimizer::shiftControlSequence()

{

auto size = control_sequence_.vx.size();

utils::shiftColumnsByOnePlace(control_sequence_.vx, -1);

utils::shiftColumnsByOnePlace(control_sequence_.wz, -1);

control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2);

control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2);

if (isHolonomic()) {

utils::shiftColumnsByOnePlace(control_sequence_.vy, -1);

control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2);

}

}

I think control_sequence_.vx(0) is now control_sequence_.vx(1) from the last iteration, since it is shifted in https://github.com/enwaytech/navigation2/blob/main/nav2_mppi_controller/src/optimizer.cpp#L219

But we don't always use 1st point, we sometimes use 0th depending on the setting of the control frequency and timestep https://github.com/enwaytech/navigation2/blob/main/nav2_mppi_controller/src/optimizer.cpp#L443

I think you need to use the last control velocity as the state initial for open loop. If you stored that and set it in the open loop setting, that seems to me to be the easiest way.

Ah okay, forgot that setup where we dont always shift control sequence.
So now we will create member variable in Optimizer, store the last_cmd_vel then use it next iteration.

@SteveMacenski, Should I create those variables in State struct ?
https://github.com/enwaytech/navigation2/blob/5c763e22d4b188a18d7d323e992159c181bb21e9/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp#L40

So now we will create member variable in Optimizer, store the last_cmd_vel then use it next iteration.

Sounds good! I thnk this can be a member of the Optimizer class, since its not regarding the current trajectory state

nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp

nav2_mppi_controller/README.md

Fix linting Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

codecov · 2025-10-17T04:44:44Z

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
...nav2_mppi_controller/models/optimizer_settings.hpp	`100.00% <ø> (ø)`
...troller/include/nav2_mppi_controller/optimizer.hpp	`100.00% <ø> (ø)`
nav2_mppi_controller/src/optimizer.cpp	`96.96% <100.00%> (+0.04%)`	⬆️

... and 11 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

nav2_mppi_controller/README.md

SteveMacenski · 2025-10-18T01:28:41Z

nav2_mppi_controller/src/optimizer.cpp

-  state.wz.col(0) = static_cast<float>(state.speed.angular.z);
+  const bool open = settings_.open_loop;
+
+  const float vx0 = open ? control_sequence_.vx(0)


But control_sequence_.vx(0) is not the last command sent to the motors. After we send a command to the motors, the trajectory is slid over to initialize for the next iteration

navigation2/nav2_mppi_controller/src/optimizer.cpp

Lines 273 to 285 in 47d13ce

void Optimizer::shiftControlSequence()

{

auto size = control_sequence_.vx.size();

utils::shiftColumnsByOnePlace(control_sequence_.vx, -1);

utils::shiftColumnsByOnePlace(control_sequence_.wz, -1);

control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2);

control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2);

if (isHolonomic()) {

utils::shiftColumnsByOnePlace(control_sequence_.vy, -1);

control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2);

}

}

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

nav2_mppi_controller/src/optimizer.cpp

SteveMacenski · 2025-10-21T18:37:17Z

How as this tested and would it be useful to setup a new unit test for the optimizer that sets the open loop to true and checks that it initializes exactly the first value? That would be good I think and cover the missing lines

chanhhoang99 · 2025-10-22T03:23:30Z

How as this tested and would it be useful to setup a new unit test for the optimizer that sets the open loop to true and checks that it initializes exactly the first value? That would be good I think and cover the missing lines

Do you mean https://github.com/ros-navigation/navigation2/pull/5617/files#diff-6d2fd58ab98a8266e2217a3d534624786349d5f40e08547ec5125ff64ecae5a5R238 ?
How should I do to cover below lines ?

last_command_vel_ = geometry_msgs::msg::Twist();
// and 
last_command_vel_ = control.twist;

is it should be a test case where

EXPECT_FLOAT_EQ(opt.last_command_vel_.linear.x, 0.0f);

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

SteveMacenski

How should I do to cover below lines ?

Like so 😉 Just check that they are populated correctly.

Or, make a unit test where you set the open loop parameter to true in a node before constructing the MPPI optimizer. Then send it some info with an odometry velocity twist of something ridiculous. Check that the trajectory is reasonable for the first and second time the velocity is requested (first for initialization conditions; second for normal conditions)

SteveMacenski · 2025-10-22T18:36:22Z

nav2_mppi_controller/src/optimizer.cpp

+  if (settings_.open_loop) {
+    last_command_vel_ = control.twist;
+  }


Suggested change

if (settings_.open_loop) {

last_command_vel_ = control.twist;

}

last_command_vel_ = control.twist;

SteveMacenski · 2025-10-22T18:36:30Z

nav2_mppi_controller/src/optimizer.cpp

+  if (settings_.open_loop) {
+    last_command_vel_ = geometry_msgs::msg::Twist();
+  }


Suggested change

if (settings_.open_loop) {

last_command_vel_ = geometry_msgs::msg::Twist();

}

last_command_vel_ = geometry_msgs::msg::Twist();

SteveMacenski

Should you also update state_.speed = robot_speed;? Or, actually set the state_.speed to be the last command. Then the lines you changed for

 state.vx.col(0) = settings_.open_loop ? last_command_vel_.linear.x :
    static_cast<float>(state.speed.linear.x);

Can be reverted. That way anything that may use state (such as the critics) are all using the same consistent thing. You'd then move hte open_loop setting check for what to set in the prepare() method. Right now, the state object is inconsistent in the information its storing, so I think this is actually a better idea.

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

chanhhoang99 · 2025-10-23T09:29:27Z

Hi @adivardi ,
Could you test this latest change on main branch ? Currently, I do not have the set up on main branch to test.(mine is kilted)
Thanks alot.

adivardi · 2025-10-23T09:31:13Z

Hi @adivardi , Could you test this latest change on main branch ? Currently, I do not have the set up on main branch to test.(mine is kilted) Thanks alot.

Hi, sure, but I am traveling for ROSCon. So it will only be in 2 weeks.

SteveMacenski · 2025-10-23T16:31:01Z

@chanhhoang99 can you verify this work well for you on Kilted? The main branch compiles with Kilted 😉 If so, then there's no reason the middleware would change behavior and we can merge before I leave tomorrow for Singapore for ROSCon

P.S. If either or both of you are at ROSCon, come find me. I have some merch for Nav2 contributors 😉

chanhhoang99 · 2025-10-24T02:42:16Z

@SteveMacenski ,
For kilted, I was testing this all day yesterday but didn't find any problem.
This included testing in a narrow space requiring rotating in place, free space and dense place with tables, chairs and people.

add open loop option for MPPI

284fd95

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

SteveMacenski reviewed Oct 16, 2025

View reviewed changes

Change parameter description.

47d13ce

Fix linting Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

eegeugur mentioned this pull request Oct 18, 2025

Preserve gaussian proposals and enforce accel limits at command boundary #5615

Open

8 tasks

SteveMacenski reviewed Oct 20, 2025

View reviewed changes

Storing last command velocity

4f124d3

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

chanhhoang99 force-pushed the master branch from 8e2f036 to 4f124d3 Compare October 21, 2025 07:04

chanhhoang99 added 5 commits October 21, 2025 14:12

trailing white space removal

e9094cc

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

code style fix

5dfaa0b

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

fix code style divergence

e15847d

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

update unit test open_loop option for optimizer

b95acf3

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

fix ament uncrusify

f368a56

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

SteveMacenski requested changes Oct 21, 2025

View reviewed changes

chanhhoang99 added 4 commits October 22, 2025 13:13

refactor

ca66895

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

fix build error

44aab33

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

add unit test for initial last_command_vel value

b176731

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

code coverage fix

7540843

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

SteveMacenski reviewed Oct 22, 2025

View reviewed changes

chanhhoang99 added 5 commits October 23, 2025 11:04

move open loop option to prepare() function

5193717

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

Refactor unit test for optimizer mppi open loop option

7309867

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

fix unit test build fail

22182c5

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

linitng

124b6e2

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

fix build error

2a8b10f

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

adivardi mentioned this pull request Oct 23, 2025

Use last command to initialize state - fix oscillations & nervous steering enwaytech/navigation2#4

Open

SteveMacenski approved these changes Oct 23, 2025

View reviewed changes

SteveMacenski merged commit 8dbc929 into ros-navigation:main Oct 24, 2025
16 checks passed

	void Optimizer::shiftControlSequence()
	{
	auto size = control_sequence_.vx.size();
	utils::shiftColumnsByOnePlace(control_sequence_.vx, -1);
	utils::shiftColumnsByOnePlace(control_sequence_.wz, -1);
	control_sequence_.vx(size - 1) = control_sequence_.vx(size - 2);
	control_sequence_.wz(size - 1) = control_sequence_.wz(size - 2);

	if (isHolonomic()) {
	utils::shiftColumnsByOnePlace(control_sequence_.vy, -1);
	control_sequence_.vy(size - 1) = control_sequence_.vy(size - 2);
	}
	}

Uh oh!

add open loop option for MPPI #5617

add open loop option for MPPI #5617

Conversation

chanhhoang99 commented Oct 16, 2025

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

chanhhoang99 Oct 17, 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!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

chanhhoang99 Oct 21, 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!

Uh oh!

Uh oh!

codecov bot commented Oct 17, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Oct 21, 2025

Uh oh!

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

Uh oh!

Uh oh!

SteveMacenski left a comment • 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 left a comment • 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 commented Oct 23, 2025

Uh oh!

adivardi commented Oct 23, 2025

Uh oh!

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

Uh oh!

Uh oh!

chanhhoang99 commented Oct 24, 2025

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

chanhhoang99 Oct 17, 2025 •

edited

Loading

chanhhoang99 Oct 21, 2025 •

edited

Loading

codecov bot commented Oct 17, 2025 •

edited

Loading

chanhhoang99 commented Oct 22, 2025 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski commented Oct 23, 2025 •

edited

Loading