SPFA.cpp

Go to the documentation of this file.

#include "SPFA.h"
 
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <iomanip>
#include <optional>
#include <sstream>
#include <tuple>
#include <vector>
 
#include <Eigen/Geometry>
 
#include <IceUtil/Time.h>
 
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/transform.hpp>
#include <range/v3/view/zip.hpp>
 
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/algorithms/smoothing/ChainApproximation.h>
#include <armarx/navigation/algorithms/spfa/ShortestPathFasterAlgorithm.h>
#include <armarx/navigation/algorithms/spfa/smoothing/SPFASmoothing.h>
#include <armarx/navigation/algorithms/spfa/smoothing/io.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/global_planning/GlobalPlanner.h>
#include <armarx/navigation/global_planning/aron/SPFAParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron_conversions.h>
#include <armarx/navigation/global_planning/core.h>
#include <armarx/navigation/global_planning/optimization/OrientationOptimizer.h>
 
namespace
{
    struct PipelineTimings
    {
        std::chrono::microseconds constructPath{0};
        std::chrono::microseconds buildTrajectory{0};
        std::chrono::microseconds resample{0};
        std::chrono::microseconds smoothPositions{0};
        std::chrono::microseconds recomputeVelocities{0};
        std::chrono::microseconds recoveryHandling{0};
        std::chrono::microseconds loadOrientationConfig{0};
        std::chrono::microseconds optimizeOrientation{0};
        std::chrono::microseconds stitchTrajectory{0};
        std::chrono::microseconds finalVelocityClamp{0};
    };
 
    class ScopedStageTimer
    {
    public:
        explicit ScopedStageTimer(std::chrono::microseconds& out) :
            out_(out), start_(std::chrono::steady_clock::now())
        {
        }
 
        ~ScopedStageTimer()
        {
            out_ = std::chrono::duration_cast<std::chrono::microseconds>(
                std::chrono::steady_clock::now() - start_);
        }
 
    private:
        std::chrono::microseconds& out_;
        std::chrono::steady_clock::time_point start_;
    };
 
    std::map<std::string, float> toMap(const PipelineTimings& timings)
    {
        const auto seconds = [](std::chrono::microseconds us) -> float
        { return static_cast<float>(us.count()) / 1e6F; };
 
        return {{"construct_path", seconds(timings.constructPath)},
                {"build_trajectory", seconds(timings.buildTrajectory)},
                {"resample", seconds(timings.resample)},
                {"smooth_positions", seconds(timings.smoothPositions)},
                {"recompute_velocities", seconds(timings.recomputeVelocities)},
                {"recovery_handling", seconds(timings.recoveryHandling)},
                {"load_orientation_config", seconds(timings.loadOrientationConfig)},
                {"optimize_orientation", seconds(timings.optimizeOrientation)},
                {"stitch_trajectory", seconds(timings.stitchTrajectory)},
                {"final_velocity_clamp", seconds(timings.finalVelocityClamp)}};
    }
 
    /// The grid search runs before `calculatePath`, so its duration is carried on the
    /// `PlanningResult` rather than in `PipelineTimings`.
    std::map<std::string, float>
    withGridSearch(const PipelineTimings& timings,
                   const armarx::navigation::global_planning::PlanningResult& planner)
    {
        std::map<std::string, float> map = toMap(timings);
        map["grid_search"] = planner.gridSearchDuration;
 
        return map;
    }
 
    std::string formatTimings(const PipelineTimings& timings)
    {
        const auto ms = [](std::chrono::microseconds us) -> std::string
        {
            std::ostringstream oss;
            oss << std::fixed << std::setprecision(3) << (us.count() / 1000.0);
            return oss.str();
        };
 
        std::ostringstream oss;
        oss << "[SPFA timing] "
            << "build=" << ms(timings.buildTrajectory) << "ms "
            << "resample=" << ms(timings.resample) << "ms "
            << "smooth=" << ms(timings.smoothPositions) << "ms "
            << "velocity=" << ms(timings.recomputeVelocities) << "ms "
            << "recovery=" << ms(timings.recoveryHandling) << "ms "
            << "orientationConfig=" << ms(timings.loadOrientationConfig) << "ms "
            << "orientationOptimize=" << ms(timings.optimizeOrientation) << "ms "
            << "stitch=" << ms(timings.stitchTrajectory) << "ms "
            << "clamp=" << ms(timings.finalVelocityClamp) << "ms";
        return oss.str();
    }
} // namespace
 
