diff --git a/configuration/packages/configuring-constrained-smoother.rst b/configuration/packages/configuring-constrained-smoother.rst index 2232065c9..185df1977 100644 --- a/configuration/packages/configuring-constrained-smoother.rst +++ b/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 diff --git a/migration/Kilted.rst b/migration/Kilted.rst index ab5cfc714..5156a9d81 100644 --- a/migration/Kilted.rst +++ b/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 `_ 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 `_ + +Values for the weights will need to be retuned for all users, unfortunately, but will get faster and more reliable results!