nav2_constrained_smoother: Ceres cost function corrected

configuration/packages/configuring-constrained-smoother.rst

@@ -268,10 +268,10 @@ Example
         keep_start_orientation: true  # whether to prevent the start orientation from being smoothed
         keep_goal_orientation: true   # whether to prevent the gpal orientation from being smoothed
         minimum_turning_radius: 0.40  # minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 with the same effect) for diff-drive/holonomic robots
-        w_curve: 30.0                 # weight to enforce minimum_turning_radius
+        w_curve: 0.5                  # weight to enforce minimum_turning_radius
         w_dist: 0.0                   # weight to bind path to original as optional replacement for cost weight
-        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: 3.0e+3              # weight to maximize smoothness of path
+        w_cost: 0.000045              # weight to steer robot away from collision and cost
 
         # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes)
         w_cost_cusp_multiplier: 3.0   # option to use higher weight during forward/reverse direction change which is often accompanied with dangerous rotations
@@ -284,6 +284,6 @@ Example
         optimizer:
           max_iterations: 70            # max iterations of smoother
           debug_optimizer: false        # print debug info
-          gradient_tol: 5e3
+          gradient_tol: 50.0
           fn_tol: 1.0e-15
           param_tol: 1.0e-20

migration/Kilted.rst

@@ -897,3 +897,14 @@ MPPI per-critic trajectory cost visualization
 When ``visualize`` is enabled, candidate trajectories are now rendered as cost-colored lines using a green-to-yellow-to-red gradient, with collision trajectories shown in magenta.
 A new ``critic_index_to_visualize`` parameter (default ``0``) selects which critic's costs to display: ``0`` shows the total cost across all critics, while ``1..N`` selects an individual critic by index.
 The ``publish_critics_stats`` parameter has been removed; critic statistics (``~/critics_stats`` topic) are now published automatically when ``visualize`` is enabled.
+
+Constrained Smoother cost function formulation corrected
+--------------------------------------------------------
+
+`PR #6000 <https://github.com/ros-navigation/navigation2/pull/6000>`_ changes the cost function formulation in `nav2_constrained_smoother`. Weights for the constrained smoother may need to be adjusted as a result.
+
+Earlier, `nav2_constrained_smoother` was using a cost formulation of :math:`cost = w_1^2 * cost_1^4 + w_2^2 * cost_2^4 + ...` because the internal squaring of residuals performed by `Ceres Solver` was not accounted for. This caused the optimizer to frequently fail to converge.
+
+The internal squaring of `Ceres` is now considered and the cost formulation is corrected to :math:`cost = w_1 * cost_1^2 + w_2 * cost_2^2 + ...`. This makes the constrained smoother approximately 10x faster in testing and results in converged solutions and improved path quality. A detailed analysis of improvement is available in: `Issue #5072 <https://github.com/ros-navigation/navigation2/issues/5072#issuecomment-3992795987>`_
+
+Values for the weights will need to be retuned for all users, unfortunately, but will get faster and more reliable results!