teb_local_planner: unfeasible trajectories with high dt_ref
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 ...
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?
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 thewheelbase
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.Did you fix this issue? I have a similar issue with the
dt_ref
parameter. What I did was to increase theacc_lim_x
,acc_lim_theta
,max_vel_x
, andmax_vel_theta
and the behavior of the robot was betterActually, after updating to a newer version of the package the issue basically disappeared.