Entirely forbid lateral displacement on differential drive base?

edit

I'm trying to use TEB in a diff-drive robot, but the planner sometimes produces paths with lateral displacements:

I have set to weight_kinematics_nh to the maximum value (10000) and it happens less, but still happens sometimes.

Any suggestion? Here my full config:

# Trajectory Configuration Parameters
teb_autosize: true
dt_ref: 0.1                                # default: 0.3; but controller freq / 10 is recommended
dt_hysteresis: 0.01                        # default: 0.1; but dt_ref / 10 is recommended
max_samples: 50                            # default: 500; not dynamically reconfigurable
global_plan_overwrite_orientation: false   # default: true
allow_init_with_backwards_motion: true     # default: false
max_global_plan_lookahead_dist: 0          # default: 3.0; 0 to use costmap size
feasibility_check_no_poses: 50             # default: 4; but this low number prevents TEB to fail when he should
force_reinit_new_goal_dist: 1              # 0 could prevent robot getting stuck oscillating with an impossible plan
                                           # but completely destroys nav; TODO: maybe should change as a recovery!!!

# Robot Configuration Parameters
max_vel_x: 0.8                             # using same values as in ODWA
max_vel_x_backwards: 0.8
max_vel_y: 0.0
max_vel_theta: 1.0                         # ODWA's value (0.6) prevents closed turns, I think because TEB tends to go at full linear speed
acc_lim_x: 0.8
acc_lim_theta: 1.0

min_turning_radius: 0                      # disable car-like robot

footprint_model:
  type: "line"                             # Toru 5.x modeled as a stadium
  line_start: [-0.18705, 0.0]
  line_end: [0.50557, 0.0]

  # type: "two_circles"                    # alternative model with two circles
  front_offset: 0.50557
  front_radius: 0.4                        # set min_obstacle_dist as 0.0
  rear_offset: 0.18705
  rear_radius: 0.4

# Goal Tolerance Parameters
xy_goal_tolerance: 0.1                     # using same values as in ODWA
yaw_goal_tolerance: 0.05
complete_global_plan: false                # default: true; end the path earlier when robot crosses the end goal
free_goal_vel: false

# Obstacle Parameters
inflation_dist: 1.2                        # same as inflation_layer/inflation_radius
min_obstacle_dist: 0.4                     # half padded footprint's width (Toru 5.x)
include_costmap_obstacles: true
costmap_obstacles_behind_robot_dist: 20.0  # in short +Inf, as we equally navigate backward
obstacle_poses_affected: 100               # TODO: better reduce and change global planner to reduce density!!!   ahhh   no!  depende de dt_ref, creo!
costmap_converter_plugin: "costmap_converter::CostmapToDynamicObstacles"
costmap_converter_spin_thread: true
costmap_converter_rate: 8                  # same as local_costmap/update_frequency

# Optimization Parameters
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: true
optimization_verbose: false
penalty_epsilon: 0.05                      # the default value (0.1) causes oscillation when entering narrow passages, no idea why
weight_max_vel_x: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 10000                # the default value (1000) allows lateral displacements that make robot oscillate
weight_kinematics_forward_drive: 0
weight_kinematics_turning_radius: 0
weight_optimaltime: 1
weight_obstacle: 40
min_obstacle_dist: 0.4
weight_inflation: 0.1
weight_dynamic_obstacle: 10 # not in use yet

# Homotopy Class Planner
enable_homotopy_class_planning: false      # hugely drops performance
enable_multithreading: true
simple_exploration: false
max_number_classes: 4
selection_cost_hysteresis: 1.0
selection_obst_cost_scale: 1.0
selection_alternative_time_cost: false
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

# Miscellaneous Parameters
odom_topic: odom
map_frame: map