Skip to content

Commit afe7175

Browse files
mini-1235SteveMacenski
mini-1235
and
SteveMacenski
authored
Fix mppi bidirectional settings (#4954)
* Add constraint; fix settings Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add lower and upper bound Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update tests Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
1 parent e5bb6cc commit afe7175

File tree

7 files changed

+50
-11
lines changed

7 files changed

+50
-11
lines changed

‎nav2_bringup/params/nav2_params.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -115,6 +115,7 @@ controller_server:
115115
ax_max: 3.0
116116
ax_min: -3.0
117117
ay_max: 3.0
118+
ay_min: -3.0
118119
az_max: 3.5
119120
vx_std: 0.2
120121
vy_std: 0.2

‎nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ struct ControlConstraints
3030
float wz;
3131
float ax_max;
3232
float ax_min;
33+
float ay_min;
3334
float ay_max;
3435
float az_max;
3536
};

‎nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,8 @@ namespace mppi::models
2727
*/
2828
struct OptimizerSettings
2929
{
30-
models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
31-
models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
30+
models::ControlConstraints base_constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
31+
models::ControlConstraints constraints{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
3232
models::SamplingStd sampling_std{0.0f, 0.0f, 0.0f};
3333
float model_dt{0.0f};
3434
float temperature{0.0f};

‎nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp‎

Lines changed: 19 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -75,14 +75,22 @@ class MotionModel
7575
float max_delta_vx = model_dt_ * control_constraints_.ax_max;
7676
float min_delta_vx = model_dt_ * control_constraints_.ax_min;
7777
float max_delta_vy = model_dt_ * control_constraints_.ay_max;
78+
float min_delta_vy = model_dt_ * control_constraints_.ay_min;
7879
float max_delta_wz = model_dt_ * control_constraints_.az_max;
7980

8081
unsigned int n_cols = state.vx.cols();
8182

8283
for (unsigned int i = 1; i < n_cols; i++) {
84+
auto lower_bound_vx = (state.vx.col(i - 1) >
85+
0).select(state.vx.col(i - 1) + min_delta_vx,
86+
state.vx.col(i - 1) - max_delta_vx);
87+
auto upper_bound_vx = (state.vx.col(i - 1) >
88+
0).select(state.vx.col(i - 1) + max_delta_vx,
89+
state.vx.col(i - 1) - min_delta_vx);
90+
8391
state.cvx.col(i - 1) = state.cvx.col(i - 1)
84-
.cwiseMax(state.vx.col(i - 1) + min_delta_vx)
85-
.cwiseMin(state.vx.col(i - 1) + max_delta_vx);
92+
.cwiseMax(lower_bound_vx)
93+
.cwiseMin(upper_bound_vx);
8694
state.vx.col(i) = state.cvx.col(i - 1);
8795

8896
state.cwz.col(i - 1) = state.cwz.col(i - 1)
@@ -91,9 +99,15 @@ class MotionModel
9199
state.wz.col(i) = state.cwz.col(i - 1);
92100

93101
if (is_holo) {
102+
auto lower_bound_vy = (state.vy.col(i - 1) >
103+
0).select(state.vy.col(i - 1) + min_delta_vy,
104+
state.vy.col(i - 1) - max_delta_vy);
105+
auto upper_bound_vy = (state.vy.col(i - 1) >
106+
0).select(state.vy.col(i - 1) + max_delta_vy,
107+
state.vy.col(i - 1) - min_delta_vy);
94108
state.cvy.col(i - 1) = state.cvy.col(i - 1)
95-
.cwiseMax(state.vy.col(i - 1) - max_delta_vy)
96-
.cwiseMin(state.vy.col(i - 1) + max_delta_vy);
109+
.cwiseMax(lower_bound_vy)
110+
.cwiseMin(upper_bound_vy);
97111
state.vy.col(i) = state.cvy.col(i - 1);
98112
}
99113
}
@@ -114,7 +128,7 @@ class MotionModel
114128
protected:
115129
float model_dt_{0.0};
116130
models::ControlConstraints control_constraints_{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
117-
0.0f};
131+
0.0f, 0.0f};
118132
};
119133

