SPFA.cpp

Go to the documentation of this file.

#include "SPFA.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <optional>
#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/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 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();
            }
        }
 
    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();
 
        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)
    {
        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
        {
            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();
 
        algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams = params_.algo;
 
        // Compute velocities for all positions
        // For recovery case: includes collision start (which is planner.start) and recovery position (which is wpts[0])
        const std::vector<float> velocitiesRespectingObstacles = [&]()
        {
            const float defaultVelocity = generalConfig_.maxVel.linear;
 
            core::Positions positions;
            positions.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();
 
            positions.emplace_back(startPosition);
            positions.insert(positions.end(), wpts.begin(), wpts.end());
            positions.emplace_back(goal.translation());
 
            const auto maxVelocityBasedOnObstacles = [&](const core::Position& position) -> float
            {
                const auto minDistanceToObstaclesOpt =
                    costmap_->value(Eigen::Vector2f{position.head<2>()});
 
                if (not minDistanceToObstaclesOpt.has_value())
                {
                    return defaultVelocity;
                }
 
                const float clippedObstacleDistance =
                    std::min(minDistanceToObstaclesOpt.value(), spfaParams.obstacleMaxDistance);
 
                const float ds =
                    std::pow(1.F - clippedObstacleDistance / spfaParams.obstacleMaxDistance,
                             spfaParams.obstacleCostExponent);
 
                const float obstacleFactor = 1 / (1 + spfaParams.obstacleDistanceWeight * ds);
 
                return defaultVelocity * obstacleFactor;
            };
 
            return positions | ranges::views::transform(maxVelocityBasedOnObstacles) |
                   ranges::to_vector;
        }();
 
        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 = core::GlobalTrajectory::FromPath(
            trajectoryStart, wpts, goal, velocitiesRespectingObstacles);
 
        // TODO(fabian.reister): resampling of trajectory
 
        std::optional<core::GlobalTrajectory> resampledTrajectory;
 
        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;
            return GlobalPlannerResult{.trajectory = resampledTrajectory.value(),
                                       .helperTrajectory = std::nullopt};
        }
 
        // Compute recovery distance and find sub-trajectory start index
        const auto computeRecoveryDistance = [&]() -> float
        {
            if (!recovery.has_value())
                return 0.0f;
            return (recovery->recoveryPosition -
                    recovery->originalStartPose.translation().head<2>())
                .norm();
        };
        const float recoveryDistance = computeRecoveryDistance();
 
        // Find first point outside recovery distance (for sub-trajectory extraction)
        size_t subTrajectoryStartIndex = 0;
        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)
        std::optional<core::GlobalTrajectory> trajectoryToOptimize;
        if (subTrajectoryStartIndex > 0)
        {
            trajectoryToOptimize = resampledTrajectory->getSubTrajectory(
                subTrajectoryStartIndex, resampledTrajectory->points().size());
        }
        else
        {
            trajectoryToOptimize = resampledTrajectory;
        }
 
        ARMARX_TRACE;
        {
            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 = 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();
 
        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);
 
        ARMARX_CHECK(costmap_.has_value());
        const auto& costmap = costmap_.value();
 
        for (auto& point : finalTrajectory.mutablePoints())
        {
            const float distance = std::min<float>(
                spfaParams.obstacleMaxDistance,
                costmap.value(Eigen::Vector2f{point.waypoint.pose.translation().head<2>()})
                    .value_or(0.F));
 
            if (spfaParams.obstacleDistanceCosts)
            {
                const float obstacleBasedVelocity =
                    generalConfig_.maxVel.linear /
                    (1.F + spfaParams.obstacleDistanceWeight *
                               std::pow(1 - distance / spfaParams.obstacleMaxDistance,
                                        spfaParams.obstacleCostExponent));
 
                // only reduce velocity
                point.velocity = std::min(obstacleBasedVelocity, point.velocity);
            }
        }
 
 
        ARMARX_TRACE;
        ARMARX_DEBUG << "Terminal velocity: " << finalTrajectory.points().back().velocity;
        return GlobalPlannerResult{.trajectory = finalTrajectory, .helperTrajectory = std::nullopt};
    }
 
    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