[Question] MPPI in narrow corridor about navigation2 HOT 5 CLOSED

Neat!

I can’t comment on your particular configurations without seeing them, but I expect that with appropriate tuning this could be made to work. The out of the box parameters of any system need to be thoughtfully adjusted for a particular application and settings of related systems like cost maps. Highly constrained navigation is a unique(ish) case that the default parameters were not specifically tuned for.

The path angle critic is something I’d look at carefully. I’ve definitely been able to tune the robot with no backward motion and rotate in place to the heading of a path. Though, not in that confined of space. In concept though, it should be the similar.

The rotation shim controller certainly would be the easy way out 😉 I think it not necessary or your only option though.

from navigation2.

Any updates?

from navigation2.

Yes. But I couldn't make it work properly, actually.

I have tried playing with path angle critic, but it didn't change the behavior much.
Maybe the problem is that, at the start, MPPI is not sampling those pure spin turn trajectories in the first place? So it does not matter if we increase the path align cost?
Is it possible to add those spin in-place to the initial trajectories?

I have tried to change the noise_generation to spread more the control signal when speed < 1e-3 (kind of trying to do exploration vs exploitation depending on the robot speed magnitude). And that helped with open space, but not narrow corridors.

Thanks you for the help.

The parameters I am using:

MPPIController:
  visualize: false
  vx_max: 1.2
  vy_max: 0.0
  vx_min: 0.0
  vy_min: 0.0
  wz_max: 2.0
  vx_std: 0.5
  vy_std: 0.0
  wz_std: 0.6
  motion_model: 0 # Differential drive
  controller_frequency: 15

  # Optimization
  model_dt: 0.05
  time_steps: 56
  batch_size: 4000
  iteration_count: 1
  temperature: 0.3
  gamma: 0.015
  retry_attempt_limit: 1
  regenerate_noises: false

  # Trajectory
  prune_distance: 0.9 
  transform_tolerance: 0.1
  enforce_path_inversion: false

  critics:
    - {name: "obstacle", type: "mppi_critics/ObstaclesCritic"}
    - {name: "goal", type: "mppi_critics/GoalCritic"}
    - {name: "goal_angle", type: "mppi_critics/GoalAngleCritic"}
    - {name: "path_align", type: "mppi_critics/PathAlignCritic"}
    - {name: "path_angle", type: "mppi_critics/PathAngleCritic"}
    - {name: "path_follow", type: "mppi_critics/PathFollowCritic"}
    - {name: "twirling", type: "mppi_critics/TwirlingCritic"}
    - {name: "constraint", type: "mppi_critics/ConstraintCritic"}


  constraint:
      enabled: true
      cost_power: 1.0
      cost_weight: 4.0

  goal_angle:
      enabled: true
      cost_power: 1.0
      cost_weight: 3.0
      threshold_to_consider: 0.5

  goal:
      enabled: true
      cost_power: 1.0
      cost_weight: 5.0
      threshold_to_consider: 0.9

  obstacle:
      enabled: true
      cost_power: 1.0
      cost_weight: 20.0
      consider_footprint: true
      repulsion_weight: 2.0
      collision_cost: 1000
      collision_margin_distance: 0.03
      near_goal_distance: 0.1
      inflation_layer_name: "inflation"

  path_align:
      enabled: true
      cost_power: 1.0
      cost_weight: 10.0
      threshold_to_consider: 0.5
      offset_from_furthest: 20
      trajectory_point_step: 4
      max_path_occupancy_ratio: 0.07
      use_path_orientations: false

  path_angle:
      enabled: true
      cost_power: 1.0
      cost_weight: 2.0
      threshold_to_consider: 0.5
      offset_from_furthest: 4
      max_angle_to_furthest: 1.2
      mode: 0 # Forward

  path_follow:
      enabled: true
      cost_power: 1.0
      cost_weight: 10.0
      offset_from_furthest: 6
      threshold_to_consider: 0.1

  twirling:
      enabled: true
      cost_power: 1.0
      cost_weight: 10.0
      threshold_to_consider: 0.5

from navigation2.

