Skip to content
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.

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

Added Minimum Translational Velocity Limit #329

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

Conversation

AlperenKosem
Copy link

Most planners have no minimum translational velocity limit and, People who want to add min limit solve this problem in low level controller. "min_vel_x" parameter is added to rqt_reconfiguration and configuration file.

@AlperenKosem
Copy link
Author

for instance :

Copy link
Collaborator

@corot corot left a comment

Choose a reason for hiding this comment

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

nice addition; I guest it can be useful on some robots with picky controllers

can you move the new param together with the other velocities (e.g. after max_vel_x), as on .cfg?
Also, there're a bunch of spurious line breaks and a configuration file out of scope

include/teb_local_planner/teb_config.h Outdated Show resolved Hide resolved
src/teb_config.cpp Outdated Show resolved Hide resolved
cfg/planner.yaml Outdated Show resolved Hide resolved
cfg/TebLocalPlannerReconfigure.cfg Show resolved Hide resolved
@AlperenKosem
Copy link
Author

thanks @corot !

Purious line breaks and a configuration file are deleted, and location of min_vel_x parameter was edited.

cfg/TebLocalPlannerReconfigure.cfg Outdated Show resolved Hide resolved
include/teb_local_planner/teb_local_planner_ros.h Outdated Show resolved Hide resolved
src/teb_config.cpp Outdated Show resolved Hide resolved
src/teb_local_planner_ros.cpp Outdated Show resolved Hide resolved
src/teb_local_planner_ros.cpp Outdated Show resolved Hide resolved
src/teb_local_planner_ros.cpp Outdated Show resolved Hide resolved
src/teb_local_planner_ros.cpp Outdated Show resolved Hide resolved
@AlperenKosem
Copy link
Author

@corot Could you review again?

@@ -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)

@@ -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

Copy link
Collaborator

@corot corot left a comment

Choose a reason for hiding this comment

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

Works only when use_proportional_saturation is false (see my comment below)

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

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.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants