MPPI nav2 controller max speed never reached
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, localcostamp size, `modedtandtime_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: 0.4
vx_max: 1.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: false
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: 0.1
critical_weight: 20.0
consider_footprint: false
collision_cost: 10000.0
collision_margin_distance: 0.1
near_goal_distance: 0.5
inflation_radius: 1.0 # (only in Humble)
cost_scaling_factor: 4.8 #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 #like a time step but in number of points in tra
threshold_to_consider: 0.40
# reducing this param to a value that make sense makes SEDIA oscillate around path
offset_from_furthest: 40 #26 #with formula
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
Local costmap PARAMS
local_costmap:
always_send_full_costmap: True
update_frequency: 25.0
publish_frequency: 25.0
global_frame: map # TODO by default this is odom
robot_base_frame: base_link
rolling_window: True
width: 10
height: 10
resolution: 0.025
plugins: ["obstacle_layer", "keepout_filter", "inflation_keepout"]
#filters: ["speedlimit_filter"]
speedlimit_filter:
plugin: "nav2_costmap_2d::SpeedFilter"
enabled: true
filter_info_topic: "/costmap_speedlimit_filter_info"
speed_limit_topic: "/speed_limit"
keepout_filter:
plugin: "nav2_costmap_2d::KeepoutFilter"
enabled: true
filter_info_topic: "/costmap_keepout_filter_info"
raytrace_filter:
plugin: "sedia_ramps_detector::RaytraceFilter"
enabled: false
filter_info_topic: "/costmap_raytrace_filter_info"
transform_tolerance: 0.1
# Measured footprint
#footprint: '[[-0.30, -0.36], [-0.30, 0.36], [0.75,
# 0.36], [0.75, -0.36]]'
# + 5cm offeset
footprint: '[[-0.35, -0.41], [-0.35, 0.41], [0.80,
0.41], [0.80, -0.41]]'
# + 10 cm offset
#footprint: '[[-0.40, -0.46], [-0.40, 0.46], [0.85,
# 0.46], [0.85, -0.46]]'
lethal_cost_threshold: 100 # new in this file
map_topic: /map # new in this file
inflation_layer:
enabled: True
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 10.0
inflation_radius: 0.4
inflation_keepout:
enabled: True
plugin: "nav2_costmap_2d::InflationLayer"
cost_scaling_factor: 4.8 #10.0
inflation_radius: 1.0
static_layer:
map_subscribe_transient_local: True
obstacle_layer:
combination_method: 1
enabled: true
footprint_clearing_enabled: true
max_obstacle_height: 2.0
plugin: nav2_costmap_2d::ObstacleLayer
#observation_sources: cameramulti laserRearLeft laserRearRight lidarmulti tofSideLeft tofSideRight tofFrontLeft tofFrontRight
observation_sources: cameramulti lidarmulti
cameramulti:
clearing: true
data_type: LaserScan
expected_update_rate: 0.0 # new in this file
inf_is_valid: false # new in this file
marking: true
max_obstacle_height: 2.0
min_obstacle_height: -1.0
observation_persistence: 0.0 # new in this file
obstacle_max_range: 1.8
obstacle_min_range: 0.2
raytrace_max_range: 2.5
raytrace_min_range: 0.0
sensor_frame: base_link
topic: /camera_scan_merged
laserRearLeft:
clearing: true
data_type: LaserScan
expected_update_rate: 0.0 # new in this file # TODO
inf_is_valid: false # new in this file # TODO
marking: true
max_obstacle_height: 2.0
min_obstacle_height: 0.0
observation_persistence: 0.0 # new in this file # TODO
obstacle_max_range: 0.7
obstacle_min_range: 0.05
raytrace_max_range: 0.8
raytrace_min_range: 0.0
sensor_frame: ultrasound_rear_left
topic: /ultrasound_rear_left
laserRearRight:
clearing: true
data_type: LaserScan
expected_update_rate: 0.0 # new in this file # TODO
inf_is_valid: false # new in this file # TODO
marking: true
max_obstacle_height: 2.0
min_obstacle_height: 0.0
observation_persistence: 0.0 # new in this file # TODO
obstacle_max_range: 0.7
obstacle_min_range: 0.05
raytrace_max_range: 0.8
raytrace_min_range: 0.0
sensor_frame: ultrasound_rear_right
topic: /ultrasound_rear_right
lidarmulti:
clearing: true
data_type: LaserScan
expected_update_rate: 0.0 # new in this file # TODO
inf_is_valid: false # new in this file # TODO
marking: true
max_obstacle_height: 2.0
min_obstacle_height: -1.0
observation_persistence: 0.0 # new in this file # TODO
obstacle_max_range: 4.5
obstacle_min_range: 0.05
raytrace_max_range: 5.0
raytrace_min_range: 0.0
sensor_frame: base_link
topic: /lidar_scan_merged
tofSideLeft:
clearing: true
data_type: LaserScan
expected_update_rate: 0.0 # new in this file # TODO
inf_is_valid: false # new in this file # TODO
marking: true
max_obstacle_height: 2.0
min_obstacle_height: 0.0
observation_persistence: 0.0 # new in this file # TODO
obstacle_max_range: 0.7
obstacle_min_range: 0.05
raytrace_max_range: 0.8
raytrace_min_range: 0.0
sensor_frame: tof_side_left
topic: /tof_side_left
tofSideRight:
clearing: true
data_type: LaserScan
expected_update_rate: 0.0 # new in this file # TODO
inf_is_valid: false # new in this file # TODO
marking: true
max_obstacle_height: 2.0
min_obstacle_height: 0.0
observation_persistence: 0.0 # new in this file # TODO
obstacle_max_range: 0.7
obstacle_min_range: 0.05
raytrace_max_range: 0.8
raytrace_min_range: 0.0
sensor_frame: tof_side_right
topic: /tof_side_right
tofFrontLeft:
clearing: true
data_type: LaserScan
expected_update_rate: 0.0 # new in this file # TODO
inf_is_valid: false # new in this file # TODO
marking: true
max_obstacle_height: 2.0
min_obstacle_height: 0.0
observation_persistence: 0.0 # new in this file # TODO
obstacle_max_range: 0.70
obstacle_min_range: 0.05
raytrace_max_range: 0.8
raytrace_min_range: 0.0
sensor_frame: tof_front_left
topic: /tof_front_left
tofFrontRight:
clearing: true
data_type: LaserScan
expected_update_rate: 0.0 # new in this file # TODO
inf_is_valid: false # new in this file # TODO
marking: true
max_obstacle_height: 2.0
min_obstacle_height: 0.0
observation_persistence: 0.0 # new in this file # TODO
obstacle_max_range: 0.70
obstacle_min_range: 0.0
raytrace_max_range: 0.80
raytrace_min_range: 0.0
sensor_frame: tof_front_right
topic: /tof_front_right
origin_x: 0.0 # new in this file
origin_y: 0.0 # new in this file
#robot_radius: 0.5 # new in this file
track_unknown_space: false # new in this file
transform_tolerance: 1.0
#trinary_costmap: true # new in this file
#unknown_cost_value: 255 # new in this file
#use_maximum: false # new in this file
Asked by Mrmara on 2023-05-02 09:17:21 UTC
Answers
I need to see your local costmap settings too. I suspect its due to https://github.com/ros-planning/navigation2/tree/main/nav2_mppi_controller#prediction-horizon-costmap-sizing-and-offsets
If you don't make the costmap large enough to be able to go your full speed with the trajectory inside of the window, it can't achieve you maximum velocity requests.
60 * 0.04 = 2.4 seconds.
2.4 * 0.62 = 1.488 which is suspiciously close to 1.5 (where as 0.63 makes it > 1.5) which would correspond to a 3m costmap window cross section, which is a common default (https://github.com/ros-planning/navigation2/blob/main/nav2_bringup/params/nav2_params.yaml#L190)
Asked by stevemacenski on 2023-05-04 17:34:45 UTC
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
Asked by Mrmara on 2023-05-05 04:00:32 UTC
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...
Asked by Mrmara on 2023-05-05 11:44:44 UTC
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/configuration/packages/configuring-mppic.html#obstacles-critic.
All of this is related: https://github.com/ros-planning/navigation2/blob/main/nav2_mppi_controller/src/path_handler.cpp#L64-L86. 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.
Asked by stevemacenski on 2023-05-05 12:41:29 UTC
After different trials, I found that slightly increasing the PathFollowCritic cost_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.
Asked by Mrmara on 2023-05-09 02:29:32 UTC
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 can finitely get a modest improvement in path tracking or absolute-furthest-from-obstacle behavior instead of going full speed. Pushing those back down would rebalance them
Asked by stevemacenski on 2023-05-09 11:15:43 UTC
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
Asked by Mrmara on 2023-05-04 05:16:02 UTC