SPFASmoothing.cpp

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
 
#include "SPFASmoothing.h"
 
#include <array>
#include <cmath>
#include <iomanip>
#include <limits>
#include <string>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/util/StringHelpers.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/spfa/smoothing/Costmap2DWrapper.h>
#include <armarx/navigation/algorithms/spfa/smoothing/io.h>
#include <armarx/navigation/algorithms/spfa/smoothing/residuals.h>
#include <armarx/navigation/algorithms/spfa/smoothing/PathGeometry.h>
#include <armarx/navigation/algorithms/spfa/smoothing/TrajectoryChecker2D.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Trajectory.h>
 
namespace armarx::navigation::algorithms::spfa::smoothing
{
    namespace
    {
        using Params = io::SmoothingParams;
 
        core::GlobalTrajectory toGlobalTrajectory(const std::vector<CenterPoint>& traj)
        {
            core::GlobalTrajectory gtraj;
            for (const auto& p : traj)
            {
                core::GlobalTrajectoryPoint gp;
                gp.waypoint.pose =
                    Eigen::Translation3f{Eigen::Vector3f(static_cast<float>(p.x),
                                                         static_cast<float>(p.y),
                                                         0.0f)} *
                    Eigen::AngleAxisf{static_cast<float>(p.theta), Eigen::Vector3f::UnitZ()};
                gp.velocity = static_cast<float>(p.v);
                gtraj.mutablePoints().push_back(gp);
            }
            return gtraj;
        }
 
        std::vector<CenterPoint> toCenterTrajectory(const core::GlobalTrajectory& gtraj)
        {
            std::vector<CenterPoint> traj;
            for (const auto& gp : gtraj.points())
            {
                CenterPoint p;
                p.x = gp.waypoint.pose.translation().x();
                p.y = gp.waypoint.pose.translation().y();
                const auto& R = gp.waypoint.pose.rotation();
                p.theta = std::atan2(R(1, 0), R(0, 0));
                p.v = gp.velocity;
                traj.push_back(p);
            }
            return traj;
        }
 
        double queryDistance(const Costmap2DWrapper& wrapper, double x, double y)
        {
            double out;
            wrapper(&x, &y, &out);
            return out;
        }
 
        /**
         * One-shot escape hatch for waypoints the optimizer cannot move.
         *
         * The costmap is an *unsigned* distance field: everything at or inside the
         * robot-radius-inflated obstacle is clamped to zero and masked invalid, which the
         * wrapper reports as a constant sentinel. A waypoint sitting there sees a flat
         * field, so no gradient method -- Ceres or otherwise -- can push it out. Only a
         * discrete jump can.
         *
         * Waypoints that merely violate the clearance are deliberately left alone: the
         * obstacle barrier has a real gradient there and moves them with the smoothness and
         * spacing terms in the loop, which a per-waypoint projection cannot do. This ran as
         * a 100-iteration hill climb before, and that was the source of the folds -- each
         * waypoint was free to travel up to `tracking_max_deviation` independently of its
         * neighbours, which is most of a segment length on a 200 mm-spaced path, so two
         * adjacent waypoints could swap order outright.
         *
         * @return Indices that could not be projected within the deviation bound.
         */
        std::vector<std::size_t> projectTrappedWaypoints(std::vector<CenterPoint>& traj,
                                                         const Costmap2DWrapper& wrapper,
                                                         const algorithms::Costmap& costmap,
                                                         const Params& params)
        {
            std::vector<std::size_t> unresolved;
 
            for (std::size_t idx = 1; idx + 1 < traj.size(); idx++)
            {
                CenterPoint& p = traj.at(idx);
 
                if (queryDistance(wrapper, p.x, p.y) > 0.0)
                {
                    continue; // the barrier has a gradient here; let the optimizer work
                }
 
                // Only far enough to clear the gate. Demanding the full target here would fail
                // in exactly the tight places the escape hatch exists for, and it is not
                // needed: once the waypoint is out of the flat interior the barrier has a
                // gradient again and pulls it the rest of the way.
                const auto vertex = costmap.findClosestCollisionFreeVertex(
                    Eigen::Vector2f{static_cast<float>(p.x), static_cast<float>(p.y)},
                    static_cast<float>(params.tracking_max_deviation),
                    static_cast<float>(params.min_clearance));
 
                if (not vertex.has_value() or
                    std::hypot(vertex->position.x() - p.x, vertex->position.y() - p.y) >
                        params.tracking_max_deviation + 1e-3)
                {
                    unresolved.push_back(idx);
                    continue;
                }
 
                p.x = vertex->position.x();
                p.y = vertex->position.y();
            }
 
            return unresolved;
        }
 
        /// Interior obstacle samples for a segment, chosen so the optimizer never evaluates a
        /// segment more coarsely than the trajectory checker tests it.
        int segmentSampleCount(const CenterPoint& a, const CenterPoint& b, const Params& params)
        {
            const double length = std::hypot(b.x - a.x, b.y - a.y);
            const int needed =
                static_cast<int>(std::ceil(length / std::max(params.segment_sample_spacing, 1.0))) - 1;
 
            return std::clamp(
                needed, params.segment_obstacle_samples, params.max_segment_obstacle_samples);
        }
 
        /// Smallest clearance [mm] anywhere on a trajectory, sampled as the checker samples it.
        float minimumClearance(const core::GlobalTrajectory& trajectory,
                               const algorithms::Costmap& costmap)
        {
            constexpr float sampleSpacing = 50.F;
 
            float smallest = std::numeric_limits<float>::max();
 
            const auto sample = [&](const Eigen::Vector2f& position)
            {
                const auto value = costmap.value(position);
                // Unknown or masked reads as collision, matching the checker.
                smallest = std::min(smallest, value.has_value() ? value.value() : 0.F);
            };
 
            const auto& points = trajectory.points();
 
            for (std::size_t i = 0; i < points.size(); i++)
            {
                sample(points[i].waypoint.pose.translation().head<2>());
 
                if (i + 1 >= points.size())
                {
                    break;
                }
 
                const Eigen::Vector2f a = points[i].waypoint.pose.translation().head<2>();
                const Eigen::Vector2f b = points[i + 1].waypoint.pose.translation().head<2>();
                const int steps =
                    std::max(1, static_cast<int>(std::ceil((b - a).norm() / sampleSpacing)));
 
                for (int step = 1; step < steps; step++)
                {
                    sample(a + (b - a) * (static_cast<float>(step) / static_cast<float>(steps)));
                }
            }
 
            return (smallest == std::numeric_limits<float>::max()) ? 0.F : smallest;
        }
 
        bool validateTrajectory(const std::vector<CenterPoint>& traj, const std::string& label)
        {
            bool valid = true;
            for (int i = 0; i < static_cast<int>(traj.size()); ++i)
            {
                if (!std::isfinite(traj[i].x) || !std::isfinite(traj[i].y) ||
                    !std::isfinite(traj[i].theta) || !std::isfinite(traj[i].v))
                {
                    ARMARX_WARNING << "[" << label << "] NaN/Inf in traj[" << i
                                   << "] : x=" << traj[i].x << " y=" << traj[i].y
                                   << " theta=" << traj[i].theta << " v=" << traj[i].v;
                    valid = false;
                }
            }
            return valid;
        }
 
        ceres::Solver::Options makeSolverOptions(const Params& params, int maxIterations)
        {
            ceres::Solver::Options options;
            options.max_num_iterations = maxIterations;
            options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
            options.minimizer_progress_to_stdout = false;
            options.num_threads = params.num_threads;
            options.function_tolerance = 1e-6;
            options.parameter_tolerance = 1e-8;
            options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
            options.initial_trust_region_radius = params.initial_trust_region_radius;
            options.max_trust_region_radius = params.max_trust_region_radius;
            options.min_trust_region_radius = params.min_trust_region_radius;
            options.min_relative_decrease = 1e-4;
            options.use_nonmonotonic_steps = true;
            return options;
        }
 
        struct PassWeights
        {
            double w_obs;
            double w_pose_smooth;
            double w_spacing;
            double w_tracking;
            double w_safe_tracking;
        };
 
        // Unweighted, so the breakdown compares terms on a common footing. Delegates to the
        // residual's own barrier so the log cannot drift from what the solver minimises.
        double evalObstacleCost(const Costmap2DWrapper& wrapper,
                                double x,
                                double y,
                                double max_dist,
                                double clearance)
        {
            double d_raw;
            wrapper(&x, &y, &d_raw);
            return obstacleBarrier(d_raw, 1.0, max_dist, clearance);
        }
 
        void logResidualBreakdown(const std::vector<CenterPoint>& traj,
                                  const Costmap2DWrapper& wrapper,
                                  const std::vector<CenterPoint>& originalTargets,
                                  const Params& params,
                                  const std::string& label)
        {
            const int N = static_cast<int>(traj.size());
            if (N < 4)
                return;
 
            double c_obs_wp = 0.0;
            double c_obs_seg = 0.0;
            double c_smooth = 0.0;
            double c_tracking = 0.0;
            double c_safe_tracking = 0.0;
            double c_spacing = 0.0;
 
            // Obstacle (waypoints)
            for (int i = 0; i < N; ++i)
            {
                double r = evalObstacleCost(wrapper,
                                            traj[i].x,
                                            traj[i].y,
                                            params.obs_max_distance,
                                            params.clearance);
                c_obs_wp += r * r;
            }
 
            // Obstacle (segment samples)
            for (int i = 0; i < N - 1; ++i)
            {
                const int samples = segmentSampleCount(traj[i], traj[i + 1], params);
                for (int s = 1; s <= samples; ++s)
                {
                    double t = static_cast<double>(s) / (samples + 1);
                    double mx = (1.0 - t) * traj[i].x + t * traj[i + 1].x;
                    double my = (1.0 - t) * traj[i].y + t * traj[i + 1].y;
                    double r = evalObstacleCost(wrapper,
                                                mx,
                                                my,
                                                params.obs_max_distance,
                                                params.clearance);
                    c_obs_seg += r * r;
                }
            }
 
            // Position smoothness
            for (int i = 1; i <= N - 2; ++i)
            {
                double rx = traj[i - 1].x - 2.0 * traj[i].x + traj[i + 1].x;
                double ry = traj[i - 1].y - 2.0 * traj[i].y + traj[i + 1].y;
                c_smooth += rx * rx + ry * ry;
            }
 
            // Bounded tracking to the reference path
            for (int i = 1; i <= N - 2; ++i)
            {
                double dx = traj[i].x - originalTargets[i].x;
                double dy = traj[i].y - originalTargets[i].y;
                double d = std::hypot(dx, dy);
                double excess = d - params.tracking_deadzone;
                if (excess > 0)
                {
                    if (excess <= params.tracking_max_deviation - params.tracking_deadzone)
                    {
                        double r = params.w_tracking_in_bounds * excess;
                        c_tracking += r * r;
                    }
                    else
                    {
                        double over = excess - (params.tracking_max_deviation - params.tracking_deadzone);
                        double r = params.w_tracking_in_bounds * (params.tracking_max_deviation - params.tracking_deadzone)
                                   + params.w_tracking_hard * over;
                        c_tracking += r * r;
                    }
                }
            }
 
            // Soft tracking to the reference path
            for (int i = 1; i <= N - 2; ++i)
            {
                double rx = traj[i].x - originalTargets[i].x;
                double ry = traj[i].y - originalTargets[i].y;
                // Use pass3 weight for breakdown (representative)
                double w = params.pass3_w_tracking;
                c_safe_tracking += (w * rx) * (w * rx) + (w * ry) * (w * ry);
            }
 
            // Spacing
            double d_avg = computeAverageSpacing(traj);
            if (d_avg <= 1e-9)
                d_avg = 0.1;
            for (int i = 0; i < N - 1; ++i)
            {
                double dx = traj[i + 1].x - traj[i].x;
                double dy = traj[i + 1].y - traj[i].y;
                double dist_sq = dx * dx + dy * dy;
                double r = params.pass3_w_spacing * (dist_sq - d_avg * d_avg);
                c_spacing += r * r;
            }
 
            double total = c_obs_wp + c_obs_seg + c_smooth + c_tracking + c_safe_tracking + c_spacing;
 
            ARMARX_INFO << "=== Residual breakdown [" << label << "] ===";
            ARMARX_INFO << "Obstacle (waypoints): " << std::scientific << std::setprecision(3) << c_obs_wp;
            ARMARX_INFO << "Obstacle (segments):  " << std::scientific << std::setprecision(3) << c_obs_seg;
            ARMARX_INFO << "Position smoothness:  " << std::scientific << std::setprecision(3) << c_smooth;
            ARMARX_INFO << "Bounded tracking:     " << std::scientific << std::setprecision(3) << c_tracking;
            ARMARX_INFO << "Soft tracking:        " << std::scientific << std::setprecision(3) << c_safe_tracking;
            ARMARX_INFO << "Spacing:              " << std::scientific << std::setprecision(3) << c_spacing;
            ARMARX_INFO << "TOTAL:                " << std::scientific << std::setprecision(3) << total;
        }
 
