Skip to content

Commit 47d13ce

Browse files
committed
Change parameter description.
Fix linting Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>
1 parent 284fd95 commit 47d13ce

File tree

3 files changed

+8
-9
lines changed

3 files changed

+8
-9
lines changed

nav2_mppi_controller/README.md

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,7 @@ This process is then repeated a number of times and returns a converged solution
6161
| publish_optimal_trajectory | bool | Publishes the full optimal trajectory sequence each control iteration for downstream control systems, collision checkers, etc to have context beyond the next timestep. |
6262
| publish_critics_stats | bool | Default false. Whether to publish statistics about each critic's performance. When enabled, publishes a `nav2_msgs::msg::CriticsStats` message containing critic names, whether they changed costs, and the sum of costs added by each critic. Useful for debugging and tuning critic behavior. |
6363

64-
| open_loop | bool | Default false. 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.
65-
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.
64+
| open_loop | bool | Default false. Useful when using low accelerations and when wheel odometry's latency causes issues in initial state estimation. |
6665

6766
#### Trajectory Visualizer
6867
| Parameter | Type | Definition |

nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ struct OptimizerSettings
3838
unsigned int iteration_count{0u};
3939
bool shift_control_sequence{false};
4040
size_t retry_attempt_limit{0};
41-
bool open_loop;
41+
bool open_loop{false};
4242
};
4343

4444
} // namespace mppi::models

nav2_mppi_controller/src/optimizer.cpp

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -352,17 +352,17 @@ void Optimizer::updateInitialStateVelocities(models::State & state) const
352352
{
353353
const bool open = settings_.open_loop;
354354

355-
const float vx0 = open ? control_sequence_.vx(0)
356-
: static_cast<float>(state.speed.linear.x);
357-
const float wz0 = open ? control_sequence_.wz(0)
358-
: static_cast<float>(state.speed.angular.z);
355+
const float vx0 = open ? control_sequence_.vx(0) :
356+
static_cast<float>(state.speed.linear.x);
357+
const float wz0 = open ? control_sequence_.wz(0) :
358+
static_cast<float>(state.speed.angular.z);
359359

360360
state.vx.col(0) = vx0;
361361
state.wz.col(0) = wz0;
362362

363363
if (isHolonomic()) {
364-
const float vy0 = open ? control_sequence_.vy(0)
365-
: static_cast<float>(state.speed.linear.y);
364+
const float vy0 = open ? control_sequence_.vy(0) :
365+
static_cast<float>(state.speed.linear.y);
366366
state.vy.col(0) = vy0;
367367
}
368368
}

0 commit comments

Comments
 (0)