teb_local_planner: unfeasible trajectories with high dt_ref

asked 2020-10-29 06:54:45 -0600

IvanV gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Do you get either of the same behaviors with the default values of the Trajectory config subset as well (specifically dt_ref and dt_hysteresis)? i.e., 0.3 and 0.1 respectively.

Also, is your wheelbase supposed to be 0?

harshal gravatar image harshal  ( 2020-11-02 05:01:37 -0600 )edit

The unfeasible trajectories start at about a dt_ref of 0.5. At 0.3 it doesn't produce unfeasible ones, but the trajectories oscillate a lot around the global path, as mentioned. Regarding the wheelbase, cmd_angle_instead_rotvel is set to false, so I understand that the wheelbase parameter is ignored. Anyway, on Friday I realized that the teb version I was using was not up to date. I'm testing now with the latest and the behavior seems much improved. Maybe it was just some bug already fixed in newer versions. I will update and close the question if that ends to be the case.

IvanV gravatar image IvanV  ( 2020-11-02 07:31:06 -0600 )edit