Skip to content

Implementation of model_delay with dynamic parameter #6066

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
BriceRenaudeau wants to merge 1 commit into
base: main
Choose a base branch
from
+27 −3
Open

Implementation of model_delay with dynamic parameter #6066

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ struct OptimizerSettings
models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
models::SamplingStd sampling_std{0.0f, 0.0f, 0.0f};
float model_dt{0.0f};
float model_delay{0.0f};
float temperature{0.0f};
float gamma{0.0f};
unsigned int batch_size{0u};
Expand Down
24 changes: 23 additions & 1 deletion nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,10 +60,12 @@ class MotionModel
* @param control_constraints Constraints on control
* @param model_dt duration of a time step
*/
void initialize(const models::ControlConstraints & control_constraints, float model_dt)
void initialize(const models::ControlConstraints & control_constraints, float model_dt,
float model_delay)
{
control_constraints_ = control_constraints;
model_dt_ = model_dt;
model_delay_ = model_delay;
}

/**
Expand Down Expand Up @@ -116,6 +118,25 @@ class MotionModel
state.vy.col(i) = state.cvy.col(i - 1);
}
}

// Apply model delay
if(model_delay_ == 0.0) {return;}
Copy link
Copy Markdown
Member

@SteveMacenski SteveMacenski Apr 7, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would it make sense if less than model_dt_ to skip? Or I suppose by the current math < 0.5 * model_dt_ would be the same since the offset would be floored to 0 in which case.

But more architecturally, if hte offset is less than one cycle's DT, does it make sense to apply any lag?

unsigned int offset = std::floor((model_delay_ / model_dt_) + 0.5);
auto state_copy = state;
for (unsigned int i = 0; i != state.vx.shape(0); i++) {
for (unsigned int j = 1; j != state.vx.shape(1); j++) {
Copy link
Copy Markdown
Member

@SteveMacenski SteveMacenski Apr 7, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'll probably take care of this for you but leaving a note for myself: These loops may be eigenized for vectorized operations.

// Keep the first value before delay (because we cannot do better)
Copy link
Copy Markdown
Member

@SteveMacenski SteveMacenski Apr 7, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// Keep the first value before delay (because we cannot do better)
// Keep the first values before delay

Making plural in case large.

if (j < offset) {
state.vx(i, j) = state_copy.vx(i, 0);
state.wz(i, j) = state_copy.wz(i, 0);
if (is_holo) {state.vy(i, j) = state_copy.vy(i, 0);}
} else { // move the command to express delay
state.vx(i, j) = state_copy.vx(i, j - offset);
state.wz(i, j) = state_copy.wz(i, j - offset);
if (is_holo) {state.vy(i, j) = state_copy.vy(i, j - offset);}
}
}
}
}

/**
Expand All @@ -132,6 +153,7 @@ class MotionModel

protected:
float model_dt_{0.0};
float model_delay_{0.0};
models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
0.0f, 0.0f};
};
Expand Down
5 changes: 3 additions & 2 deletions nav2_mppi_controller/src/optimizer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,7 @@ void Optimizer::getParams()
}

getParam(s.model_dt, "model_dt", 0.05f);
getParam(s.model_delay, "model_delay", 0.0f);
getParam(s.time_steps, "time_steps", 56);
getParam(s.batch_size, "batch_size", 1000);
getParam(s.iteration_count, "iteration_count", 1);
Expand Down Expand Up @@ -192,7 +193,7 @@ void Optimizer::reset(bool reset_dynamic_speed_limits)
generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);

noise_generator_.reset(settings_, isHolonomic());
motion_model_->initialize(settings_.constraints, settings_.model_dt);
motion_model_->initialize(settings_.constraints, settings_.model_dt, settings_.model_delay);
trajectory_validator_->initialize(
parent_, name_ + ".TrajectoryValidator",
costmap_ros_, parameters_handler_, tf_buffer_, settings_);
Expand Down Expand Up @@ -590,7 +591,7 @@ void Optimizer::setMotionModel(const std::string & model)
"Model " + model + " is not valid! Valid options are DiffDrive, Omni, "
"or Ackermann"));
}
motion_model_->initialize(settings_.constraints, settings_.model_dt);
motion_model_->initialize(settings_.constraints, settings_.model_dt, settings_.model_delay);
}

void Optimizer::setSpeedLimit(double speed_limit, bool percentage)
Expand Down
Loading