teb_local_planner: unfeasible trajectories with high dt_ref

edit

Hi,

I'm trying to use teb_local_planner in a double Ackerman robot and I'm unable to achieve smooth trayectories.

Playing with the parameters I have found hard to understand some effects of dt_ref.

So, from the description, I understand that dt_ref is each fraction of time new estimated robot poses are calculated. I.e. for a dt_ref of 1, you calculate robot poses and associated twist commands each 1 second in the future.

It also says that dt_ref should be in the order of 1/control frequency. So I understand that if I have a control frequency of 20Hz, it should be around 0.05. Is that right?

Now, if I set in my robot a low dt_ref value, what I get is that the robot moves very slowly, oscillating around the global plan and never achieving anything close to robot's max_vel_x. I guess that this can be caused by too many twist updates with small corrections due the too high temporal resolution and a control point too close.

To start getting smooth movements, I need to increase a lot dt_ref up to 0.6-0.7. With that dt_ref the robot movement is good, reaches nearly max_vel_x, but I'm constantly getting the famous "Unfeasible trajectory. Resetting planner" error. This happens even in empty spaces with literally nothing in 10-15 meters around, so I guess it is not caused by obstacles. Whenever this happens, the planner commands a 0 speed twist, which makes the robot to stop (which is something not very good in a 1200 Kg robot moving at 3 m/s).

I fail to understand why increasing dt_ref causes infeasible trajectories. Any hint?

This is my configuration:

TebLocalPlannerROS:
  odom_topic: odom
  map_frame: map

  # Trajectory

  teb_autosize: true
  dt_ref: 0.7
  dt_hysteresis: 0.07
  global_plan_overwrite_orientation: false
  max_global_plan_lookahead_dist: 5.0
  feasibility_check_no_poses: 5
  allow_init_with_backwards_motion: true

  # Robot

  max_vel_x: 3.0
  max_vel_x_backwards: 0.2
  max_vel_y: 0.0
  max_vel_theta: 2.0
  acc_lim_x: 0.5
  acc_lim_y: 0.0
  acc_lim_theta: 1.0
  min_turning_radius: 3.0
  wheelbase: 0.0
  cmd_angle_instead_rotvel: false

  # Polygon footprint
  footprint_model:
    type: "polygon"
    vertices: [[-1.51, -0.76], [1.51, -0.76], [1.51, 0.76], [-1.51, 0.76]]
  min_obstacle_dist: 0.05
  inflation_dist: 0.75

  # GoalTolerance

  xy_goal_tolerance: 0.3
  yaw_goal_tolerance: 0.3
  free_goal_vel: false

  # Obstacles

  include_costmap_obstacles: true
  costmap_obstacles_behind_robot_dist: 2.0
  obstacle_poses_affected: 30
  legacy_obstacle_association: true

  #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
  #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
  #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
  #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
  costmap_converter_plugin: "" # deactivate plugin
  costmap_converter_spin_thread: true
  costmap_converter_rate: 5
  #costmap_converter/CostmapToLinesDBSRANSAC:
  #  cluster_max_distance: 0.4

  # Optimization

  no_inner_iterations: 5
  no_outer_iterations: 4
  optimization_activate: true
  optimization_verbose: false
  penalty_epsilon: 0.1
  weight_max_vel_x: 2
  weight_max_vel_y: 0
  weight_max_vel_theta: 1
  weight_acc_lim_x: 1
  weight_acc_lim_y: 0
  weight_acc_lim_theta: 1
  weight_kinematics_nh: 1000 # if it is an holonomic robot, must be low
  weight_kinematics_forward_drive: 1
  weight_kinematics_turning_radius: 10
  weight_optimaltime: 1
  weight_obstacle: 10
  weight_dynamic_obstacle: 10 # not in use yet20.0
  selection_alternative_time_cost: false

  # Homotopy Class Planner

  enable_homotopy_class_planning: false
  enable_multithreading: true
  selection_cost_hysteresis: 1.0
  selection_obst_cost_scale: 100.0
  selection_prefer_initial_plan: 0.95
  selection_viapoint_cost_scale: 1.0
  simple_exploration: false
  max_number_classes: 4
  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5
  h_signature_prescaler: 0.5
  h_signature_threshold: 0.1
  obstacle_keypoint_offset: 0.1
  obstacle_heading_threshold: 0.45
  visualize_hc_graph: false

  # Global plan ...