nav2_constrained_smoother: Ceres cost function corrected

Signed-off-by: videh25 <videhpatel25@gmail.com>

Author: videh25

configuration/packages/configuring-constrained-smoother.rst

@@ -270,8 +270,8 @@ Example
         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_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: 5.0e5               # weight to maximize smoothness of path
+        w_cost: 0.12                  # 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: 5.0e3
           fn_tol: 1.0e-15
           param_tol: 1.0e-20

migration/Kilted.rst

@@ -897,3 +897,12 @@ 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>`_