        void runPositionPass(std::vector<CenterPoint>& traj,
                             const Costmap2DWrapper& wrapper,
                             const std::vector<CenterPoint>& originalTargets,
                             const PassWeights& w,
                             const Params& params,
                             int maxIterations,
                             bool pinTightWaypoints = false)
        {
            const int N = static_cast<int>(traj.size());
            if (N < 4)
            {
                ARMARX_INFO << "[SPFA smoothing] need at least 4 points for position pass";
                return;
            }
 
            normalizeTrajectoryAngles(traj);
 
            // Avoid zero-length segments.
            for (int i = 1; i < N - 1; ++i)
            {
                double dx = traj[i].x - traj[i - 1].x;
                double dy = traj[i].y - traj[i - 1].y;
                if (std::hypot(dx, dy) < 1.0)
                {
                    double ndx = (i + 1 < N) ? (traj[i + 1].x - traj[i].x) : 1.0;
                    double ndy = (i + 1 < N) ? (traj[i + 1].y - traj[i].y) : 0.0;
                    double nlen = std::hypot(ndx, ndy);
                    if (nlen < 1e-6)
                    {
                        ndx = 1.0;
                        ndy = 0.0;
                        nlen = 1.0;
                    }
                    traj[i].x += 1.0 * ndx / nlen;
                    traj[i].y += 1.0 * ndy / nlen;
                }
            }
 
            ceres::Problem problem;
            for (int i = 0; i < N; ++i)
                problem.AddParameterBlock(&traj[i].x, 4);
 
            problem.SetParameterBlockConstant(&traj.front().x);
            problem.SetParameterBlockConstant(&traj.back().x);
 
            // Position pass: only x, y are free.
            //
            // Optionally pin interior waypoints that are already close to an obstacle. The
            // count is logged because a threshold that pins most of a tight corridor leaves
            // nothing for the solver to do, and that is invisible from the result alone.
            int frozen = 0;
 
            for (int i = 1; i < N - 1; ++i)
            {
                const bool freeze =
                    (params.use_obstacle_freeze or pinTightWaypoints) and
                    queryDistance(wrapper, traj[i].x, traj[i].y) <= params.obstacle_freeze_threshold;
 
                if (freeze)
                {
                    problem.SetParameterBlockConstant(&traj[i].x);
                    frozen++;
                }
                else
                {
                    std::vector<int> constant_indices = {2, 3}; // theta, v
                    SET_CONSTRAINT(problem, &traj[i].x, new SubsetConstraint(4, constant_indices));
                }
            }
 
            if (params.use_obstacle_freeze or pinTightWaypoints)
            {
                ARMARX_INFO << "[SPFA smoothing] Pinned " << frozen << " of " << (N - 2)
                            << " interior waypoints below "
                            << params.obstacle_freeze_threshold << " mm clearance.";
            }
 
            // 1) Position smoothness
            for (int i = 1; i <= N - 2; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<PositionSmoothResidual, 2, 4, 4, 4>(
                        new PositionSmoothResidual(w.w_pose_smooth)),
                    nullptr,
                    &traj[i - 1].x,
                    &traj[i].x,
                    &traj[i + 1].x);
            }
 
            // 2) Spacing
            double d_avg = computeAverageSpacing(traj);
            if (d_avg <= 1e-9)
                d_avg = 0.1;
            for (int i = 0; i < N - 1; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<SpacingResidual, 1, 4, 4>(
                        new SpacingResidual(d_avg, w.w_spacing)),
                    nullptr,
                    &traj[i].x,
                    &traj[i + 1].x);
            }
 
