Skip to content

Velocity smoother true curvature #5604

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: main
Choose a base branch
from
+285 −32
Open

Velocity smoother true curvature #5604

Changes from 3 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
c805c78
make velocity smoother keep curvature
Sep 22, 2025
c77cfd2
Cleanup and log levels
ipa-dmo Oct 13, 2025
ccd288a
Removed debugging output, used epsilon instead of zero check
ipa-dmo Oct 14, 2025
c23a8f0
Updated comments and return positive 1 in eta calculation
Oct 19, 2025
f1aa293
Added tests for curvature scaling
Oct 19, 2025
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
16 changes: 12 additions & 4 deletions nav2_velocity_smoother/src/velocity_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -274,12 +274,16 @@ double VelocitySmoother::findEtaConstraint(
v_component_min = decel / smoothing_frequency_;
}

if (std::abs(v_cmd) < 1e-6) {
return -1.0;
}
Comment on lines 277 to 281
Copy link
Member

Choose a reason for hiding this comment

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

How should we smooth while slowing then if v_cmd cannot be zero without a divide by zero error for the new implementation? Slowing to a stop seems like a pretty key element of this work

Copy link
Member

Choose a reason for hiding this comment

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

Is there an option that would retain the shape as you see it that allows for the (pretty important?) feature of being able to slow to a stop using the smoother?

Copy link
Author

Choose a reason for hiding this comment

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

This return does no change the slowdown on a stop, it just does not update the eta, that is used the lower the other factors (yaw velocity e.g.) to keep the curvature. So this should be fine imo.

Copy link
Member

@SteveMacenski SteveMacenski Oct 21, 2025 •
edited
Loading

Choose a reason for hiding this comment

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

If dv passes the max / min constraints because we're going at full speed and suddenly a zero-velocity command comes in, we simply skip etas to scale down by the limits. That seems to bypass the eta scaling when zero-velocity commands are issued since all axes will all have v_cmd = 0.0. Then each axis would be limited by its maximum deceleration limits and warp the velocity out of shape

Slowing down to a stop in the case of this "estop" is a key feature of this server


if (dv > v_component_max) {
return v_component_max / dv;
return ( v_component_max + v_curr ) / v_cmd;
}

if (dv < v_component_min) {
return v_component_min / dv;
return ( v_component_min + v_curr ) / v_cmd;
}

return -1.0;
Expand All @@ -289,7 +293,8 @@ double VelocitySmoother::applyConstraints(
const double v_curr, const double v_cmd,
const double accel, const double decel, const double eta)
{
double dv = v_cmd - v_curr;
double v_scaled = eta * v_cmd;
double v_diff = v_scaled - v_curr;

double v_component_max;
double v_component_min;
Expand All @@ -305,7 +310,10 @@ double VelocitySmoother::applyConstraints(
v_component_min = decel / smoothing_frequency_;
}

return v_curr + std::clamp(eta * dv, v_component_min, v_component_max);
auto v_diff_clamped = std::clamp(v_diff, v_component_min, v_component_max);
auto v_cmd_restricted = v_curr + v_diff_clamped;

return v_cmd_restricted;
}

void VelocitySmoother::smootherTimer()
Expand Down
Loading