namespace armarx::navigation::global_planning
{
 
    // SPFAParams
 
    Algorithms
    SPFAParams::algorithm() const
    {
        return Algorithms::SPFA;
    }
 
    aron::data::DictPtr
    SPFAParams::toAron() const
    {
        arondto::SPFAParams dto;
 
        aron_conv::toAron(dto, *this);
 
        return dto.toAron();
    }
 
    SPFAParams
    SPFAParams::FromAron(const aron::data::DictPtr& dict)
    {
        ARMARX_CHECK_NOT_NULL(dict);
 
        // ARMARX_DEBUG << dict->getAllKeysAsString();
 
        arondto::SPFAParams dto;
        dto.fromAron(dict);
 
        SPFAParams bo;
        aron_conv::fromAron(dto, bo);
 
        return bo;
    }
 
    // SPFA
 
    SPFA::SPFA(const Params& params,
               const core::GeneralConfig& generalConfig,
               const core::Scene& ctx) :
        GlobalPlanner(generalConfig, ctx),
        impl_(params,
              generalConfig,
              ctx.staticScene.has_value() ? ctx.staticScene->distanceToObstaclesCostmap
                                          : std::nullopt)
    {
    }
 
    std::optional<GlobalPlannerResult>
    SPFA::plan(const core::Pose& goal)
    {
        const core::Pose start(scene.robot->getGlobalPose());
        return plan(start, goal);
    }
 
    std::optional<GlobalPlannerResult>
    SPFA::plan(const core::Pose& start, const core::Pose& goal)
    {
        if (scene.staticScene.has_value())
        {
            impl_.updateCostmap(scene.staticScene->distanceToObstaclesCostmap);
        }
        return impl_.plan(start, goal);
    }
 
    SPFA::PlanningResult
    SPFA::executePlanner(const core::Pose& start)
    {
        return impl_.executePlanner(start);
    }
 
    std::optional<GlobalPlannerResult>
    SPFA::calculatePath(const PlanningResult& planner, const core::Pose& goal)
    {
        return impl_.calculatePath(planner, goal);
    }
 
    // SPFAImpl
 
    SPFAImpl::SPFAImpl(const Params& params,
                           const core::GeneralConfig& generalParams,
                           const std::optional<navigation::algorithms::Costmap>& costmap) :
            params_(params), generalConfig_(generalParams), costmap_(costmap)
        {
        }
 
        void
        SPFAImpl::updateCostmap(const std::optional<navigation::algorithms::Costmap>& costmap)
        {
            if (costmap.has_value())
            {
                costmap_.emplace(costmap.value());
            }
            else
            {
                costmap_.reset();
            }
        }
 
    algorithms::ObstacleAwareVelocityLimit
    SPFAImpl::velocityLimit() const
    {
        ARMARX_CHECK(costmap_.has_value());
 
        return algorithms::ObstacleAwareVelocityLimit{
            costmap_.value(),
            algorithms::ObstacleAwareVelocityLimit::Parameters{
                .maxVelocity = generalConfig_.maxVel.linear,
                .obstacleMaxDistance = params_.algo.obstacleMaxDistance,
                .obstacleDistanceWeight = params_.algo.obstacleDistanceWeight,
                .obstacleCostExponent = params_.algo.obstacleCostExponent}};
    }
 
