teb_local_planner
teb_local_planner copied to clipboard
[feature request] add penalty for switching forward/reverse (carlike)
Currently, teb_local_planner assumes it is possible to instantly switch from going forward to going backward (and from backward to forward) with no penalty (e.g. with constant acceleration from +1.0 to -1.0) However for carlike robots it is not the case most of the time: because of the mechanical gear change, the robot needs first to stop, handle the gear change then can drive backward.
Because of that, the teb_local_planner may select very impracticable paths for the carlike robot. For instance, if the robot is asked to turn around in place, the planner may find that doing many mini-turns going back and forth in place is a good solution, while a more practical (and way faster) way would be to do some kind of 3-point turn.
For example, this problem could be solved by adding a new parameter "reverse_gear_switch_time", which would be added to the path time travel whenever the robot's velocity would change sign.
Here is a very simple implementation of what could be an EdgeGearChange
:
void computeError()
{
ROS_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeGearChange()");
const VertexPose* pose1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexPose* pose2 = static_cast<const VertexPose*>(_vertices[1]);
const VertexPose* pose3 = static_cast<const VertexPose*>(_vertices[2]);
// ORIENTATION
const Eigen::Vector2d diff1 = pose2->position() - pose1->position();
const Eigen::Vector2d diff2 = pose3->position() - pose2->position();
_error[0] = diff1.dot(diff2) < 0.0 ? cfg_->robot.gear_change_time : 0.0;
}
used in the AddEdgesGearChange
in optimal_planner.cpp
if (cfg_->robot.max_vel_y == 0 || cfg_->robot.acc_lim_y == 0) // non-holonomic robot
{
Eigen::Matrix<double,1,1> information;
information.fill(cfg_->optim.weight_gear_change);
// add the usual acceleration edge for each tuple of three teb poses
for (int i=0; i < n - 2; ++i)
{
EdgeGearChange* gear_change_edge = new EdgeGearChange;
gear_change_edge->setVertex(0,teb_.PoseVertex(i));
gear_change_edge->setVertex(1,teb_.PoseVertex(i+1));
gear_change_edge->setVertex(2,teb_.PoseVertex(i+2));
gear_change_edge->setInformation(information);
gear_change_edge->setTebConfig(*cfg_);
optimizer_->addEdge(gear_change_edge);
}
}
where robot.gear_change_time
represents the time required for the robot to switch between forward and backward (eg. 2 seconds), and weight_gear_change the optimization weight for satisfying the constraint (eg. 100)
I have tested the result with a carlike robot in simulation, asking the planner to move behind and reverse the vehicle orientation.
Without the extra penalty, I often get this kind of "star-shape" turn around plans:
With the extra penality, I am more likely to get clean paths like this:
@VRichardJP Nice work Richard, is there a possibility to share with me the implementation to test it and give some feedback ?
Hi, sorry I don't have much time to make a clean pull request, but the piece of code I showed above is pretty much the whole implementation, so it should not be very difficult to reproduce. I really just followed the existing code design to fit in the new function/variables/parameters
Thank you Vincent, i'll implement and test it and give some feedback !