            // 3) Obstacle residuals (waypoints)
            for (int i = 0; i < N; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<ObstacleResidual2D, 1, 4>(
                        new ObstacleResidual2D(wrapper,
                                               w.w_obs,
                                               params.obs_max_distance,
                                               params.clearance)),
                    nullptr,
                    &traj[i].x);
            }
 
            // 4) Obstacle residuals along segments (interior samples)
            for (int i = 0; i < N - 1; ++i)
            {
                // Sized from the segment as it stands when the problem is built. The optimizer
                // can lengthen it, so the bound below is deliberately generous.
                const int samples = segmentSampleCount(traj[i], traj[i + 1], params);
                for (int s = 1; s <= samples; ++s)
                {
                    double t = static_cast<double>(s) / (samples + 1);
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<SegmentObstacleResidual2D, 1, 4, 4>(
                            new SegmentObstacleResidual2D(wrapper,
                                                          w.w_obs,
                                                          params.obs_max_distance,
                                                          params.clearance,
                                                          t)),
                        nullptr,
                        &traj[i].x,
                        &traj[i + 1].x);
                }
            }
 
            // Per-waypoint leash strength from the clearance at its reference position:
            // 1 where room is scarce, fading to 0 where it is plentiful. Evaluated on the
            // reference rather than the current iterate so the weights are fixed for the
            // solve -- a weight that moved with the parameters would make the problem
            // non-stationary and the trust region meaningless.
            std::vector<double> trackingScale(N, 1.0);
 
            if (params.use_tracking_clearance_adaptive)
            {
                for (int i = 0; i < N; ++i)
                {
                    trackingScale[i] = trackingScaleFromClearance(
                        queryDistance(wrapper, originalTargets[i].x, originalTargets[i].y),
                        params.tracking_fade_start_clearance,
                        params.tracking_fade_end_clearance);
                }
            }
 
            // 5) Bounded tracking: hard cap on how far a waypoint may leave the
            //    reference path, which is what the final deviation check measures.
            //
            // Deliberately not scaled. This one enforces `tracking_max_deviation`, which the
            // caller checks afterwards, so relaxing it in open space would let the optimizer
            // produce a path the deviation check then rejects.
            for (int i = 1; i <= N - 2; ++i)
            {
                problem.AddResidualBlock(
                    new ceres::AutoDiffCostFunction<BoundedTrackingResidual, 1, 4>(
                        new BoundedTrackingResidual(originalTargets[i].x,
                                                    originalTargets[i].y,
                                                    params.tracking_deadzone,
                                                    params.tracking_max_deviation,
                                                    params.w_tracking_in_bounds,
                                                    params.w_tracking_hard)),
                    nullptr,
                    &traj[i].x);
            }
 
            // 6) Soft tracking to the original SPFA path (weak, keeps deviation small)
            if (w.w_tracking > 0.0)
            {
                for (int i = 1; i <= N - 2; ++i)
                {
                    const double weight = w.w_tracking * trackingScale[i];
 
                    // A zero weight is a residual block that can only cost time.
                    if (weight <= 0.0)
                    {
                        continue;
                    }
 
                    problem.AddResidualBlock(
                        new ceres::AutoDiffCostFunction<SoftTrackingResidual, 2, 4>(
                            new SoftTrackingResidual(
                                originalTargets[i].x, originalTargets[i].y, weight)),
                        nullptr,
                        &traj[i].x);
                }
            }
 
            ceres::Solver::Options options = makeSolverOptions(params, maxIterations);
            ceres::Solver::Summary summary;
            ceres::Solve(options, &problem, &summary);
 
            normalizeTrajectoryAngles(traj);
 
            ARMARX_VERBOSE << summary.FullReport();
            ARMARX_INFO << "[Position pass] Final cost: " << summary.final_cost
                        << " iterations: " << summary.num_successful_steps
                        << " termination: " << summary.termination_type;
        }
 
        bool repairWaypoints(std::vector<CenterPoint>& traj,
                             const algorithms::Costmap& costmap,
                             const Costmap2DWrapper& wrapper,
                             const std::vector<CenterPoint>& originalTargets,
                             const Params& params)
        {
            const int N = static_cast<int>(traj.size());
 
            for (int i = 1; i < N - 1; ++i)
            {
                // Repair what the gate refuses, not everything short of the target: a waypoint
                // between the two is one the optimizer chose and the gate accepts, so snapping
                // it elsewhere would undo the optimization for nothing.
                double d = queryDistance(wrapper, traj[i].x, traj[i].y);
                if (d > params.min_clearance)
                    continue;
 
                // Search around where the waypoint *is*, not around where it started. Its
                // neighbours keep their optimized positions, so snapping this one back to the
                // reference path drags it against them -- which is a fold, manufactured by the
                // repair itself. The deviation bound still applies to the reference, but as a
                // constraint on the result rather than as the search origin.
                const auto vertex = costmap.findClosestCollisionFreeVertex(
                    Eigen::Vector2f{static_cast<float>(traj[i].x),
                                    static_cast<float>(traj[i].y)},
                    static_cast<float>(params.tracking_max_deviation),
                    static_cast<float>(params.min_clearance));
 
                if (!vertex)
                    return false;
 
                const double dx = vertex->position.x() - originalTargets[i].x;
                const double dy = vertex->position.y() - originalTargets[i].y;
                if (std::hypot(dx, dy) > params.tracking_max_deviation + 1e-3)
                    return false;
 
                traj[i].x = vertex->position.x();
                traj[i].y = vertex->position.y();
            }
 
            return true;
        }
 
        double computeMaxDeviation(const std::vector<CenterPoint>& traj,
                                   const std::vector<CenterPoint>& originalTargets)
        {
            double maxDev = 0.0;
            for (std::size_t i = 0; i < traj.size(); ++i)
            {
                double dx = traj[i].x - originalTargets[i].x;
                double dy = traj[i].y - originalTargets[i].y;
                maxDev = std::max(maxDev, std::hypot(dx, dy));
            }
            return maxDev;
        }
    } // namespace
 
    SPFASmoothing::SPFASmoothing(const core::GlobalTrajectory& trajectory,
                                 const algorithms::Costmap& costmap,
                                 const Params& params) :
        trajectory(trajectory),
        costmap(costmap),
        params(params)
    {
    }
 
    SPFASmoothing::OptimizationResult SPFASmoothing::optimize()
    {
        Costmap2DWrapper wrapper{costmap};
 
        std::vector<CenterPoint> traj = toCenterTrajectory(trajectory);
        const std::vector<CenterPoint> originalTargets = traj;
 
        if (!validateTrajectory(traj, "pre-processing"))
        {
            ARMARX_WARNING << "[SPFA smoothing] Invalid trajectory before pre-processing. Aborting.";
            return {std::nullopt, std::nullopt, false, false, {}};
        }
 
        const std::vector<std::size_t> trapped =
            projectTrappedWaypoints(traj, wrapper, costmap, params);
 
        if (not trapped.empty())
        {
            ARMARX_WARNING << "[SPFA smoothing] " << trapped.size()
                           << " waypoint(s) sit inside an obstacle and could not be projected "
                              "clear within "
                           << params.tracking_max_deviation
                           << " mm; first at index " << trapped.front()
                           << ". The optimizer sees no gradient there, so the result will very "
                              "likely fail the collision check.";
        }
 
        // The path after the escape hatch, kept for the caller and as the fallback if a pass
        // produces non-finite values.
        const std::vector<CenterPoint> preprocessed = traj;
 
        if (!validateTrajectory(traj, "post-projection"))
        {
            ARMARX_WARNING << "[SPFA smoothing] Invalid trajectory after pre-processing. Aborting.";
            return {std::nullopt, std::nullopt, false, false, {}};
        }
 
        logResidualBreakdown(traj, wrapper, originalTargets, params, "after pre-processing");
 
        // Multi-pass position optimization.
        const std::array<PassWeights, 3> passes = {
            PassWeights{params.pass1_w_obs,
                        params.pass1_w_pose_smooth,
                        params.pass1_w_spacing,
                        params.pass1_w_tracking,
                        params.pass1_w_safe_tracking},
            PassWeights{params.pass2_w_obs,
                        params.pass2_w_pose_smooth,
                        params.pass2_w_spacing,
                        params.pass2_w_tracking,
                        params.pass2_w_safe_tracking},
            PassWeights{params.pass3_w_obs,
                        params.pass3_w_pose_smooth,
                        params.pass3_w_spacing,
                        params.pass3_w_tracking,
                        params.pass3_w_safe_tracking}};
 
        for (size_t pass = 0; pass < passes.size(); ++pass)
        {
            ARMARX_INFO << "[SPFA smoothing] Starting position pass " << (pass + 1);
            runPositionPass(traj, wrapper, originalTargets, passes[pass], params, params.max_iterations);
            logResidualBreakdown(traj, wrapper, originalTargets, params,
                                 "after position pass " + std::to_string(pass + 1));
 
            if (!validateTrajectory(traj, "post-position-pass-" + std::to_string(pass + 1)))
            {
                ARMARX_WARNING << "[SPFA smoothing] NaN/Inf after position pass " << (pass + 1)
                               << ". Reverting to the pre-processed path.";
                traj = preprocessed;
                break;
            }
        }
 
        normalizeTrajectoryAngles(traj);
 
        // Collision repair if needed.
        // The gate enforces the hard limit; the target is what the barrier aims for and is
        // deliberately not enforced. Rejecting a path merely for missing the target discards it
        // in favour of the unsmoothed one, which is rougher and no safer.
        //
        // And when the input is *itself* inside the limit, the same argument applies to the
        // limit: no smoothed path can clear a floor the route it was given does not clear, so
        // enforcing it guarantees the unsmoothed path is driven -- at the very same distance
        // from obstacles, only rougher. In that case the floor becomes what the input already
        // had, which asks the smoother not to make the worst point worse rather than to fix
        // something outside its reach.
        const float inputMinClearance = minimumClearance(trajectory, costmap);
 
        const bool judgeAgainstInput =
            params.allow_degraded_input and
            inputMinClearance < static_cast<float>(params.min_clearance);
 
 
        // Never below a positive floor, whatever the input did. The relative rule is for a
        // path that is *tight* -- the robot fits, with less room than we would like. A path
        // the robot does not fit through at all reads 0 here, and taking that as the floor
        // would make the gate vacuous: nothing is below zero, so every path would pass,
        // including one straight through a wall. An input like that is a recovery problem,
        // not something for the smoother to be judged lenient about.
        constexpr double absoluteFloor = 1.0;
 
        const double effectiveLimit =
            judgeAgainstInput ? std::max(absoluteFloor, static_cast<double>(inputMinClearance))
                              : params.min_clearance;
 
        if (judgeAgainstInput)
        {
            ARMARX_INFO << "[SPFA smoothing] The path handed in already reaches "
                        << inputMinClearance << " mm, inside the " << params.min_clearance
                        << " mm limit. Judging the result against that instead, so the "
                           "alternative is not simply a rougher path at the same clearance.";
        }
 
        TrajectoryChecker2D checker(costmap, effectiveLimit);
        core::GlobalTrajectory smoothedTrajectory = toGlobalTrajectory(traj);
        bool collisionFree = checker.check(smoothedTrajectory);
 
        if (!collisionFree)
        {
            ARMARX_WARNING << "[SPFA smoothing] Initial smoothing has collisions. Running repair pass.";
 
            if (!repairWaypoints(traj, costmap, wrapper, originalTargets, params))
            {
                ARMARX_ERROR << "[SPFA smoothing] Repair failed: could not project colliding waypoints "
                                "to collision-free positions within max deviation.";
            }
            else
            {
                PassWeights repairWeights{params.repair_w_obs,
                                          params.repair_w_pose_smooth,
                                          params.repair_w_spacing,
                                          params.repair_w_tracking,
                                          params.repair_w_safe_tracking};
                runPositionPass(traj, wrapper, originalTargets, repairWeights, params, params.repair_max_iterations);
                logResidualBreakdown(traj, wrapper, originalTargets, params, "after repair pass");
 
                smoothedTrajectory = toGlobalTrajectory(traj);
                collisionFree = checker.check(smoothedTrajectory);
            }
        }
 
        // Last resort: pin the waypoints that have no room left and solve once more.
        //
        // Pinning is a poor policy but a good fallback. Applied to every request it roughly
        // doubles the worst-case corner, because the kinks worth removing sit exactly where
        // clearance is scarce and pinning protects them. Applied only after a request has
        // already failed, it costs nothing on the requests that succeeded and converts most of
        // the rest -- and a pinned, accepted path is still far straighter than the unsmoothed
        // one it would otherwise fall back to.
        //
        // Deliberately outside the repair branch above: this is the response to "still
        // colliding", regardless of whether the projection that preceded it worked.
        if (not collisionFree and not params.use_obstacle_freeze)
        {
            ARMARX_WARNING << "[SPFA smoothing] Still colliding. Retrying with tight waypoints "
                              "pinned.";
 
            // Restart from the planner's own path rather than the optimized one. Pinning
            // decides what to hold from where the waypoints currently are, and by this point
            // some of them are exactly the ones that moved into collision -- pinning those
            // guarantees the retry fails. The grid path is collision-free by construction, so
            // pinning its tight waypoints holds them somewhere valid and lets the rest move.
            //
            // The full pass schedule, not just the repair weights: this is a fresh attempt
            // from the original path, so it needs the same progression from gentle to firm
            // that the first attempt had.
            std::vector<CenterPoint> pinned = originalTargets;
 
            for (const PassWeights& pass : passes)
            {
                runPositionPass(pinned,
                                wrapper,
                                originalTargets,
                                pass,
                                params,
                                params.max_iterations,
                                /*pinTightWaypoints=*/true);
            }
 
            const core::GlobalTrajectory candidate = toGlobalTrajectory(pinned);
 
            if (checker.check(candidate))
            {
                ARMARX_WARNING << "[SPFA smoothing] Pinned retry succeeded.";
                traj = pinned;
                smoothedTrajectory = candidate;
                collisionFree = true;
            }
        }
 
        const double maxDeviation = computeMaxDeviation(traj, originalTargets);
        const bool deviationOk = maxDeviation <= params.tracking_max_deviation + 1e-3;
 
        if (collisionFree && deviationOk)
        {
            ARMARX_INFO << "[SPFA smoothing] Smoothed trajectory is collision-free. Max deviation: "
                        << maxDeviation << " mm";
        }
        else
        {
            if (!collisionFree)
                ARMARX_WARNING << "[SPFA smoothing] Smoothed trajectory still has collisions after repair.";
            if (!deviationOk)
                ARMARX_WARNING << "[SPFA smoothing] Max deviation too large: " << maxDeviation
                               << " mm (limit: " << params.tracking_max_deviation << " mm).";
        }
 
        for (int i = 0; i < static_cast<int>(traj.size()); ++i)
        {
            ARMARX_DEBUG << std::fixed << std::setprecision(3) << "i=" << i << " : x=" << traj[i].x
                         << " y=" << traj[i].y << " theta=" << traj[i].theta << " v=" << traj[i].v;
        }
 
        // Geometry check, after the collision repair so it sees the path that would actually be
        // handed on. A fold is invisible to every residual in this optimizer -- squared distances
        // are sign-blind and the second difference vanishes for a point placed behind its
        // predecessor -- and it is invisible to the collision check too, so this is the only
        // place it can be caught.
        std::vector<std::size_t> repairedWaypoints;
        bool geometryValid = true;
 
        const std::vector<std::size_t> violations = findGeometryViolations(smoothedTrajectory);
 
        if (not violations.empty())
        {
            ARMARX_WARNING << "[nav-guard] path-fold-detected: the smoothed path reverses "
                              "direction at "
                           << violations.size() << " waypoint(s), first at index "
                           << violations.front()
                           << ". Repairing by removing them; a fold collapses the spline tangent "
                              "downstream and stalls the reparametrization there.";
 
            const core::GlobalTrajectory candidate =
                repairGeometry(smoothedTrajectory, repairedWaypoints);
 
            // Removing a waypoint creates a new segment between its neighbours, and the straight
            // line between them can cut a corner the original path went around.
            const bool repairedIsCollisionFree = checker.check(candidate);
            const bool repairedIsMonotone = findGeometryViolations(candidate).empty();
 
            if (repairedIsCollisionFree and repairedIsMonotone)
            {
                ARMARX_WARNING << "[nav-guard] path-fold-repaired: removed "
                               << repairedWaypoints.size() << " waypoint(s), "
                               << smoothedTrajectory.points().size() << " -> "
                               << candidate.points().size()
                               << " points, still collision-free.";
 
                smoothedTrajectory = candidate;
            }
            else
            {
                ARMARX_WARNING << "[nav-guard] path-fold-unrepairable: removing the folded "
                                  "waypoint(s) would "
                               << (repairedIsCollisionFree ? "leave the path folded"
                                                           : "cut a corner into an obstacle")
                               << ". Rejecting the smoothed path; the caller falls back to the "
                                  "unsmoothed one.";
 
                repairedWaypoints.clear();
                geometryValid = false;
            }
        }
 
        const core::GlobalTrajectory preprocessedTrajectory = toGlobalTrajectory(preprocessed);
        const bool success = collisionFree && deviationOk;
 
        return {smoothedTrajectory,
                preprocessedTrajectory,
                success,
                geometryValid,
                repairedWaypoints,
                inputMinClearance,
                minimumClearance(smoothedTrajectory, costmap),
                judgeAgainstInput};
    }
 
} // namespace armarx::navigation::algorithms::spfa::smoothing