MPPI nav2 controller max speed never reached

asked 2023-05-02 09:17:21 -0600

Mrmara
71 ●7 ●11 ●13

updated 2023-05-05 04:03:51 -0600

Hello, I recently started using the new MPPI controller. Everything seems to be working fine except for a couple of strange issues I experienced. The first one is quite blocking for my system but I found a workaround. The second one is less annoying but I could not find a way to solve it.

FIRST ISSUE

With the parameters given in official NAV2 documentation, my robot is not able to properly navigate, in fact, it oscillates around the global path, like a windshield wiper. These oscillations are rough and sudden, and they seem to be uncontrolled. It was sufficient to set PathAlignCritic.offset_from_furthest=200, which practically disable this critic, to solve the problem. The navigation is smooth and everything seems to be working well.

SECOND ISSUE

No matter how many parameters I change, MPPI does not want to command velocities higher than 0.62 m/s. I read the documentation and I tried to tune prune_distance, local_costamp size, mode_dt and time_steps according to suggestions but I have been able to increase it only from ~0.52 m/s to ~0.62 m/s.

BONUS

I encounter also another strange problem when using MPPI, I already asked a questino but unfortunately no answears. I am not sure linking another question is legal, if not sorry for that, I will remove it. QUESTION

I attach to this question my params, If anybody has any suggestion that could help me it would be greatly appreciated

PARAMS

MPPI:
    plugin: "nav2_mppi_controller::MPPIController"
    time_steps: 60
    model_dt: 0.05
    batch_size: 2000
    vx_std: 0.2
    vy_std: 0.2
    wz_std: 0.4
    vx_max: 2.0 #0.5
    vx_min: -0.35
    vy_max: 0.5
    wz_max: 1.9
    iteration_count: 1
    prune_distance: 3.5 #1.7
    transform_tolerance: 0.1
    temperature: 0.3
    gamma: 0.015
    motion_model: "DiffDrive"
    visualize: true
    reset_period: 1.0 # (only in Humble)
    TrajectoryVisualizer:
      trajectory_step: 5
      time_step: 3
    AckermannConstrains:
      min_turning_r: 0.2
    critics: ["ConstraintCritic", "ObstaclesCritic", "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: 1.0
    GoalAngleCritic:
      enabled: true
      cost_power: 1
      cost_weight: 3.0
      threshold_to_consider: 0.4
    PreferForwardCritic:
      enabled: true
      cost_power: 1
      cost_weight: 5.0
      threshold_to_consider: 0.4
    ObstaclesCritic:
      enabled: true
      cost_power: 1
      repulsion_weight: 1.5
      critical_weight: 20.0
      consider_footprint: false
      collision_cost: 10000.0
      collision_margin_distance: 0.1
      near_goal_distance: 0.5
      inflation_radius: 0.4 #0.55 # (only in Humble)
      cost_scaling_factor: 10.0 # (only in Humble)
    PathAlignCritic:
      enabled: true
      cost_power: 1
      cost_weight: 14.0
      max_path_occupancy_ratio: 0.05
      trajectory_point_step: 3
      threshold_to_consider: 0.40
      # reducing this param to a value that make sense makes SEDIA oscillate around path
      offset_from_furthest: 200
    PathFollowCritic:
      enabled: true
      cost_power: 1
      cost_weight: 5.0
      offset_from_furthest: 6
      threshold_to_consider: 0.6
    PathAngleCritic:
      enabled: true
      cost_power: 1
      cost_weight: 2.0
      offset_from_furthest: 4
      threshold_to_consider: 0.40
      max_angle_to_furthest: 1.0
    # TwirlingCritic:
    #   enabled: true
    #   twirling_cost_power: 1
    #   twirling_cost_weight: 10.0

Edit: With this set of params MPPI does not drive faster than 0.52 m/s

plugin: "nav2_mppi_controller::MPPIController"
     time_steps: 60
    model_dt: 0.04
    batch_size: 2000
    vx_std: 0.2
    vy_std: 0.2
    wz_std ...

(more)

edit retag flag offensive close merge delete

Comments

Update:

I have been able to use all critics with acceptable and sensed parameter values. The user experience is still less comfortable than what was achieved with TEB but can be considered acceptable.

The problem I am still struggling with is the maximum reached velocity.

With this set of params, MPPI does not drive faster than 0.52 m/s

Leaving the params in an answer because of comment char limit

Mrmara ( 2023-05-04 05:16:02 -0600 )edit

add a comment

Comments

Thank you for your answer. I was aware of the possible speed limitation that the local costmap size may introduce, but its size is 10x10. I am going to include in the question the full local_costmap parameters list. Increasing the prediction horizon to 5s I have been able to push the max reached velocity to 0.62 m/s but I introduced a strange oscillation behavior around path which is somehow related to PathAlignCritic and ObstaclesCritic

Mrmara ( 2023-05-05 04:00:32 -0600 )edit

Just thinking out loud, I am not even 100% sure I understood what an ABI is. Isn't it possible that like inflation_radius and cost_scaling_factor were breaking ABI, getting the size of the costmap is doing the same? When you say "1.488 which is _suspiciously_ close to 1.5 (where as 0.63 makes it > 1.5)", this makes methink that it could be possible that MPPI is not able to see the actual size of the local costmap and assume a default one, which may be 3x3.

I am not sure this thought makes sense, but I checked the code and I could not find a line of code that explicitly check whether the projectable velocity distance is or not inside the costmap, do you have an int on how to check this? if this makes sense at all...

Mrmara ( 2023-05-05 11:44:44 -0600 )edit

I mean those do need to be properly set, that is important. You have to set that yourself to be consistent with the costmap values https://navigation.ros.org/configurat....

All of this is related: https://github.com/ros-planning/navig.... Note that you have prune_dist set to 3.5 in yoru configurations above, that means only 3.5m of the path is going to be taken to try to align or control in context to. If your costmap is actually much larger and you want to move much faster, that could be easily the cause.

stevemacenski ( 2023-05-05 12:41:29 -0600 )edit

After different trials, I found that slightly increasing the PathFollowCriticcost_weight the maximum reached velocity significantly increases. The best I could achieve with acceptable navigation is 0.82 m/s, unfortunately sitting on the vehicle the user perception is nervous and discontinuous navigation.

Trying to increase the prediction horizon by increasing time_steps and/or increasing model_dt also seems to have beneficial effects on maximum velocity but unfortunately cannot overgo a certain limit (around 3s), in fact over this limit the vehicle start to strongly wobble.

I am continuing to try different combinations of parameters, the goal in to achive maximum speed with comfort navigation.

Mrmara ( 2023-05-09 02:29:32 -0600 )edit

That makes sense to me that increasing the weight of PathFollow would increase the pressure to drive towards the path, that is its role. It may be that you have the PathAlign or other critics set too high so the system is trying to optimize more for aligning precisely with the path (or whatever other critic is set too high, obstacles perhaps) than it is trying to hit the maximum velocity. I should have brought that up to you earlier, but also one of the other etc section comments you see in the README.

I'm going to guess at this point given what you've tried and tested, that's likely your issue. I'd start with PathAlign and Obstacle critics since from my experience, badly tuned versions of those can definitely create the behavior you're describing. The system basically will select to move more slowly so it ...(more)

stevemacenski ( 2023-05-09 11:15:43 -0600 )edit

add a comment

MPPI nav2 controller max speed never reached

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

MPPI nav2 controller max speed never reached edit

Comments

1 Answer

Comments

Question Tools

Stats

Related questions

MPPI nav2 controller max speed never reached