Removed squaring from constrained smoother cost function shape by videh25 · Pull Request #6000 · ros-navigation/navigation2

videh25 · 2026-03-04T06:37:42Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5072
Primary OS tested on	Ubuntu
Robotic platform tested on	Loopback Sim
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

Description of contribution in a few bullet points

Removed squaring of residuals in ceres cost fucntions of nav2_constrained_smoother.
Tuned the new nav2_constraind_smoother.

Description of documentation updates required from your changes

nav2_constrained_smoother's configure guide will have to be updated with the new parameters.

Description of how this change was tested

Copied the current nav2_constrained_smoother as nav2_constrained_smoother_old so that both the improvements can be compared side by side with the older smoother via benchmarking script.
Made changes to the nav2_constrained_smoother to remove the squaring and added temporary changes to publish the ceres summary data over a topic for becnchmarking.
Made changes to the smoother benchmarking scripts to be able to incorporate the ceres summaries in results.
Copied and modified maps for testing from different packages.
Ran the smoother benchmarking scripts to measure the performance of both smoothers across different maps and with different planners. Tested for 200 trajectories over different maps and planners.
More detailed results posted on Constrained smoother cost function shape #5072.

Future work that may be required in bullet points

NA

For Maintainers:

Check that any new parameters added are updated in docs.nav2.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
Should this be backported to current distributions? If so, tag with backport-*.

Signed-off-by: videh25 <[email protected]>

SteveMacenski

A couple of questions and a request for doc updates! LGTM

SteveMacenski · 2026-03-04T17:08:59Z

-      w_smooth: 2000000.0           # weight to maximize smoothness of path
-      w_cost: 0.015                 # weight to steer robot away from collision and cost
+      w_smooth: 5.0e5           # weight to maximize smoothness of path
+      w_cost: 0.12                 # weight to steer robot away from collision and cost


Please add to the migration guide at docs.nav2.org that this improvement has been had which increases path quality + convergence time. Also update the parameter defaults to be more in line with what folks should use in the configuration guide for the constrained smoother :-)

SteveMacenski · 2026-03-04T17:12:19Z

    }

-    r += (T)weight * ki_minus_kmax * ki_minus_kmax;  // objective function value
+    r += (T)std::sqrt(weight) * ki_minus_kmax;  // objective function value


If we're changing the formulation for the defaults to need adjustment, could the defaults themselves just account on being squared so we don't perform these sqrt operations over the weights? I.e. retune the defaults to be sqrt of it's updated value? That would remove some relatively heavy operations

The cost check points may also be the same?

I too agree with removing the repeated square root functions.
Just listing down ways and concerns in doing so:

Square root the weights in configuration and remove sqrt from the code.

For anyone considering the mathematical formulation of cost function, it is natural to think of the weights mentioned in the config to be the ones directly being multiplied to individual squared costs, since that is how the formulae are generally mentioned in papers and academia.

Weights being squared internally by ceres will be misleading.

This can work if we also update the documentation to explicitly mention the mathematical formulation used in the smoother to mention that the yaml weights are squared by ceres.

Keep the yaml values and sqrt them when the parameter is fetched/updated

Values mentioned in the config will be the same as the ones multiplied to the squared cost terms, despite of ceres squaring them internally.

Might have to rename the class variables to something like weight_sqrted since they are not the exact same values as in yaml?

Please let me know what approach sounds best!

I think the second option probably makes the most sense

I agree with the second one :-)

mini-1235

Also tests are failing after the changes: https://app.circleci.com/pipelines/github/ros-navigation/navigation2/18155/workflows/b2ce8ae9-f4b4-4915-927a-e6f465eb63d3/jobs/54622/tests

Signed-off-by: videh25 <[email protected]>

SteveMacenski

LGTM!

Only 3 asks:

There are a number of tests that fail now, can you take a look and address? This includes DCO (which is easy if you click on the job it tells you what to run)
Can you update the migration guide in docs.nav2.org to mention this fix + updated parameters required?
Does it make sense to update the defaults in the code / example yaml in the configuration guide for the constrained smoother as well to be what you've found works well now?

videh25 · 2026-03-16T07:55:20Z

