1
$\begingroup$

My setup is:

  • MPPI code ported to ROS Noetic and have a custom differential drive robot.
  • MPPI is running at 50 hz.

I have a global plan from move_base's GlobalPlanenr (Dijkshtra's algorithm). I started with using only one critic that is path align. I have followed the tuning guide from ReadMe.md of nav2. My parameters are:

local_planner:
  time_steps: 56
  model_dt: 0.02
  batch_size: 1000
  vx_std: 0.2
  vy_std: 0.2
  wz_std: 0.4
  vx_max: 2.0
  vx_min: -2.0
  vy_max: 0.0
  wz_max: 0.8
  iteration_count: 1
  prune_distance: 100
  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
  TrajectoryVisualizer:
    trajectory_step: 5
    time_step: 3
  AckermannConstraints:
    min_turning_r: 0.0
  # critics: ["ConstraintCritic", "ObstaclesCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
  critics: ["PathAlignCritic"]
  ConstraintCritic:
    enabled: true
    cost_power: 1
    cost_weight: 4.0
  GoalCritic:
    enabled: true
    cost_power: 1
    cost_weight: 5.0
    threshold_to_consider: 1.4
  GoalAngleCritic:
    enabled: true
    cost_power: 1
    cost_weight: 3.0
    threshold_to_consider: 0.5
  PreferForwardCritic:
    enabled: true
    cost_power: 1
    cost_weight: 5.0
    threshold_to_consider: 0.5
  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.55 # (only in Humble)
    cost_scaling_factor: 10.0 # (only in Humble)
  PathAlignCritic:
    enabled: true
    cost_power: 1
    cost_weight: 10.0
    max_path_occupancy_ratio: 0.05
    trajectory_point_step: 10
    threshold_to_consider: 0.5
    offset_from_furthest: 140
    use_path_orientations: false
  PathFollowCritic:
    enabled: true
    cost_power: 1
    cost_weight: 5.0
    offset_from_furthest: 140
    threshold_to_consider: 1.4
  PathAngleCritic:
    enabled: true
    cost_power: 1
    cost_weight: 2.0
    offset_from_furthest: 55
    threshold_to_consider: 0.5
    max_angle_to_furthest: 1.0
    mode: 0
  # TwirlingCritic:
  #   enabled: true
  #   twirling_cost_power: 1
  #   twirling_cost_weight: 10.0

The green straight line is the global path and the markers are the generated trajectories and optimal trajectory. enter image description here

I can also share a small video for the behavior. Please let me know what would help. Thanks

$\endgroup$
1
  • $\begingroup$ You'll need to isolate variables here a bit. What happens if you enable one critic at a time, or all but one at a time? This may allow you to identify if a critic is being over-critical, and if so, reduce its impact until it's behaving appropriately. $\endgroup$
    – cst0
    Nov 16, 2023 at 14:42

1 Answer 1

1
$\begingroup$

Its not possible to run the controller with just that single critic. Nothing is driving the robot forward (PathFollow), among other things. You need to look critically (he he he...) at the critics to find the minimum viable cross section to do what you want to to. There is no objective functions "built-in", you have to specify each to get each element of the total behavior.

While I understand why what you see happening is happening, its frankly not worth explaining because its conceptually totally irrelevant to what you're trying to do or what is expected of a functioning system. In effect, no critics are currently being evaluated. You won't see this behavior once you add the reasonable cross section of plugins to create the minimum demo you are potentially after. I'll say trying to go 1-by-1 is of dubious value.

See my tuning guide: https://github.com/ros-planning/navigation2/tree/main/nav2_mppi_controller#notes-to-users

$\endgroup$
4
  • $\begingroup$ Thanks for your input. I have also tried using all the critics and I get exactly same behavior. $\endgroup$ Nov 17, 2023 at 10:39
  • $\begingroup$ costs while using all the critic critic name : ConstraintCritic min cost : 0 max cost :0 critic name : ObstaclesCritic min cost : 0 max cost :0 critic name : GoalCritic min cost : 0 max cost :0 critic name : GoalAngleCritic min cost : 0 max cost :0 return 2: 2 critic name : PathAlignCritic min cost : 0 max cost :0 critic name : PathFollowCritic min cost : 0.356347 max cost :1.20029 critic name : PathAngleCritic min cost : 0.356347 max cost :1.20029 critic name : PreferForwardCritic min cost : 0.499139 max cost :1.71577 $\endgroup$ Nov 17, 2023 at 11:00
  • $\begingroup$ I have no idea what these min/max cost parameters are. Where did you get those from? That's not a valid configuration. $\endgroup$ Nov 17, 2023 at 17:17
  • $\begingroup$ I had added logs in the evalTrajectoriesScore to see what each critic was doing. void CriticManager::evalTrajectoriesScores( CriticData & data) const { for (size_t q = 0; q < critics_.size(); q++) { if (data.fail_flag) { break; } critics_[q]->score(data); std::cout << "critic name : " << critic_names_[q] << std::endl; std::cout << "min cost : " << *std::min_element(data.costs.begin(), data.costs.end()) << std::endl; std::cout << "max cost :" << *std::max_element(data.costs.begin(), data.costs.end()) << std::endl; } } $\endgroup$ Nov 19, 2023 at 11:42

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.