Skip to content

Added Minimum Translational Velocity Limit #329

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 5 commits into
base: melodic-devel
Choose a base branch
from
Open

Added Minimum Translational Velocity Limit #329

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
845b65e
added minimum translational velocity constraint
AlperenKosem Nov 23, 2021
dcce602
added cfg file
AlperenKosem Nov 23, 2021
f5e9580
redundancy removed/edited
AlperenKosem Nov 26, 2021
4a852b0
Added minimum translational Velocity Limit
AlperenKosem Nov 29, 2021
4c280f7
Merge branch 'melodic-devel' into melodic-devel
AlperenKosem Dec 3, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions cfg/TebLocalPlannerReconfigure.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,10 @@ grp_robot.add("max_vel_x", double_t, 0,
"Maximum translational velocity of the robot",
0.4, 0.01, 100)

grp_robot.add("min_vel_x", double_t, 0,
"Minimum translational velocity of the robot",
0.00, 0.00, 100)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
0.00, 0.00, 100)
0.0, 0.0, 100)


grp_robot.add("max_vel_x_backwards", double_t, 0,
"Maximum translational velocity of the robot for driving backwards",
0.2, 0.01, 100)
Expand Down
1 change: 1 addition & 0 deletions include/teb_local_planner/teb_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,7 @@ class TebConfig
struct Robot
{
double max_vel_x; //!< Maximum translational velocity of the robot
double min_vel_x; //!< Minimum translational velocity of the robot
double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards
double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
double max_vel_theta; //!< Maximum angular velocity of the robot
Expand Down
3 changes: 2 additions & 1 deletion include/teb_local_planner/teb_local_planner_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -351,11 +351,12 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap
* @param[in,out] vy Strafing velocity which can be nonzero for holonomic robots
* @param[in,out] omega The angular velocity that should be saturated.
* @param max_vel_x Maximum translational velocity for forward driving
* @param min_vel_x Minimum translational velocity for forward driving
* @param max_vel_y Maximum strafing velocity (for holonomic robots)
* @param max_vel_theta Maximum (absolute) angular velocity
* @param max_vel_x_backwards Maximum translational velocity for backwards driving
*/
void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double min_vel_x, double max_vel_y,
double max_vel_theta, double max_vel_x_backwards) const;


Expand Down
3 changes: 2 additions & 1 deletion src/teb_config.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ void TebConfig::loadRosParamFromNodeHandle(const ros::NodeHandle& nh)

// Robot
nh.param("max_vel_x", robot.max_vel_x, robot.max_vel_x);
nh.param("min_vel_x", robot.min_vel_x, robot.min_vel_x);
nh.param("max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards);
nh.param("max_vel_y", robot.max_vel_y, robot.max_vel_y);
nh.param("max_vel_theta", robot.max_vel_theta, robot.max_vel_theta);
Expand Down Expand Up @@ -199,10 +200,10 @@ void TebConfig::reconfigure(TebLocalPlannerReconfigureConfig& cfg)
trajectory.feasibility_check_no_poses = cfg.feasibility_check_no_poses;
trajectory.publish_feedback = cfg.publish_feedback;
trajectory.control_look_ahead_poses = cfg.control_look_ahead_poses;
trajectory.prevent_look_ahead_poses_near_goal = cfg.prevent_look_ahead_poses_near_goal;

// Robot
robot.max_vel_x = cfg.max_vel_x;
robot.min_vel_x = cfg.min_vel_x;
robot.max_vel_x_backwards = cfg.max_vel_x_backwards;
robot.max_vel_y = cfg.max_vel_y;
robot.max_vel_theta = cfg.max_vel_theta;
Expand Down
25 changes: 19 additions & 6 deletions src/teb_local_planner_ros.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -421,7 +421,7 @@ uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseSt

// Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints).
saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z,
cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);
cfg_.robot.max_vel_x, cfg_.robot.min_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);

// convert rot-vel to steering angle if desired (carlike robot).
// The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint
Expand Down Expand Up @@ -869,12 +869,25 @@ double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector<geomet
}


void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards) const
void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards, double min_vel_x) const
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same order everywhere

Suggested change
void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards, double min_vel_x) const
void TebLocalPlannerROS::saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double min_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards) const

{
double ratio_x = 1, ratio_omega = 1, ratio_y = 1;
// Limit translational velocity for forward driving
if (vx > max_vel_x)
ratio_x = max_vel_x / vx;
double ratio_x = 1, ratio_omega = 1, ratio_y = 1, ratio_x_min = 1;

// backward driving
if (vx < 0)
{
if (vx < -max_vel_x_backwards)
ratio_x = -max_vel_x_backwards / vx;
else if (vx > -min_vel_x)
ratio_x = -min_vel_x / vx;
}
else // forward driving
{
if (vx > max_vel_x)
ratio_x = max_vel_x / vx;
else if (vx < min_vel_x)
ratio_x = min_vel_x / vx;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure about this; ratio_x will get a value > 1, but that will be discarded here if using proportional saturation (which is the preferred option, and so we will move below min_vel_x

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I didn't think proportional saturation side. Therefore i thought that there is no problem but you are right. If proportional saturation contains min velocity feature all twist components must speed up. Increased speed can causes unstability. Maybe this section can hard coded as below otherwise i have no solution for proportional saturation for min speed feature.
(I wrote pseudocode for explaining. When moved backwards, vx < min_vel_vx does not valid.)

if (cfg_.robot.use_proportional_saturation)
{
    double ratio = std::min(std::min(ratio_x, ratio_y), ratio_omega);
    vx *= ratio;
    if (vx < min_vel_x)
        vx = min_vel_x
    vy *= ratio;
    omega *= ratio;
}

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, for sure; but I wonder if that may trigger other problems... 🤔
@croesmann , @RainerKuemmerle , would u mind to take a look at this solution and confirm it doesn't break the intended behavior of proportional saturation?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But this will fail with negative x velocities, right?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think with a min velocity that requires other components to speed up for keeping the curvature is problematic, i.e. proportional scaling. There are in general cases where, for example, vy would violate max_vel_y. In particular on an omni directional drive for which it is perfectly fine to have (0, vy, 0) as a target command.
For a differential drive, the min vel param should affect the wheels if the motivation is that below some velocity command, the wheels are not moving. In this case, it might make more sense to reason on the min rotational velocity on the individual wheels directly to minimize the error in the curvature to follow by the robot.

Copy link
Collaborator

@corot corot Jan 5, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good point; I also read the old @croesmann's answer to the same issue and he agrees that a minimum velocity doesn't play well with TEB architecture.
If nothing against, I'll close this PR by now.
@AlperenKosem, as suggested, the min wheels velocity makes more sense at controller level.
But thanks a lot for the contribution; please don't feel discouraged to do more in the future!

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Firstly i constrained velocity at controller level side but in this case, robot could not able to follow the trajectory because wheel velocities did not match the velocities which generated by planner. Therefore i added to min. vel. constraint to planner side. But i understand possible problems, thanks for reviews.

}

// limit strafing velocity
if (vy > max_vel_y || vy < -max_vel_y)
Expand Down