Mppi: Strange behavior with Controller acceleration

[MateoGarciaSubiran]

Required Info:

• Operating System:
  • Ubuntu 22.04
• Computer:
  • Intel Celeron J6412 2.00Ghz 16GB memory
• ROS2 Version:
  • Humble binaries
  • Nav2 humble_main
• Version or commit hash:
  • navigation2 -> d50b68f
• DDS implementation:
  • default
• Nav2 Package:
  • MPPI
• Have you asked this on Robotics Stack Exchange?
  • No

Tuning / Configuration Goal

Hello, I am trying to configure my real robot so that it can navigate from its position to a specific point.
To do this, I have set the actual speeds and accelerations that my robot can handle.

What I've Tried

Description:
I followed the steps to configure the MPPI according to the notes in the documentation, and in the first test I saw that my robot is able to follow the path and navigate, but it goes past its destination. At first, I thought it was an issue related to the horizon and the aspects mentioned in the user notes, but after conducting more in-depth research, I see that the robot is only able to reach its destination accurately if I set very high linear and angular accelerations that are not the robot's actual ones.

The configuration with the actual limitations of the robot is as follows:

controller_server:
  ros__parameters:
    use_sim_time: false
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.001
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["precise_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]
    speed_limit_topic: "/speed_limit" 
    odom_topic: "/odometry/filtered"
    publish_critics_stats: true
    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.0
      movement_time_allowance: 10.0
    # Goal checker parameters
    precise_goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.4
      yaw_goal_tolerance: 0.2
      stateful: False
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.15
      yaw_goal_tolerance: 0.20
    # MPPI
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 1000
      vx_std: 0.27
      vy_std: 0.0
      wz_std: 0.15
      vx_max: 0.8
      vx_min: -0.8
      vy_max: 0.0
      wz_max: 0.3
      ax_max: 0.7
      ax_min: -0.7
      ay_min: -0.0
      ay_max: 0.0
      az_max: 1.75
      iteration_count: 1
      
      # prune_distance: 4.06
      prune_distance: 2.3
      
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: false
      reset_period: 1.0 # (only in Humble)
      regenerate_noises: false
      publish_optimal_trajectory: true
      enforce_path_inversion: false
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      TrajectoryValidator:
        plugin: "mppi::DefaultOptimalTrajectoryValidator"
        collision_lookahead_time: 2.0
        
        # consider_footprint: false
        consider_footprint: true
        
      AckermannConstraints:
        min_turning_r: 0.2
      
      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
      #critics: ["ConstraintCritic","GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic"]
      
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 2.3
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 2.3
      PreferForwardCritic:
        enabled: false
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 2.3
      ObstaclesCritic:
        enabled: false
        cost_power: 1
        repulsion_weight: 0.6
        critical_weight: 20.0
        consider_footprint: false
        collision_cost: 100000.0
        collision_margin_distance: 0.1
        near_goal_distance: 0.5
        inflation_radius: 2.0 # (only in Humble)
        cost_scaling_factor: 3.0 # (only in Humble)
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
        critical_cost: 300.0
        consider_footprint: false
        collision_cost: 1000000.0
        near_goal_distance: 0.5
        trajectory_point_step: 2
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 4
        threshold_to_consider: 2.3
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 2.3
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 2.3
        max_angle_to_furthest: 1.0
        mode: 1
      # VelocityDeadbandCritic:
      #   enabled: true
      #   cost_power: 1
      #   cost_weight: 35.0
      #   deadband_velocities: [0.05, 0.05, 0.05]
      # TwirlingCritic:
      #   enabled: true
      #   twirling_cost_power: 1
      #   twirling_cost_weight: 10.0
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 4.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      use_sim_time: false
      rolling_window: true
      width: 9
      height: 9
      resolution: 0.03
      robot_radius: 1.15
      #footprint: "[[0.800, 0.800], [-0.800, 0.800], [-0.800, -0.800], [0.800, -0.800]]"
      plugins: ["obstacle_layer", "inflation_layer"]
      # plugins: ["voxel_layer","obstacle_layer", "inflation_layer", "exclusive_layer"]
      filters: ["keepout_filter", "speed_filter"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 2.0
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        footprint_clearing_enabled: true
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 50.0
          raytrace_min_range: 0.0
          obstacle_max_range: 10.0
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True     
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "keepout_costmap_filter_info"
        override_lethal_cost: True
        lethal_override_cost: 200           
      speed_filter:
        plugin: "nav2_costmap_2d::SpeedFilter"
        enabled: False
        filter_info_topic: "speed_costmap_filter_info"
        speed_limit_topic: "speed_limit"

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: false
      robot_radius: 1.15
      resolution: 0.03
      track_unknown_space: false
      plugins: ["static_layer","obstacle_layer","inflation_layer"]
      filters: ["keepout_filter","inflation_layer", "exclusive_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        footprint_clearing_enabled: true
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 50.0
          raytrace_min_range: 0.0
          obstacle_max_range: 10.0
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 2.0
      always_send_full_costmap: True
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: True
        filter_info_topic: "keepout_costmap_filter_info"
        override_lethal_cost: True
        lethal_override_cost: 200
      exclusive_layer:
        plugin: "nav2_costmap_2d::ExclusiveLayer"
        enabled: true
        directional_map_file: "/root/MecaluxFileSystem/Maps/Current/GeneralCostmap/FixedDirectionZones.dat"
        local_layer: true
        map_frame: map
        transform_tolerance: 0.1
        debug_mode: false

If we calculate the accelerations that the robot receives when navigating with this configuration, we see that it never reaches the maximum we are asking for, and this results in the behavior described above.

errorNavigation.mp4

The blue line is barely visible, but it coincides with the red line, which is the speed that is ultimately transferred to the engines.

With this configuration, I would expect to see calculated accelerations of around 0.6 or 0.7 when braking or starting to navigate.

If we change ax_max to 3.0, ax_min to 3.0, and aw_max to 3.5, we see that it achieves higher accelerations, which are what I would expect it to achieve with the “correct/real” configuration.

correcNavigation.mp4

I don't know if I have something configured incorrectly, but I assumed it was a matter of acceleration because with that change it is now able to meet all the objectives we asked for.
Thank you very much.

[SteveMacenski]

What happens if you increase the sampling from 1000x to 2000x? That's relatively few for 20hz and I'm wondering if the space isn't being effectively explored.

      vx_std: 0.27
      vy_std: 0.0
      wz_std: 0.15

These are also somewhat low for your 0.8 m/s target velocity, which would also limit exploration. What happens if you increase these?

[MateoGarciaSubiran]

I won't be able to test these changes today, but when I do, I'll post the results here. Thanks for responding so quickly.

[MateoGarciaSubiran]

This week we ran simulation tests to make sure that no hardware issues were affecting us.
This simulation was done with nav2_minimal_turteblot_simulation (https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git), but we modified the mechanical properties to match our actual robot.

<plugin
        filename="gz-sim-diff-drive-system"
        name="gz::sim::systems::DiffDrive">
        <left_joint>left_wheel_joint</left_joint>
        <right_joint>right_wheel_joint</right_joint>
        <wheel_separation>0.233</wheel_separation>
        <wheel_radius>0.03575</wheel_radius>
        <max_linear_acceleration>0.7</max_linear_acceleration>
        <min_linear_acceleration>-0.7</min_linear_acceleration>
        <max_angular_acceleration>1.75</max_angular_acceleration>
        <min_angular_acceleration>-1.75</min_angular_acceleration>
        <max_linear_velocity>1.45</max_linear_velocity>
        <min_linear_velocity>-1.45</min_linear_velocity>
        <max_angular_velocity>3.18</max_angular_velocity>
        <min_angular_velocity>-3.18</min_angular_velocity>
        <topic>/cmd_vel</topic>
        <odom_topic>/odom</odom_topic>
        <tf_topic>tf</tf_topic>
        <frame_id>odom</frame_id>
        <child_frame_id>base_link</child_frame_id>
        <odom_publish_frequency>30</odom_publish_frequency>
      </plugin>

We have also applied the changes you mentioned, such as increasing the linear speed to 1.45, which is the robot's actual speed, and scaling the deviations as you suggested, and we see that the behavior is the same.
This is the MPPI configuration we are using.

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    costmap_update_timeout: 0.30
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.0 
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath","FollowRoute"]
    use_realtime_priority: false
    speed_limit_topic: "speed_limit"

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: False
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.50
      yaw_goal_tolerance: 0.25
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56 
      model_dt: 0.05
      batch_size: 2000
      ax_max: 0.7
      ax_min: -0.7
      ay_max: 0.0 
      ay_min: -0.0 
      az_max: 1.75
      vx_std: 0.8
      vy_std: 0.0 
      wz_std: 0.8
      vx_max: 1.45
      vx_min: -1.45
      vy_max: 0.0 
      wz_max: 3.18
      iteration_count: 1
      prune_distance: 4.06 
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: true
      publish_optimal_trajectory: true
      regenerate_noises: true
      TrajectoryVisualizer:
        trajectory_step: 15
        time_step: 9
      AckermannConstraints:
        min_turning_r: 0.2
      critics:
        [
          "ConstraintCritic",
          "CostCritic",
          "GoalCritic",
          "GoalAngleCritic",
          "PathAlignCritic",
          "PathFollowCritic",
          "PathAngleCritic",
          "PreferForwardCritic",
        ]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 4.1
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 4.1
      PreferForwardCritic:
        enabled: false
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 4.1
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
 #       near_collision_cost: 253
        critical_cost: 300.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 0.5
        trajectory_point_step: 2
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 4
        threshold_to_consider: 3.0
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 4.1
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 4.1
        max_angle_to_furthest: 1.0
        mode: 1

With this configuration the robot achives the max speed, so I think the calculations for the horizon, prune distance, and thresholds are done correctly.

This is the result of how it behaves in simulation.

VelocidadesRealesSePasa.webm

[MateoGarciaSubiran]

Any updates? @SteveMacenski

[SteveMacenski]

What's with the jumping in the video? Is that acceleration not appropriate - it seems to accelerate slowly which seems at least from my eye to not be far off from 0.7 m/s2 (though I cant know just by looking at rviz; just intuition from looking at alot of rviz feeds)

Also, do you see this issue when you have visualize set to false? That visualization really slows down the process non-trivially and I have noticed before that it can result in "sluggish" behavior, which is why it is said in the documentation to not have that vis on if not doing active debugging.

[MateoGarciaSubiran]

The jumps in the video are a problem when recording, but the robot in the simulator runs more or less smoothly.
In the simulation, I couldn't run the application we have for viewing the acceleration of the motors, but since it behaved exactly the same, I assumed it would be the same problem.
I tried removing Visualize, but I don't see any improvement. I'm going to try to calculate the accelerations used when braking at the target to see if it's still the problem.
When I have it, I'll attach a video with the behavior in simulation after the changes and the acceleration that is being applied.
Thanks!

[rafal-gorecki]

Hello @SteveMacenski,

I also observe strange behavior when changing acceleration parameters, and I don't understand why this caused strange behavior.

1. When I use default nav2_params form jazzy branch, I get quite good behavior, but there is a small oscillation around path.

az_max_eq_3.5.webm

I don't really know what causes these oscillations.

2. When I tried to limit the angular accelerations of the robot and set the parameter to az_max=1.0, I observed a behavior that was completely unclear to me.

az_max_eq_1.0.webm

My first suspicion was some lag in the odometry relative to the controls. However, comparing the data in Plot Juggler, it seems the odometry responds quickly enough to changes, and the odometry publication frequency is fine (20Hz).

I sincerely appreciate your work and commitment in the matter of navigation and would like to ask for your advice/help.

[adivardi]

The issue when you reduce az_max is very similar to what I have. I think that with low acceleration limits the trajectories cloud is "stuck" on one side and doesn't explore enough space.

You can try increasing wz_std, though it caused oscillations for me.
Instead, #5617 really worked for my case.
You can also try #5615, which should help, though it is causing oscillations in the steering for my use case