    std::optional<GlobalPlannerResult>
    SPFAImpl::plan(const core::Pose& start, const core::Pose& goal)
    {
        const Eigen::Vector2f startPos2D = conv::to2D(start.translation());
 
        // Check if start is in collision
        if (costmap_->isInCollision(startPos2D))
        {
            ARMARX_WARNING << "Start position " << startPos2D << " is in collision. "
                           << "Searching for recovery position within "
                           << generalConfig_.inCollisionDistanceThresholdForRecovery << " mm...";
 
            const auto recoveryVertex = costmap_->findClosestCollisionFreeVertex(
                startPos2D, generalConfig_.inCollisionDistanceThresholdForRecovery);
 
            if (!recoveryVertex)
            {
                ARMARX_ERROR << "No valid recovery position found within threshold of "
                             << generalConfig_.inCollisionDistanceThresholdForRecovery << " mm";
                return std::nullopt;
            }
 
            const Eigen::Vector2f recoveryPos = recoveryVertex->position;
 
            ARMARX_WARNING << "Found recovery position at " << recoveryPos
                           << " (distance: " << (recoveryPos - startPos2D).norm() << " mm). "
                           << "Prepending original start position to trajectory.";
 
            const auto planningResult = executePlannerFromPosition(recoveryPos);
 
            if (!planningResult.plan.has_value())
            {
                return std::nullopt;
            }
 
            const RecoveryInfo recoveryInfo{start, recoveryPos};
 
            auto result = calculatePath(planningResult, goal, recoveryInfo);
 
            if (!result && generalConfig_.navigateCloseAsPossible)
            {
                const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
                const auto closest = algorithms::spfa::findClosestReachablePosition(
                    planningResult.plan.value(), goalPos, costmap_.value());
                if (closest)
                {
                    ARMARX_WARNING << "Goal " << goalPos << " is not reachable. "
                                   << "Navigating to closest reachable position at "
                                   << closest->position
                                   << " (distance to goal: " << closest->euclideanDistanceToGoal
                                   << " mm)";
                    core::Pose alternativeGoal = goal;
                    alternativeGoal.translation().head<2>() = closest->position;
                    result = calculatePath(planningResult, alternativeGoal, recoveryInfo);
                }
            }
 
            return result;
        }
 
        // Normal case: start is valid
        const auto planningResult = executePlanner(start);
        auto result = calculatePath(planningResult, goal);
 
        if (!result && generalConfig_.navigateCloseAsPossible && planningResult.plan.has_value())
        {
            const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
            const auto closest = algorithms::spfa::findClosestReachablePosition(
                planningResult.plan.value(), goalPos, costmap_.value());
            if (closest)
            {
                ARMARX_WARNING << "Goal " << goalPos << " is not reachable. "
                               << "Navigating to closest reachable position at "
                               << closest->position
                               << " (distance to goal: " << closest->euclideanDistanceToGoal
                               << " mm)";
                core::Pose alternativeGoal = goal;
                alternativeGoal.translation().head<2>() = closest->position;
                result = calculatePath(planningResult, alternativeGoal);
            }
        }
 
        return result;
    }
 
    SPFAImpl::PlanningResult
    SPFAImpl::executePlanner(const core::Pose& start)
    {
        ARMARX_TRACE;
 
        const Eigen::Vector2f startPosition = conv::to2D(start.translation());
 
        // FIXME check if costmap is available
        ARMARX_CHECK(costmap_.has_value());
 
        algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams = params_.algo;
 
        PlanningResult result{
            .start = start, // Preserve full pose including orientation
            .algorithm =
                algorithms::spfa::ShortestPathFasterAlgorithm(costmap_.value(), spfaParams),
            .plan = std::nullopt,
        };
 
        const auto timeStart = IceUtil::Time::now();
 
        algorithms::spfa::ShortestPathFasterAlgorithm::PlanningResult plan;
        try
        {
            result.plan = result.algorithm.spfa(startPosition, false);
        }
        catch (...)
        {
            ARMARX_INFO << "Could not execute spfa from " << "(" << startPosition.x() << ","
                        << startPosition.y() << ")" << " due to exception "
                        << GetHandledExceptionString();
        }
 
        const auto timeEnd = IceUtil::Time::now();
 
        result.gridSearchDuration =
            static_cast<float>((timeEnd - timeStart).toMicroSeconds()) / 1e6F;
 
        ARMARX_VERBOSE << "SPFA execution time: " << (timeEnd - timeStart).toMilliSeconds()
                       << " ms";
 
        return result;
    }
 
