Differential drive favor backwards movement teb_local_planner

Question

We have a differential drive robot, and we are using the teb_local_planner with the navigation stack. Overall, it works great, but we'd like to tune the planner so that it puts equal weight on being able to drive backward and forward. For example, one scenario that we see is the robot backing out in a straight line -- ideally, we would like to have the robot only drive backwards, without any change in driving angle. However, we are seeing the robot back up initially, turn around so that it drives forward, then once it reaches its destination, turn around again, effectively giving us a 360 degree rotation.

The parameters that we have found relevant and have set: weight_kinematics_forward_drive: 0 allow_init_with_backwards_motion: true We have also changed weight_kinematics_turning_radius but have not found a good set of parameters for our desired motion.

Is this possible with the teb_local_planner?

Update:

For anybody interested, I set the global planner's orientation_mode to 2 (interpolate), and global_plan_overwrite_orientation to false, and I'm happy with my results.

---

Originally posted by andrewl on ROS Answers with karma: 13 on 2016-12-15

Post score: 1

Answer 1

The current behavior is intended. Let me try to explain:

The local planner follows the global reference plan by tracking the farest available intermediate pose from the global plan within the local costmap (and also not exceeding max_global_plan_lookahead_dist).

Ideally, the global plan already contains reasonable and valid orientations since the global planner is aware of the complete map. Since some global planners do not even provide the orientation part for intermediate poses (NavFcn) or just support forward orientations (default in GlobalPlanner) the teb_local_planner may overwrite orientations by default (global_plan_overwrite_orientation) except the final orientation.

The following strategy is implemented currently for global_plan_overwrite_orientation==true:

Reference goal orientation:

• if global goal (last pose of the global plan): take desired final orientation
• if intermediate goal: the reference orientation of local goals is chosen according to the tangential vectors between pose k and pose k+1 in the global plan (always forward).

Initial Trajectory (TEB) for optimziation

The local trajectory is initialized to the current reference goal (see above, within local costmap). Note, the optimizer just finds local minima, hence the initialization is quite important.

• if allow_init_with_backwards_motion==true and goal is located behind the robot (w.r.t. heading): initialize all orientations backwards for the subsequent optimization.
• Else: initialize with forward orientations.

This strategy has the following implications: If the global goal is close to the robot (within the local costmap window) and is located behind it, the robot drives backwards. Otherwise, the robot always tries to drive forward.

In my opinion, if you would also initialize backwards for goals outside of the local costmap window, you would have to drive backwards along the complete global plan until you reach the global goal. Otherwise, we would need to add more assumptions and heuristics which is not appropriate for a local planner that has just a limited view of the environment. I think, this might be better decided by a global planner in general as mentioned above. Furthermore, most robots have more sensors in their front hence keeping the default strategy to drive forwards for long distances and backwards just for short ways is reasonable in my eyes. But I would be happy to discuss whether better or more intuitive strategies exist.

Note, weight_kinematics_turning_radius defines the penalty weight for satisfying min_turning_radius for carlike robots.

---

Originally posted by croesmann with karma: 2531 on 2016-12-16

This answer was ACCEPTED on the original site

Post score: 7

---

Original comments

Comment by andrewl on 2016-12-16:
Great, that was really helpful! Tried picking a global goal within the local costmap window and behind the robot, and verifying that it drives backwards.

Comment by andrewl on 2016-12-16:
Sounds like our options are to: (1) explore global planners that do not have a bias towards forward motion, or (2) play around with changing the intermediate goals to depend on the robot's current orientation.

Comment by Vimal Raj A on 2021-09-02:
Hey Croesmann, Thanks for that,

But I'm facing this same issue even my localCostmap window is 20x20.My goal is also within the localCostMap window. Also , as you mentioned, Ïf the goal is close to Robot"" , when you mean close, how much ? Are there any default/threshold values for "closeness":, added for optimization? Could you please help on that?

Adding my params, global_plan_overwrite_orientation : true allow_init_with_backwards_motion: true