controller_frequency: 15

That's a big red flag, I mention pretty clearly in the docs that 20hz is a bare minimum, but still not great. 30hz is the minimum I recommend. That's also not set in the right place, so I don't think that even is working.

  critics:
    - {name: "obstacle", type: "mppi_critics/ObstaclesCritic"}
    - {name: "goal", type: "mppi_critics/GoalCritic"}
    - {name: "goal_angle", type: "mppi_critics/GoalAngleCritic"}
    - {name: "path_align", type: "mppi_critics/PathAlignCritic"}
    - {name: "path_angle", type: "mppi_critics/PathAngleCritic"}
    - {name: "path_follow", type: "mppi_critics/PathFollowCritic"}
    - {name: "twirling", type: "mppi_critics/TwirlingCritic"}
    - {name: "constraint", type: "mppi_critics/ConstraintCritic"}

I don't know if setting it like this works fine, I suspect it does, but I'd verify carefully that the parameters below this block are even being used since this is set unusually. I think its probably fine, but it worth a manual check since that could explain why you're not seeing behavioral changes if the parameters you set aren't even being read. This is not how the demo config files I use are formatted and the mppi_critics prefix makes me suspicious that this isn't even working given those are not the names or namespaces of our critics.

Overall, you have some odd things set in odd places, so I'd check that any of this is actually working or being used, including your controller paramater namespace altogether, since its not under the Nav2 Controller namespace or setting of plugins. The MPPI defaults work pretty well, so if this entire param file isn't being used, you might not just see terrible behavior.

However, I don't have the cycles to tune your controller (as you imagine, I am flooded by dozens of such requests and I'd never actually get anything other than tuning other people's robots done). I kind of doubt you need a new critic, but its possible that it could help in this situation. You mentioned tuning your path_angle, but it still looks like 2.0 which doesn't seem like you've actually worked on it all that much. I also don't understand the twirling critic since you don't have an omnidirectional robot, if you look at the documentation. You may benefit from reviewing the documentation and performing some tuning. If you still need help at that point, you can reach out to Open Navigation at [email protected] for commercial support.

from navigation2.

That's a big red flag, I mention pretty clearly in the docs that 20hz is a bare minimum, but still not great. 30hz is the minimum I recommend. That's also not set in the right place, so I don't think that even is working.

I am aware of it. However I have tried 20Hz and 30Hz but I did not notice a huge difference in behavior.
The issue continues. Of course, I have tried with the vanilla parameters, but no success.

  critics:
    - {name: "obstacle", type: "mppi_critics/ObstaclesCritic"}
    - {name: "goal", type: "mppi_critics/GoalCritic"}
    - {name: "goal_angle", type: "mppi_critics/GoalAngleCritic"}
    - {name: "path_align", type: "mppi_critics/PathAlignCritic"}
    - {name: "path_angle", type: "mppi_critics/PathAngleCritic"}
    - {name: "path_follow", type: "mppi_critics/PathFollowCritic"}
    - {name: "twirling", type: "mppi_critics/TwirlingCritic"}
    - {name: "constraint", type: "mppi_critics/ConstraintCritic"}

I don't know if setting it like this works fine, ...

Overall, you have some odd things set in odd places, ...

Sorry for the confusion. The parameter format is ROS 1, because I was playing on porting MPPI. But I copied them to the ROS 2 workspace in the correct format and the behavior I got was the same.

However, I don't have the cycles to tune your controller ...

Sorry, I am not asking you to tune. I was just wondering if others or you had the same problem, or if it is something related to my setup.
My idea was to share the behavior I am having and to get some feedback from people that also tried that.
Note that I am currently using DWA / DWB and it is working very well (without ShimController).

You mentioned tuning your path_angle, but it still looks like 2.0 which doesn't seem like you've actually worked on it all that much. ...

That is a snapshot of the original parameters before tuning. The parameters are dynamic, so I have changed them at runtime. Sorry if it was not clear that I have tried to tune it, but I have changed the cost from 2 to 100.

Finally, I think we can close the question.
It was answered, since you have managed to make it work by just tuning.

from navigation2.