    SPFAImpl::PlanningResult
    SPFAImpl::executePlannerFromPosition(const Eigen::Vector2f& startPosition)
    {
        // Create a pose with identity orientation for backward compatibility
        const core::Pose startPose(Eigen::Translation3f(conv::to3D(startPosition)));
        return executePlanner(startPose);
    }
 
    std::optional<GlobalPlannerResult>
    SPFAImpl::calculatePath(const PlanningResult& planner,
                            const core::Pose& goal,
                            const std::optional<RecoveryInfo>& recovery)
    {
        PipelineTimings timings;
 
        if (not planner.plan.has_value())
        {
            ARMARX_INFO << "Invalid spfa result, could not calculate path!";
            return std::nullopt;
        }
 
        const Eigen::Vector2f goalPos = conv::to2D(goal.translation());
 
        // Determine effective start position
        Eigen::Vector2f effectiveStartPos;
        if (recovery.has_value())
        {
            effectiveStartPos = recovery->recoveryPosition;
        }
        else
        {
            effectiveStartPos = conv::to2D(planner.start.translation());
        }
 
        algorithms::spfa::ShortestPathFasterAlgorithm::PlanningResult plan;
        try
        {
            ScopedStageTimer timer(timings.constructPath);
            plan =
                planner.algorithm.constructPath(effectiveStartPos, planner.plan->parents, goalPos);
            ARMARX_CHECK(plan);
        }
        catch (...)
        {
            ARMARX_INFO << "Could not plan collision-free path from"
                        << (recovery ? recovery->recoveryPosition
                                     : conv::to2D(planner.start.translation()))
                        << " to " << "(" << goal.translation().x() << "," << goal.translation().y()
                        << ")" << " due to exception " << GetHandledExceptionString();
 
            return std::nullopt;
        }
 
        // Prepend original start position if recovery was used
        if (recovery.has_value())
        {
            plan.path.insert(plan.path.begin(),
                             conv::to2D(recovery->originalStartPose.translation()));
            ARMARX_INFO << "Prepended original start position. Path now has " << plan.path.size()
                        << " points.";
        }
 
        ARMARX_VERBOSE << "Path contains " << plan.path.size() << " points";
 
        ARMARX_DEBUG << "The plan consists of the following positions:";
        for (const auto& position : plan.path)
        {
            ARMARX_DEBUG << position;
        }
 
        ARMARX_TRACE;
        const auto plan3d = conv::to3D(plan.path);
 
        std::vector<core::Position> wpts;
        // when the position is already reached (but not the orientation) the planned path is empty
        if (plan3d.size() >= 2)
        {
            for (size_t i = 1; i < (plan3d.size() - 1); i++)
            {
                wpts.push_back(plan3d.at(i));
            }
        }
 
        // ARMARX_TRACE;
        // auto smoothPlan = postProcessPath(plan.path);
        // ARMARX_IMPORTANT << "Smooth path contains " << smoothPlan.size() << " points";
 
        // ARMARX_TRACE;
        // // we need to strip the first and the last points from the plan as they encode the start and goal position
        // smoothPlan.erase(smoothPlan.begin());
        // smoothPlan.pop_back();
 
        // Compute velocities for all positions
        // For recovery case: includes collision start (which is planner.start) and recovery position (which is wpts[0])
        core::Positions positionsForInitialVelocities;
        positionsForInitialVelocities.reserve(wpts.size() + 2);
 
        // For recovery case: use the collision start pose from recoveryInfo
        // For normal case: use planner.start
        const core::Position startPosition = recovery.has_value()
                                                 ? recovery->originalStartPose.translation()
                                                 : planner.start.translation();
 
        positionsForInitialVelocities.emplace_back(startPosition);
        positionsForInitialVelocities.insert(
            positionsForInitialVelocities.end(), wpts.begin(), wpts.end());
        positionsForInitialVelocities.emplace_back(goal.translation());
 
        const std::vector<float> velocitiesRespectingObstacles =
            velocityLimit().at(positionsForInitialVelocities);
 
        ARMARX_TRACE;
 
        // Build trajectory normally (same for both recovery and non-recovery cases)
        // Use the collision start pose for recovery case, planner.start for normal case
        const core::Pose& trajectoryStart =
            recovery.has_value() ? recovery->originalStartPose : planner.start;
 
        core::GlobalTrajectory trajectory;
        {
            ScopedStageTimer timer(timings.buildTrajectory);
            trajectory = core::GlobalTrajectory::FromPath(
                trajectoryStart, wpts, goal, velocitiesRespectingObstacles);
        }
 
        // TODO(fabian.reister): resampling of trajectory
 
        std::optional<core::GlobalTrajectory> resampledTrajectory;
 
        {
            ScopedStageTimer timer(timings.resample);
            try
            {
                resampledTrajectory = trajectory.resample(200);
                ARMARX_DEBUG << "Terminal velocity: " << resampledTrajectory->points().back().velocity;
            }
            catch (...)
            {
                ARMARX_INFO << "Caught exception during resampling: " << GetHandledExceptionString();
                resampledTrajectory = trajectory;
            }
 
            ARMARX_VERBOSE << "Resampled trajectory contains " << resampledTrajectory->points().size()
                           << " points";
 
            resampledTrajectory->setMaxVelocity(generalConfig_.maxVel.linear);
            ARMARX_DEBUG << "Terminal velocity: " << resampledTrajectory->points().back().velocity;
        }
 
        if (resampledTrajectory->points().size() == 2)
        {
            ARMARX_VERBOSE << "Only start and goal provided. Not optimizing orientation";
            ARMARX_TRACE;
            ARMARX_INFO << formatTimings(timings);
            return GlobalPlannerResult{.trajectory = resampledTrajectory.value(),
                                       .helperTrajectory = std::nullopt,
                                       .timings = withGridSearch(timings, planner),
                                       .gridPath = plan3d};
        }
 
        bool positionSmoothingApplied = false;
 
        // Smooth the resampled trajectory using the 2-D distance-to-obstacle costmap.
        // Theta is not optimized here; orientations are computed afterwards by the
        // OrientationOptimizer.
        if (params_.enablePositionSmoothing)
        {
            ScopedStageTimer timer(timings.smoothPositions);
            ARMARX_TRACE;
            const auto smoothingParams =
                algorithms::spfa::smoothing::io::loadSmoothingParams();
            algorithms::spfa::smoothing::SPFASmoothing smoother(
                resampledTrajectory.value(), costmap_.value(), smoothingParams);
            const auto smoothingResult = smoother.optimize();
 
            if (smoothingResult.trajectory && smoothingResult.isCollisionFree &&
                smoothingResult.isGeometryValid)
            {
                resampledTrajectory = smoothingResult.trajectory.value();
                positionSmoothingApplied = true;
 
                if (smoothingResult.repairedWaypoints.empty())
                {
                    ARMARX_INFO << "SPFA position smoothing succeeded.";
                }
                else
                {
                    ARMARX_INFO << "SPFA position smoothing succeeded after removing "
                                << smoothingResult.repairedWaypoints.size()
                                << " folded waypoint(s).";
                }
            }
            else
            {
                // Says which check failed: a collision-free path can still be folded, and the two
                // need different fixes.
                ARMARX_WARNING << "[nav-guard] smoothing-rejected: "
                               << (smoothingResult.isCollisionFree
                                       ? "the smoothed path is not geometrically valid (it "
                                         "reverses direction)"
                                       : "the smoothed path is not collision-free")
                               << "; using original resampled SPFA path.";
            }
        }
        else
        {
            ARMARX_INFO << "SPFA position smoothing is disabled.";
        }
 
        // Recompute velocities based on the (possibly smoothed) positions so that obstacle
        // proximity is reflected correctly after the path has been deformed.
        {
            ScopedStageTimer timer(timings.recomputeVelocities);
            ARMARX_TRACE;
            core::Positions smoothedPositions;
            smoothedPositions.reserve(resampledTrajectory->points().size());
            for (const auto& point : resampledTrajectory->points())
            {
                smoothedPositions.emplace_back(point.waypoint.pose.translation());
            }
 
            const std::vector<float> recomputedVelocities = velocityLimit().at(smoothedPositions);
 
            ARMARX_CHECK(recomputedVelocities.size() == resampledTrajectory->points().size());
 
            auto& mutablePoints = resampledTrajectory->mutablePoints();
            for (size_t i = 0; i < mutablePoints.size(); ++i)
            {
                mutablePoints[i].velocity = recomputedVelocities[i];
            }
        }
 
        // Compute recovery distance and find sub-trajectory start index
        size_t subTrajectoryStartIndex = 0;
        std::optional<core::GlobalTrajectory> trajectoryToOptimize;
        float recoveryDistance = 0.0f;
        {
            ScopedStageTimer timer(timings.recoveryHandling);
            const auto computeRecoveryDistance = [&]() -> float
            {
                if (!recovery.has_value())
                    return 0.0f;
                return (recovery->recoveryPosition -
                        recovery->originalStartPose.translation().head<2>())
                    .norm();
            };
            recoveryDistance = computeRecoveryDistance();
 
            // Find first point outside recovery distance (for sub-trajectory extraction)
            if (recoveryDistance > 0)
            {
                auto& pts = resampledTrajectory->mutablePoints();
                const auto startOrientation = pts.front().waypoint.pose.linear();
 
                // Fix orientations for all points within recovery distance (collision segment)
                for (size_t i = 0; i < pts.size(); i++)
                {
                    const float dist =
                        (pts[i].waypoint.pose.translation() - pts.front().waypoint.pose.translation())
                            .norm();
                    if (dist <= recoveryDistance)
                    {
                        pts[i].waypoint.pose.linear() = startOrientation;
                    }
                    else
                    {
                        // Found first point outside collision - fix it for smooth transition
                        subTrajectoryStartIndex = i;
                        if (subTrajectoryStartIndex < pts.size())
                        {
                            pts[subTrajectoryStartIndex].waypoint.pose.linear() = startOrientation;
                        }
                        break;
                    }
                }
                ARMARX_INFO << "Recovery distance: " << recoveryDistance << " mm, fixed "
                            << subTrajectoryStartIndex
                            << " collision segment points, sub-trajectory starts at index "
                            << subTrajectoryStartIndex;
            }
 
            // Extract sub-trajectory for optimization (full trajectory if no recovery)
            if (subTrajectoryStartIndex > 0)
            {
                trajectoryToOptimize = resampledTrajectory->getSubTrajectory(
                    subTrajectoryStartIndex, resampledTrajectory->points().size());
            }
            else
            {
                trajectoryToOptimize = resampledTrajectory;
            }
        }
 
        ARMARX_TRACE;
        {
            ScopedStageTimer timer(timings.loadOrientationConfig);
            const armarx::PackagePath pp("armarx_navigation",
                                         "config/global_planning/OrientationOptimizer.json");
            const std::string filename = pp.toSystemPath();
 
            ARMARX_CHECK(std::filesystem::exists(filename))
                << "OrientationOptimizer config file does not exist: " << filename;
 
            ARMARX_INFO << "Loading config from file `" << filename << "`.";
            std::ifstream ifs{filename};
 
            nlohmann::json jsonConfig;
            ifs >> jsonConfig;
 
            ARMARX_VERBOSE << "Initializing config";
 
 
            armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
 
            armarx::navigation::global_planning::arondto::OrientationOptimizerParams dto;
 
            ARMARX_VERBOSE << "reading file.";
            dto.read(reader, jsonConfig);
 
            aron_conv::fromAron(dto, params_.optimizerParams);
        }
 
        OrientationOptimizer optimizer(trajectoryToOptimize.value(), params_.optimizerParams);
        auto result = [&]() {
            ScopedStageTimer timer(timings.optimizeOrientation);
            return optimizer.optimize();
        }();
 
        if (not result)
        {
            ARMARX_ERROR << "Optimizer failure";
            return std::nullopt;
        }
 
        // Combine collision segment (fixed) with optimized sub-trajectory
        core::GlobalTrajectory finalTrajectory = result.trajectory.value();
 
        {
            ScopedStageTimer timer(timings.stitchTrajectory);
            if (recoveryDistance > 0 && subTrajectoryStartIndex > 0)
            {
                // Stitch: fixed collision segment + optimized post-collision segment
                auto& resampledPts = resampledTrajectory->mutablePoints();
                const auto& optimizedPts = result.trajectory->points();
 
                // Verify sizes match
                if (subTrajectoryStartIndex + optimizedPts.size() == resampledPts.size())
                {
                    // Copy optimized orientations and velocities to the resampled trajectory
                    for (size_t i = 0; i < optimizedPts.size(); i++)
                    {
                        resampledPts[subTrajectoryStartIndex + i].waypoint.pose.linear() =
                            optimizedPts[i].waypoint.pose.linear();
                        resampledPts[subTrajectoryStartIndex + i].velocity = optimizedPts[i].velocity;
                    }
 
                    // Use the resampled trajectory (with fixed collision segment + optimized post-collision)
                    finalTrajectory = resampledTrajectory.value();
 
                    ARMARX_INFO << "Combined fixed collision segment (" << subTrajectoryStartIndex
                                << " points) with optimized trajectory (" << optimizedPts.size()
                                << " points)";
                }
                else
                {
                    ARMARX_WARNING << "Trajectory size mismatch in collision recovery stitching. "
                                   << "Using optimized trajectory only.";
                }
            }
        }
 
        // TODO circular path smoothing should be done now
 
        // algorithm::CircularPathSmoothing smoothing;
        // auto smoothTrajectory = smoothing.smooth(result.trajectory.value());
        // smoothTrajectory.setMaxVelocity(params.linearVelocity);
 
        // Safety net: the orientation optimization and the recovery stitching above may change
        // positions or velocities, so re-apply the same limit that produced them. This only ever
        // reduces a velocity.
        if (params_.enableFinalVelocityClamp)
        {
            ScopedStageTimer timer(timings.finalVelocityClamp);
            const auto limit = velocityLimit();
 
            for (auto& point : finalTrajectory.mutablePoints())
            {
                const float permissible =
                    limit.at(Eigen::Vector2f{point.waypoint.pose.translation().head<2>()});
 
                point.velocity = std::min(permissible, point.velocity);
            }
        }
        else
        {
            ARMARX_INFO << "Final obstacle-aware velocity clamp is disabled.";
        }
 
 
        ARMARX_TRACE;
        ARMARX_DEBUG << "Terminal velocity: " << finalTrajectory.points().back().velocity;
        ARMARX_INFO << formatTimings(timings);
        return GlobalPlannerResult{.trajectory = finalTrajectory,
                                   .helperTrajectory = std::nullopt,
                                   .timings = withGridSearch(timings, planner),
                                   .gridPath = plan3d,
                                   .positionSmoothingApplied = positionSmoothingApplied};
    }
 
    std::vector<Eigen::Vector2f>
    SPFAImpl::postProcessPath(const std::vector<Eigen::Vector2f>& path)
    {
        /// chain approximation
        algorithm::ChainApproximation approx(
            path, algorithm::ChainApproximation::Params{.distanceTh = 200.F});
        approx.approximate();
        const auto p = approx.approximatedChain();
 
        // visualizePath(p, "approximated", simox::Color::green());
 
        // algo::CircularPathSmoothing smoothing;
        // const auto points = smoothing.smooth(p);
 
        return p;
    }
 
} // namespace armarx::navigation::global_planning