There are a number of tests that fail now, can you take a look and address? This includes DCO (which is easy if you click on the job it tells you what to run)

Still working on the tests. Need to retune the parameters used in the tests for some of them.
A bunch of them are failing due to improvements in the performance, where smoother is improving the path more than the tests expected. Example: testingAnchoringToOriginalPath in here. For such test, should I update the values expected by the tests? Or should I update the tests to a greater than comparision rather than the current expect near?

Can you update the migration guide in docs.nav2.org to mention this fix + updated parameters required?

Yes, I have those changes ready offline. Will create a PR!

Does it make sense to update the defaults in the code / example yaml in the configuration guide for the constrained smoother as well to be what you've found works well now?

Yes, still finalising the parameters. Will get a final set of parameters once I fix all the tests.

mini-1235 · 2026-03-16T09:51:20Z

I would suggest keeping the EXPECT_NEAR and updating the expected value. At the same time, please sanity check the new value to make sure it actually makes sense to you

SteveMacenski · 2026-03-16T16:00:18Z

Agreed with @mini-1235

…vable: 483, 1044 (Could not nudge the smoothness improvement by increasing w_smooth Signed-off-by: videh25 <[email protected]>

Signed-off-by: videh25 <[email protected]>

SteveMacenski · 2026-03-20T17:34:26Z

Let me know when you want me to review again :-)

Signed-off-by: videh25 <[email protected]>

videh25 · 2026-03-21T13:13:59Z

Hey @SteveMacenski @mini-1235!

I am almost done with tuning the tests. Just want to run some specific cases through you guys.

Here is a google drive folder containing the visuals of how each smoother call in tests improved the path previously vs now.

During retuning the parameters, I tried to ensure that the qualitative nature of improvements remain the same for each smoother call. However, here are a few notable deviations:

SmootherTest.testingSmoothness
- Location: test at test_constrained_smoother.cpp: 481
- This is currently the only failing test.
- The new orientation_smoothness_improvement is lower than the previous expectation.
- Old Expected = 28.5 ± 1.0
- New Improvement = 24.13
- Increasing w_smooth further does not show any significant increase the metric, suggesting that this is not an issue of suboptimal tuning. :-/
- Need guidance on what to do with this test. Would it be reasonable to change the expected value, given that the qualitative nature of improvement remains the same?
SmootherTest.testingStartGoalOrientations
- Location: tests at test_constrained_smoother.cpp: 1059, 1061, 1062
- These tests showed a significantly higher improvement in the metric for new smoother.
- Old Expected = 58.9 ± 1.0
- New Improvement = 99.9
- Even a small value of w_smooth results in a similar improvement (tried tuning it down, does not work).
- Considering the parameters for this smoother call, it makes sense to observe the new behaviour. (Only w_smooth and w_curve are non-zero --> Path straightens out prioritizing only smoothness).
- Start and goal orientations also change to point along the line connecting them. Thus, both having an orientation of PI/4 rather than the previously hard coded PI/8 and 7*PI/8.
- Have updated the tests to expect the new path.
SmootherTest.testingObstacleAvoidanceNearCusps
- New improvements are in general more aggressive compared to the previous improvements.
- Please take a look at the results of smoother calls in this folder.

SteveMacenski · 2026-03-23T15:58:54Z

Would it be reasonable to change the expected value, given that the qualitative nature of improvement remains the same?

Yes, this is a different formulation so I would expect some variation. They both look good though 👍

Considering the parameters for this smoother call, it makes sense to observe the new behaviour. (Only w_smooth and w_curve are non-zero --> Path straightens out prioritizing only smoothness).

Does this test not try to maintain the start/goal orientations? If so, then this makes sense. Technically a straight line is the lowest-curvature option as long as the boundary conditions aren't being respected to force a curve.

Please take a look at the results of smoother calls in this folder.