120134
/**

‎nav2_mppi_controller/src/optimizer.cpp‎

Lines changed: 20 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,7 @@ void Optimizer::getParams()
7575
getParam(s.base_constraints.ax_max, "ax_max", 3.0f);
7676
getParam(s.base_constraints.ax_min, "ax_min", -3.0f);
7777
getParam(s.base_constraints.ay_max, "ay_max", 3.0f);
78+
getParam(s.base_constraints.ay_min, "ay_min", -3.0f);
7879
getParam(s.base_constraints.az_max, "az_max", 3.5f);
7980
getParam(s.sampling_std.vx, "vx_std", 0.2f);
8081
getParam(s.sampling_std.vy, "vy_std", 0.2f);
@@ -89,6 +90,14 @@ void Optimizer::getParams()
8990
"Sign of the parameter ax_min is incorrect, consider setting it negative.");
9091
}
9192

93+
if (s.base_constraints.ay_min > 0.0) {
94+
s.base_constraints.ay_min = -1.0 * s.base_constraints.ay_min;
95+
RCLCPP_WARN(
96+
logger_,
97+
"Sign of the parameter ay_min is incorrect, consider setting it negative.");
98+
}
99+
100+
92101
getParam(motion_model_name, "motion_model", std::string("DiffDrive"));
93102

94103
s.constraints = s.base_constraints;
@@ -245,6 +254,7 @@ void Optimizer::applyControlSequenceConstraints()
245254
float max_delta_vx = s.model_dt * s.constraints.ax_max;
246255
float min_delta_vx = s.model_dt * s.constraints.ax_min;
247256
float max_delta_vy = s.model_dt * s.constraints.ay_max;
257+
float min_delta_vy = s.model_dt * s.constraints.ay_min;
248258
float max_delta_wz = s.model_dt * s.constraints.az_max;
249259
float vx_last = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, control_sequence_.vx(0));
250260
float wz_last = utils::clamp(-s.constraints.wz, s.constraints.wz, control_sequence_.wz(0));
@@ -259,7 +269,11 @@ void Optimizer::applyControlSequenceConstraints()
259269
for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) {
260270
float & vx_curr = control_sequence_.vx(i);
261271
vx_curr = utils::clamp(s.constraints.vx_min, s.constraints.vx_max, vx_curr);
262-
vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr);
272+
if(vx_last > 0) {
273+
vx_curr = utils::clamp(vx_last + min_delta_vx, vx_last + max_delta_vx, vx_curr);
274+
} else {
275+
vx_curr = utils::clamp(vx_last - max_delta_vx, vx_last - min_delta_vx, vx_curr);
276+
}
263277
vx_last = vx_curr;
264278

265279
float & wz_curr = control_sequence_.wz(i);
@@ -270,7 +284,11 @@ void Optimizer::applyControlSequenceConstraints()
270284
if (isHolonomic()) {
271285
float & vy_curr = control_sequence_.vy(i);
272286
vy_curr = utils::clamp(-s.constraints.vy, s.constraints.vy, vy_curr);
273-
vy_curr = utils::clamp(vy_last - max_delta_vy, vy_last + max_delta_vy, vy_curr);
287+
if(vy_last > 0) {
288+
vy_curr = utils::clamp(vy_last + min_delta_vy, vy_last + max_delta_vy, vy_curr);
289+
} else {
290+
vy_curr = utils::clamp(vy_last - max_delta_vy, vy_last - min_delta_vy, vy_curr);
291+
}
274292
vy_last = vy_curr;
275293
}
276294
}

‎nav2_mppi_controller/test/optimizer_unit_tests.cpp‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -177,13 +177,14 @@ class OptimizerTester : public Optimizer
177177
float max_delta_vx = settings_.constraints.ax_max * settings_.model_dt;
178178
float min_delta_vx = settings_.constraints.ax_min * settings_.model_dt;
179179
float max_delta_vy = settings_.constraints.ay_max * settings_.model_dt;
180+
float min_delta_vy = settings_.constraints.ay_min * settings_.model_dt;
180181
float max_delta_wz = settings_.constraints.az_max * settings_.model_dt;
181182
propagateStateVelocitiesFromInitials(state);
182183
EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6);
183184
EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6);
184185
EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6);
185186
EXPECT_NEAR(state.vx(0, 1), std::clamp(0.75, 5.0 + min_delta_vx, 5.0 + max_delta_vx), 1e-6);
186-
EXPECT_NEAR(state.vy(0, 1), std::clamp(0.5, 1.0 - max_delta_vy, 1.0 + max_delta_vy), 1e-6);
187+
EXPECT_NEAR(state.vy(0, 1), std::clamp(0.5, 1.0 + min_delta_vy, 1.0 + max_delta_vy), 1e-6);
187188
EXPECT_NEAR(state.wz(0, 1), std::clamp(0.1, 6.0 - max_delta_wz, 6.0 + max_delta_wz), 1e-6);
188189

189190
// Putting them together: updateStateVelocities
@@ -225,16 +226,18 @@ TEST(OptimizerTests, BasicInitializedFunctions)
225226
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
226227
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
227228
node->declare_parameter("mppic.ax_min", rclcpp::ParameterValue(3.0));
229+
node->declare_parameter("mppic.ay_min", rclcpp::ParameterValue(3.0));
228230
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
229231
"dummy_costmap", "", true);
230232
ParametersHandler param_handler(node);
231233
rclcpp_lifecycle::State lstate;
232234
costmap_ros->on_configure(lstate);
233235
optimizer_tester.initialize(node, "mppic", costmap_ros, &param_handler);
234236

235-
// Test value of ax_min, it should be negative
237+
// Test value of ax_min, ay_min it should be negative
236238
auto & constraints = optimizer_tester.getControlConstraints();
237239
EXPECT_EQ(constraints.ax_min, -3.0);
240+
EXPECT_EQ(constraints.ay_min, -3.0);
238241

239242
// Should be empty of size batches x time steps
240243
// and tests getting set params: time_steps, batch_size, controller_frequency
@@ -530,6 +533,7 @@ TEST(OptimizerTests, updateStateVelocitiesTests)
530533
node->declare_parameter("mppic.ax_max", rclcpp::ParameterValue(3.0));
531534
node->declare_parameter("mppic.ax_min", rclcpp::ParameterValue(-3.0));
532535
node->declare_parameter("mppic.ay_max", rclcpp::ParameterValue(3.0));
536+
node->declare_parameter("mppic.ay_min", rclcpp::ParameterValue(-3.0));
533537
node->declare_parameter("mppic.az_max", rclcpp::ParameterValue(3.5));
534538
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
535539
"dummy_costmap", "", true);

‎nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@ controller_server:
6161
ax_max: 3.0
6262
ax_min: -3.0
6363
ay_max: 3.0
64+
ay_min: -3.0
6465
az_max: 3.5
6566
vx_std: 0.2
6667
vy_std: 0.2

0 commit comments

Comments
 (0)