Velocity smoother true curvature #5604
Conversation
Signed-off-by: Dominik Moss <[email protected]>
Signed-off-by: Dominik Moss <[email protected]>
Signed-off-by: Dominik Moss <[email protected]>
ipa-dmo
force-pushed
the
velocity_smoother_true_curvature
branch
from
9c3885a to
ccd288a
Compare
| if (std::abs(v_cmd) < 1e-6) { | ||
| return -1.0; | ||
| } |
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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
Signed-off-by: DMO <[email protected]>
Signed-off-by: DMO <[email protected]>
|
I added some tests to show the behavior of the scale_velocity feature.
|
Codecov Report✅ All modified and coverable lines are covered by tests.
... and 25 files with indirect coverage changes 🚀 New features to boost your workflow:
|
|
|
||
| return -1.0; | ||
| // no acceleration limit exceeded, so return "no change" | ||
| return 1.0; |
There was a problem hiding this comment.
If there's no change, then -1 would be the right return because if eta < 0 we skip updating curr_eta
2 participants
The scale velocity flag is not maintaining the curvature as i would assume. It says:
The eta factor is currently calculated based on the planned velocity change
dvand the maximum velocity changev_component_max. So not related to the absolute velocity but only the difference of the commanded velocity to the current velocity.Later later the eta factor is used in the
applyConstraintsfunction in the same way:return v_curr + std::clamp(eta * dv, v_component_min, v_component_max);This is fine, as long both are used with the same velocities, but when the eta is calculated from e.g. the turning limitations and used in the x-velocity constraint this results in problems.
E.g. we have a limitation in turning acceleration resulting in following eta and then used in the v_x and omega constraint.
With some example values: omega_component_max = 10, frequency = 20 Hz, v_x_curr = 1.5, v_x_cmd = 2.0, omega_curr = 0.0, omega_cmd = 1.5
This has a commanded radius of r = 2.0/1.5 = 1.33.
eta = omega_component_max / (omega_cmd - omega_curr) = 1/3results for the scaled difference before clamping of the velocity:
v_diff_before_clamping = eta * (v_x_cmd - v_x_curr) = 1/6and the angular velocity:
omega_diff_before_clamping = eta * (omega_cmd - omega_curr) = 1/2If we assume no further clamping, we get the following resulting path radius:
r = ( v_curr + v_diff_before_clamping ) / ( omega_curr + omega_diff_before_clamping ) = 3.33So the robot would accelerate and miss the planned curvature.
This happens because only the difference is scaled by eta not the absolute, resulting values.
With the suggested approach, we redefine eta a bit by (actually bad example for this since omega_curr = 0, so it is the same result here):
eta = (omega_component_max + omega_curr) / omega_cmd = 1/3and use it in the calculation for x velocity:
v_diff_before_clamping = eta * v_x_cmd - v_x_curr = -5/6and the angular velocity:
omega_diff_before_clamping = eta * omega_cmd - omega_curr = 1/2If we assume no further clamping, we get the following resulting path radius:
r = ( v_curr + v_diff_before_clamping ) / ( omega_curr + omega_diff_before_clamping ) = 1.33Tests on our robot showed this improves the accuracy of the curvature drastically on different limit settings. And from my understanding this is the expected behavior based on the parameter description.
Or did i misunderstood the desired behavior?
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Description of how this change was tested
Future work that may be required in bullet points
For Maintainers:
backport-*.