A number of these look rather odd, having pretty extreme moves (like #4) that don't seem justified / well traded off for smoothness / length. But I suppose the originals also look a little extreme. Can we tone that down a bit? The behavior in #2 looks pretty reasonable.

… some tests Signed-off-by: videh25 <[email protected]>

Signed-off-by: videh25 <[email protected]>

videh25 · 2026-03-29T18:16:09Z

Does this test not try to maintain the start/goal orientations? If so, then this makes sense. Technically a straight line is the lowest-curvature option as long as the boundary conditions aren't being respected to force a curve.

No, that test is specifically for the case where start and goal locations are not maintained.

A number of these look rather odd, having pretty extreme moves (like #4) that don't seem justified / well traded off for smoothness / length. But I suppose the originals also look a little extreme. Can we tone that down a bit? The behavior in #2 looks pretty reasonable.

I have tuned the parameters in testingObstacleAvoidanceNearCusps to generate smoother paths. Please find the new images in the same folder.

However, a test is failing now. Here is the context for that:

Step1: Smooth with baseline params.
Step2: Smooth with lower w_cost and higher w_cost_cusp_multiplier (less penalty on normal poses, more on cusps)
The test expects origin_similarity_improvement > 0, assuming Step2 stays closer to the original path due to lower w_cost.
The test expects origin_similarity_improvement to be positive. It assumes the path form Step2 will be closer to original path due to smaller w_cost.
In practice, increasing w_cost_cusp_multiplier shifts cusps farther from obstacles. Since current paths are prioritizing smoothness more, the deviation in Step2 path is more than the deviation in Step1, so the metric becomes negative.
Need advice on what to do about these tests.

Also, testingObstacleAvoidanceNearCusps contains multiple inter-dependent EXPECT statements which is makes the parameter tuning for the test brittle. Changes that satisfy one assertion often break another. I’ve tried multiple parameter combinations to avoid the current failure, but they consistently violate at least one of the other expectations. This is the best trade-off scenario I was able to achieve among all those tuning trials.

SteveMacenski · 2026-03-30T21:04:58Z

I have tuned the parameters in testingObstacleAvoidanceNearCusps to generate smoother paths. Please find the new images in the same folder.

Looks great to me!

For the tests: I'd say delete any that don't pass your sniff test and create some new ones that are better / less fragile. The cusp movement feature was added after the fact and may have been too prescriptive based on the behavior at the time rather than good general-purpose tests (it appears)

Signed-off-by: videh25 <[email protected]>

videh25 · 2026-04-03T19:39:11Z

Here is a summary of changes made in tests in testingObstacleAvoidanceNearCusps for your reference:

Introduced a cusp_cost_criterion, which considers solely the cost at the cusp pose.
Replaced all worst-cost comparisions with cusp-cost comparisions, since cusp cost exhibits more predictable behaviour, whereas worst-cost pose varies with path geometry.
Removed origin similarity test due to it being sensitive to other parameters and can change with tuning.
Removed a repeated checks .(previously at 830-844 in the older version, already covered earlier in the test).

codecov · 2026-04-03T19:49:26Z

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
...ther/include/nav2_constrained_smoother/options.hpp	`100.00% <100.00%> (ø)`
...her/include/nav2_constrained_smoother/smoother.hpp	`95.93% <100.00%> (ø)`
...v2_constrained_smoother/smoother_cost_function.hpp	`100.00% <100.00%> (ø)`

... and 9 files with indirect coverage changes

🚀 New features to boost your workflow:

❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

videh25 · 2026-04-03T19:57:22Z

Opened a corresponding PR in nav2_docs here.

…avigation#6000) * Removed squaring from constrained smoother cost function shape Signed-off-by: videh25 <[email protected]> * Moved sqrt to param loading Signed-off-by: videh25 <[email protected]> * Retuned parameters Signed-off-by: videh25 <[email protected]> * Updated the retuned parameters in tests Signed-off-by: videh25 <[email protected]> * Paritally fixed test- fixed all solvable performance decreases. Unsolvable: 483, 1044 (Could not nudge the smoothness improvement by increasing w_smooth Signed-off-by: videh25 <[email protected]> * Tuned ObstacleAvoidanceNearCusps to only have slight improvements Signed-off-by: videh25 <[email protected]> * Tuned ObstacleAvoidanceNearCusps to only have slight improvements Signed-off-by: videh25 <[email protected]> * Nudged improvement tests to expect better improvements Signed-off-by: videh25 <[email protected]> * Tuned ObstacleAvoidanceNearCusps to only have smoother paths -failing some tests Signed-off-by: videh25 <[email protected]> * Updated README Signed-off-by: videh25 <[email protected]> * Modified testingObstacleAvoidanceNearCusps tests to be less fragile Signed-off-by: videh25 <[email protected]> --------- Signed-off-by: videh25 <[email protected]>

Removed squaring from constrained smoother cost function shape

dafa77d

Signed-off-by: videh25 <[email protected]>

videh25 force-pushed the constrained_smoother_cost_fn_correction branch from 3ebf1c1 to dafa77d Compare March 4, 2026 06:49

SteveMacenski requested changes Mar 4, 2026

View reviewed changes

mini-1235 reviewed Mar 8, 2026

View reviewed changes

videh25 added 3 commits March 15, 2026 08:36

Moved sqrt to param loading

f23dfa2

Signed-off-by: videh25 <[email protected]>

Retuned parameters

3b64e35

Signed-off-by: videh25 <[email protected]>

Updated the retuned parameters in tests

b7ecbdc

Signed-off-by: videh25 <[email protected]>

SteveMacenski approved these changes Mar 15, 2026

View reviewed changes

videh25 added 2 commits March 17, 2026 12:17

Paritally fixed test- fixed all solvable performance decreases. Unsol…

15b2642

…vable: 483, 1044 (Could not nudge the smoothness improvement by increasing w_smooth Signed-off-by: videh25 <[email protected]>

Tuned ObstacleAvoidanceNearCusps to only have slight improvements

b833e20

Signed-off-by: videh25 <[email protected]>

videh25 force-pushed the constrained_smoother_cost_fn_correction branch from e0f8695 to b833e20 Compare March 20, 2026 13:42

videh25 added 2 commits March 21, 2026 09:47

Tuned ObstacleAvoidanceNearCusps to only have slight improvements

12d1d52

Signed-off-by: videh25 <[email protected]>

Nudged improvement tests to expect better improvements

824f21e

Signed-off-by: videh25 <[email protected]>

videh25 added 2 commits March 29, 2026 17:22

Tuned ObstacleAvoidanceNearCusps to only have smoother paths -failing…

1a46e4c

… some tests Signed-off-by: videh25 <[email protected]>

Updated README

9f2a7b2

Signed-off-by: videh25 <[email protected]>

Modified testingObstacleAvoidanceNearCusps tests to be less fragile

21022d5

Signed-off-by: videh25 <[email protected]>

videh25 mentioned this pull request Apr 3, 2026

nav2_constrained_smoother: Ceres cost function corrected ros-navigation/docs.nav2.org#895

Merged

SteveMacenski approved these changes Apr 3, 2026

View reviewed changes

SteveMacenski merged commit f88cd02 into ros-navigation:main Apr 3, 2026
18 checks passed

Conversation

videh25 commented Mar 4, 2026

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:

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Mar 4, 2026

Choose a reason for hiding this comment

Uh oh!

Uh oh!

SteveMacenski Mar 4, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

videh25 Mar 7, 2026

Choose a reason for hiding this comment

Uh oh!

mini-1235 Mar 8, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Mar 9, 2026

Choose a reason for hiding this comment

Uh oh!

mini-1235 left a comment

Choose a reason for hiding this comment

Uh oh!

SteveMacenski left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

videh25 commented Mar 16, 2026

Uh oh!

mini-1235 commented Mar 16, 2026

Uh oh!

SteveMacenski commented Mar 16, 2026

Uh oh!

SteveMacenski commented Mar 20, 2026

Uh oh!

videh25 commented Mar 21, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski commented Mar 23, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

videh25 commented Mar 29, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski commented Mar 30, 2026

Uh oh!

videh25 commented Apr 3, 2026

Uh oh!

codecov Bot commented Apr 3, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

videh25 commented Apr 3, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

SteveMacenski Mar 4, 2026 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

videh25 commented Mar 21, 2026 •

edited

Loading

SteveMacenski commented Mar 23, 2026 •

edited

Loading

videh25 commented Mar 29, 2026 •

edited

Loading

codecov Bot commented Apr 3, 2026 •

edited

Loading