Tuning guide for mpc_local_planner

edit

When applying mpc_local_planner on navigation stack, move_base met a problem of "Control loop missed its desired rate".

This controller rate I set is same as when the teb_local_planner is used.

What confused me is that how to increase the controller frequency for mpc_local_planner?

Which parameter should I change?

The parameters for mpc_local_planner are listed below:

base_local_planner: "mpc_local_planner/MpcLocalPlannerROS"
controller_frequency: 20.0  
controller_patience: 3.0

MpcLocalPlannerROS:

  odom_topic: odom_combined

   ## Robot settings
  robot:
    type: "unicycle"
    unicycle:
      max_vel_x: 0.7
      max_vel_x_backwards: 0.7
      max_vel_theta: 0.3
      acc_lim_x: 0.4 # deactive bounds with zero
      dec_lim_x: 0.4 # deactive bounds with zero
      acc_lim_theta: 0.3 # deactivate bounds with zero

    # type: "simple_car"
    # simple_car:
    #   wheelbase: 1.311
    #   front_wheel_driving: True
    #   max_vel_x: 0.7
    #   max_vel_x_backwards: 0.7
    #   max_steering_angle: 1.4
    #   acc_lim_x: 0.4 # deactive bounds with zero
    #   dec_lim_x: 0.4 # deactive bounds with zero
    #   max_steering_rate: 0.3 # deactive bounds with zero

  ## Footprint model for collision avoidance
  # footprint_model:
  #   type: "point"
  #   is_footprint_dynamic: False
  footprint_model: 
    type: "line"
    radius: 0.2 # for type "circular"
    line_start: [0.0, 0.0] # for type "line"
    line_end: [0.4, 0.0] # for type "line"
    front_offset: 0.2 # for type "two_circles"
    front_radius: 0.2 # for type "two_circles"
    rear_offset: 0.2 # for type "two_circles"
    rear_radius: 0.2 # for type "two_circles"
    vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"

  ## Collision avoidance
  collision_avoidance:
    min_obstacle_dist: 0.8 # Note, this parameter must be chosen w.r.t. the footprint_model
    enable_dynamic_obstacles: False
    force_inclusion_dist: 0.5
    cutoff_dist: 2.5 
    include_costmap_obstacles: False
    costmap_obstacles_behind_robot_dist: 1.5

  ## Planning grid 
  grid:
    type: "fd_grid"
    grid_size_ref: 20
    dt_ref: 0.3
    xf_fixed: [True, True, True]
    warm_start: True
    collocation_method: "forward_differences"
    cost_integration_method: "left_sum"
    variable_grid:
      enable: True  
      min_dt: 0.0;
      max_dt: 10.0;
      grid_adaptation:
        enable: True
        dt_hyst_ratio: 0.1
        min_grid_size: 5
        max_grid_size: 50

  ## Planning options
  planning:
    objective:
      type: "minimum_time" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
      quadratic_form:
        state_weights: [2.0, 2.0, 2.0]
        control_weights: [1.0, 1.0]
        integral_form: False
      minimum_time_via_points:
        position_weight: 5.0
        orientation_weight: 3.0
        via_points_ordered: False
    terminal_cost:
      type: "none" # can be "none"
      quadratic:
        final_state_weights: [2.0, 2.0, 2.0]
    terminal_constraint:
      type: "none" # can be "none"
      l2_ball:
        weight_matrix: [1.0, 1.0, 1.0]
        radius: 5

  ## Controller options
  controller:
    outer_ocp_iterations: 5
    xy_goal_tolerance: 0.03
    yaw_goal_tolerance: 0.1
    global_plan_overwrite_orientation: False
    global_plan_prune_distance: 1.0
    allow_init_with_backward_motion: True
    max_global_plan_lookahead_dist: 1.5 
    global_plan_viapoint_sep: 0.1
    force_reinit_new_goal_dist: 1.0
    force_reinit_new_goal_angular: 1.57
    force_reinit_num_steps: 0
    prefer_x_feedback: False
    publish_ocp_results: False

  ## Solver settings
  solver:
    type: "ipopt"
    ipopt:
      iterations: 25
      max_cpu_time: -1.0
      ipopt_numeric_options:
        tol: 1e-4
      ipopt_string_options:
        linear_solver: "mumps" 
        hessian_approximation: "limited-memory" # exact or limited-memory
    lsq_lm:
      iterations: 10
      weight_init_eq: 2
      weight_init_ineq: 2
      weight_init_bounds: 2
      weight_adapt_factor_eq: 1.5
      weight_adapt_factor_ineq: 1.5
      weight_adapt_factor_bounds: 1.5
      weight_adapt_max_eq: 500
      weight_adapt_max_ineq: 500
      weight_adapt_max_bounds: 500

Any suggestion will be greatly appreciated, thanks.