AStarWithOrientation.cpp

Go to the documentation of this file.

#include "AStarWithOrientation.h"
 
#include <algorithm>
#include <chrono>
#include <optional>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <range/v3/algorithm/transform.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/enumerate.hpp>
#include <range/v3/view/transform.hpp>
 
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
#include <VirtualRobot/XML/RobotIO.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/util/StringHelpers.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
 
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
 
#include <armarx/navigation/algorithms/orientation_aware/AStarPlanner.h>
#include <armarx/navigation/algorithms/orientation_aware/TrajectoryChecker.h>
#include <armarx/navigation/algorithms/orientation_aware/io.h>
#include <armarx/navigation/algorithms/orientation_aware/smoothing/Costmap3DWrapper.h>
#include <armarx/navigation/algorithms/orientation_aware/smoothing/Smoothing.h>
#include <armarx/navigation/algorithms/orientation_aware/smoothing/io.h>
#include <armarx/navigation/algorithms/orientation_aware/util.h>
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/aron/Trajectory.aron.generated.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/AStarWithOrientationParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron_conversions.h>
#include <armarx/navigation/global_planning/core.h>
 
namespace armarx::navigation::global_planning
{
    // AStarWithOrientationParams
 
    Algorithms
    AStarWithOrientationParams::algorithm() const
    {
        return Algorithms::AStarWithOrientation;
    }
 
    aron::data::DictPtr
    AStarWithOrientationParams::toAron() const
    {
        arondto::AStarWithOrientationParams dto;
 
        AStarWithOrientationParams bo;
        aron_conv::toAron(dto, bo);
 
        return dto.toAron();
    }
 
    AStarWithOrientationParams
    AStarWithOrientationParams::FromAron(const aron::data::DictPtr& dict)
    {
        ARMARX_CHECK_NOT_NULL(dict);
 
        arondto::AStarWithOrientationParams dto;
        dto.fromAron(dict);
 
        AStarWithOrientationParams bo;
        aron_conv::fromAron(dto, bo);
 
        return bo;
    }
 
    // AStarWithOrientation
 
    // TODO: How can we work with arbitrary robot shapes?
    //       Maybe take a 2D-shape as input and create a 5 degree sweep shape out of it
    //
    //       How to work with orientation?
    //       - One option would be to keep the robot center at the same location
    //         This way, we could make use of the precomputed costmap and check the cart borders for collisions.
    //
 
    AStarWithOrientation::AStarWithOrientation(const Params& params,
                                               const core::GeneralConfig& generalConfig,
                                               const core::Scene& ctx) :
        GlobalPlanner(generalConfig, ctx),
        impl(params,
             generalConfig,
             ctx.staticScene.has_value() && ctx.staticScene->orientationAwareCostmap.has_value()
                 ? std::make_optional(ctx.staticScene->orientationAwareCostmap.value())
                 : std::nullopt,
             ctx.robot)
    {
    }
 
    std::optional<GlobalPlannerResult>
    AStarWithOrientation::plan(const core::Pose& goal)
    {
        const core::Pose start(scene.robot->getGlobalPose());
        return plan(start, goal);
    }
 
    std::optional<GlobalPlannerResult>
    AStarWithOrientation::plan(const core::Pose& startRobotRoot, const core::Pose& goalRobotRoot)
    {
        if (scene.staticScene.has_value() && scene.staticScene->orientationAwareCostmap.has_value())
        {
            impl.updateCostmap(scene.staticScene->orientationAwareCostmap.value());
        }
        return impl.plan(startRobotRoot, goalRobotRoot);
    }
 
    void
    AStarWithOrientation::visualizeDebugInfo(viz::Client& vizClient,
                                             const std::string& vizLayerNamePrefix)
    {
        impl.visualizeDebugInfo(vizClient, vizLayerNamePrefix);
    }
 
    // AStarWithOrientationImpl
 
 
    AStarWithOrientationImpl::AStarWithOrientationImpl(
        const Params& params,
        const core::GeneralConfig& generalConfig,
        const std::optional<navigation::algorithms::orientation_aware::Costmap3D>& costmap,
        VirtualRobot::RobotPtr robot) :
        params(params), generalConfig(generalConfig), costmap(costmap), robot(robot)
    {
    }
 
    void
    AStarWithOrientationImpl::updateCostmap(
        const std::optional<navigation::algorithms::orientation_aware::Costmap3D>& costmap)
    {
        if (costmap.has_value())
        {
            this->costmap.emplace(costmap.value());
        }
        else
        {
            this->costmap.reset();
        }
    }
 
    std::optional<GlobalPlannerResult>
    AStarWithOrientationImpl::plan(const core::Pose& startRobotRoot,
                                   const core::Pose& goalRobotRoot)
    {
        ARMARX_CHECK(costmap.has_value())
            << "AStarWithOrientation cannot be executed because there is no 3D costmap available";
        const auto aStarParams =
            aStarParamsOverride.has_value()
                ? aStarParamsOverride.value()
                : algorithms::orientation_aware::io::loadAStarWithOrientationParams();
        algorithms::orientation_aware::AStarPlanner planner{costmap.value(), aStarParams};
 
        expandedNodes = 0;
        aStarSeconds = 0.0;
        smootherSeconds = 0.0;
 
 
        const Eigen::Isometry3f root_T_used_root =
            algorithms::orientation_aware::util::get_root_T_used_root(*costmap, robot);
 
        // start and goal in the costmap; consistent with the generation of the costmap
        const core::Pose start = startRobotRoot * root_T_used_root;
        const core::Pose goal = goalRobotRoot * root_T_used_root;
 
        lastRecoveryOriginalStart.reset();
        lastRecoveryPosition.reset();
 
        // If the robot is standing in collision (or within the planner's clearance zone),
        // plan from the closest position where it fits at its current orientation and
        // prepend the actual start pose afterwards, mirroring the SPFA recovery.
        core::Pose2D start2D = conv::to2D(start);
        const float startOrientationDeg = costmap->rotationDegrees(start2D);
        bool recovered = false;
 
        if (not costmap->isFreeWithClearance(
                Eigen::Vector2f{start2D.translation()}, startOrientationDeg, aStarParams.clearance))
        {
            ARMARX_WARNING << "Start position " << start2D.translation().transpose()
                           << " is in collision (within clearance=" << aStarParams.clearance
                           << " mm). Searching for recovery position within "
                           << generalConfig.inCollisionDistanceThresholdForRecovery << " mm...";
 
            const auto recoveryVertex = costmap->findClosestCollisionFreeVertex(
                start2D.translation(),
                startOrientationDeg,
                generalConfig.inCollisionDistanceThresholdForRecovery,
                aStarParams.clearance);
 
            if (not recoveryVertex.has_value())
            {
                ARMARX_ERROR << "No valid recovery position found within threshold of "
                             << generalConfig.inCollisionDistanceThresholdForRecovery << " mm";
                return {};
            }
 
            ARMARX_WARNING << "Found recovery position at "
                           << recoveryVertex->position.transpose() << " (distance: "
                           << (recoveryVertex->position - start2D.translation()).norm()
                           << " mm). Prepending original start position to trajectory.";
 
            lastRecoveryOriginalStart = start;
            lastRecoveryPosition = recoveryVertex->position;
 
            // keep the current orientation; only translate out of the collision
            start2D.translation() = recoveryVertex->position;
            recovered = true;
        }
 
        // The effective start of the planned trajectory: the recovery position (current
        // orientation kept) if the robot is in collision, the actual start otherwise.
        const core::Pose effectiveStart = conv::to3D(start2D);
 
        std::vector<core::Pose2D> plan;
        const auto aStarStart = std::chrono::steady_clock::now();
        try
        {
            plan = planner.plan(start2D, conv::to2D(goal));
            aStarSeconds =
                std::chrono::duration<double>(std::chrono::steady_clock::now() - aStarStart)
                    .count();
            expandedNodes = planner.lastExpandedNodes();
        }
        catch (...)
        {
            ARMARX_WARNING << "Could not execute orientation-aware A* from "
                           << start2D.translation().transpose() << " due to exception "
                           << GetHandledExceptionString();
            return {};
        }
 
        if (plan.empty())
        {
            ARMARX_WARNING << "Could not calculate a path with orientation_aware A*";
            return {};
        }
 
        std::vector<core::GlobalTrajectoryPoint> trajectory;
        trajectory.reserve(plan.size() + 2);
        std::vector<core::GlobalTrajectoryPoint> trajBeforeTransform;
        trajBeforeTransform.reserve(plan.size() + 2);
 
        const Eigen::Isometry3f root_used_T_root = root_T_used_root.inverse();
 
        trajectory.push_back({.waypoint = {effectiveStart * root_used_T_root}, .velocity = 300.F});
        trajBeforeTransform.push_back({.waypoint = {effectiveStart}, .velocity = 300.F});
 
        for (std::size_t i = 1; i < plan.size(); ++i)
        {
            const auto& pose2d = plan[i];
            // need to transform points back to robot's root frame
            trajectory.push_back(core::GlobalTrajectoryPoint{
                .waypoint = {conv::to3D(pose2d) * root_used_T_root},
                .velocity = 300.F,
            });
            trajBeforeTransform.push_back(core::GlobalTrajectoryPoint{
                .waypoint = {conv::to3D(pose2d)},
                .velocity = 300.F,
            });
        }
 
        // Replace the grid-snapped goal with the exact requested goal pose so the
        // smoothed trajectory terminates precisely at the target.
        trajBeforeTransform.back().waypoint.pose = goal;
        trajectory.back().waypoint.pose = goal * root_used_T_root;
 
        auto traj = core::GlobalTrajectory(trajectory);
 
        // Resampling
        //traj = traj.resample(30.F); // not working properly for orientation
        //traj = traj.calcSubsampledTrajectory(3); // working, but not really needed (smoothing is good enough without
 
        lastRawTrajectory = core::GlobalTrajectory{trajBeforeTransform};
 
        const auto smoothingParams =
            smoothingParamsOverride.has_value()
                ? smoothingParamsOverride.value()
                : algorithms::orientation_aware::smoothing::io::loadSmoothingParams();
 
        // `smoothingEnabled` is true unless an offline evaluation turned it off, so the deployed
        // path is unchanged: smooth, then collision-check with fallback to the raw A* trajectory.
        core::GlobalTrajectory traj_smoothed = lastRawTrajectory;
        if (smoothingEnabled)
        {
            const auto smootherStart = std::chrono::steady_clock::now();
 
            algorithms::orientation_aware::smoothing::Smoothing smoother(
                core::GlobalTrajectory{trajBeforeTransform},
                costmap.value(),
                smoothingParams);
            const auto optimizationResult = smoother.optimize();
 
            smootherSeconds =
                std::chrono::duration<double>(std::chrono::steady_clock::now() - smootherStart)
                    .count();
 
            traj_smoothed = optimizationResult.trajectory.value();
            lastPreprocessedTrajectory = optimizationResult.preprocessedTrajectory.value();
        }
        else
        {
            lastPreprocessedTrajectory = lastRawTrajectory;
        }
        lastSmoothedTrajectory = traj_smoothed;
 
        auto traj_smoothed_converted = traj_smoothed.mutablePoints() |
                                       ranges::views::transform(
                                           [root_used_T_root](const core::GlobalTrajectoryPoint& pt)
                                           {
                                               auto pt2 = pt;
                                               pt2.waypoint.pose =
                                                   pt2.waypoint.pose * root_used_T_root;
                                               return pt2;
                                           }) |
                                       ranges::to_vector;
 
        // Collision check with fallback to original A* trajectory
        algorithms::orientation_aware::TrajectoryChecker checker(costmap.value());
        auto checkResult = checker.check(traj_smoothed, /*logDetails=*/true);
        lastPointsInCollision = checkResult.collisionPoints;
 
        core::GlobalTrajectory traj_to_use;
        core::GlobalTrajectory traj_converted;
        if (checkResult.collisionCount > 2 && !smoothingParams.always_return_smoothed)
        {
            ARMARX_WARNING << "Smoothed trajectory has " << checkResult.collisionCount
                           << " collision points; falling back to original A* trajectory.";
            traj_to_use = lastRawTrajectory;
            traj_converted = lastRawTrajectory.mutablePoints() |
                             ranges::views::transform(
                                 [root_used_T_root](const core::GlobalTrajectoryPoint& pt)
                                 {
                                     auto pt2 = pt;
                                     pt2.waypoint.pose =
                                         pt2.waypoint.pose * root_used_T_root;
                                     return pt2;
                                 }) |
                             ranges::to_vector;
        }
        else
        {
            if (!checkResult.isCollisionFree())
            {
                if (smoothingParams.always_return_smoothed)
                {
                    ARMARX_WARNING << "Smoothed trajectory has " << checkResult.collisionCount
                                   << " collision point(s); returning smoothed anyway because "
                                      "alwaysReturnSmoothed is enabled.";
                }
                else
                {
                    ARMARX_WARNING << "Smoothed trajectory has " << checkResult.collisionCount
                                   << " minor collision(s); returning smoothed anyway.";
                }
            }
            else
            {
                ARMARX_INFO << "Smoothed trajectory is collision-free.";
            }
            traj_to_use = traj_smoothed;
            traj_converted = traj_smoothed_converted;
        }
 
        if (recovered)
        {
            // Prepend the original (in-collision) start pose so the first trajectory segment
            // moves the robot out of the collision. Keep the escape segment slow while the
            // robot is (potentially) in contact.
            constexpr float maxRecoveryVelocity = 150.F; // [mm/s]
 
            auto& trajectoryPoints = traj_converted.mutablePoints();
            const float recoveryVelocity =
                std::min(trajectoryPoints.front().velocity, maxRecoveryVelocity);
            trajectoryPoints.front().velocity = recoveryVelocity;
            trajectoryPoints.insert(trajectoryPoints.begin(),
                                    core::GlobalTrajectoryPoint{.waypoint = {startRobotRoot},
                                                                .velocity = recoveryVelocity});
 
            auto& helperPoints = traj_to_use.mutablePoints();
            helperPoints.front().velocity = recoveryVelocity;
            helperPoints.insert(helperPoints.begin(),
                                core::GlobalTrajectoryPoint{.waypoint = {start},
                                                            .velocity = recoveryVelocity});
 
            // also show the escape segment in the debug visualization
            auto& rawPoints = lastRawTrajectory.mutablePoints();
            rawPoints.insert(rawPoints.begin(),
                             core::GlobalTrajectoryPoint{.waypoint = {start},
                                                         .velocity = recoveryVelocity});
        }
 
        return GlobalPlannerResult{.trajectory = traj_converted,
                                   .helperTrajectory = traj_to_use};
    }
 
    void
    AStarWithOrientationImpl::visualizeDebugInfo(viz::Client& vizClient,
                                                 const std::string& vizLayerNamePrefix)
    {
        // 1) Raw A* trajectory (yellow)
        {
            auto layer = vizClient.layer(vizLayerNamePrefix + "_raw");
            layer.clear();
            layer.add(viz::Path("path")
                          .points(lastRawTrajectory.positions())
                          .color(simox::Color::yellow()));
            for (const auto& [idx, tp] : lastRawTrajectory.points() | ranges::views::enumerate)
            {
                const Eigen::Vector3f target =
                    100.0F * tp.waypoint.pose.linear() * Eigen::Vector3f::UnitY();
                layer.add(viz::Arrow("theta_" + std::to_string(idx))
                              .fromTo(tp.waypoint.pose.translation(),
                                      tp.waypoint.pose.translation() + target)
                              .color(simox::Color::yellow()));
            }
            vizClient.commit(layer);
        }
 
        // 2) Pre-processed trajectory (green)
        {
            auto layer = vizClient.layer(vizLayerNamePrefix + "_preprocessed");
            layer.clear();
            layer.add(viz::Path("path")
                          .points(lastPreprocessedTrajectory.positions())
                          .color(simox::Color::green()));
            for (const auto& [idx, tp] : lastPreprocessedTrajectory.points()
                                            | ranges::views::enumerate)
            {
                const Eigen::Vector3f target =
                    100.0F * tp.waypoint.pose.linear() * Eigen::Vector3f::UnitY();
                layer.add(viz::Arrow("theta_" + std::to_string(idx))
                              .fromTo(tp.waypoint.pose.translation(),
                                      tp.waypoint.pose.translation() + target)
                              .color(simox::Color::green()));
            }
            vizClient.commit(layer);
        }
 
        // 3) Smoothed trajectory with clearance-based coloring
        {
            auto layer = vizClient.layer(vizLayerNamePrefix + "_smoothed");
            layer.clear();
 
            if (costmap.has_value())
            {
                algorithms::orientation_aware::smoothing::Costmap3DWrapper wrapper(
                    costmap.value());
                std::vector<simox::color::Color> colors;
                colors.reserve(lastSmoothedTrajectory.points().size());
                for (const auto& tp : lastSmoothedTrajectory.points())
                {
                    double x = tp.waypoint.pose.translation().x();
                    double y = tp.waypoint.pose.translation().y();
                    double theta = std::atan2(tp.waypoint.pose.linear()(1, 0),
                                              tp.waypoint.pose.linear()(0, 0));
                    double d_raw;
                    wrapper(&x, &y, &theta, &d_raw);
                    if (d_raw < 50.0)
                        colors.push_back(simox::Color::red());
                    else if (d_raw < 200.0)
                        colors.push_back(simox::Color::yellow());
                    else
                        colors.push_back(simox::Color::green());
                }
 
                // Draw path segments with per-point colors
                const auto positions = lastSmoothedTrajectory.positions();
                for (std::size_t i = 0; i + 1 < positions.size(); ++i)
                {
                    std::vector<Eigen::Vector3f> seg = {positions[i], positions[i + 1]};
                    // Use the more conservative color of the two endpoints
                    auto color = (colors[i] == simox::Color::red() || colors[i + 1] == simox::Color::red())
                                     ? simox::Color::red()
                                 : (colors[i] == simox::Color::yellow()
                                    || colors[i + 1] == simox::Color::yellow())
                                     ? simox::Color::yellow()
                                     : simox::Color::green();
                    layer.add(viz::Path("segment_" + std::to_string(i)).points(seg).color(color));
                }
 
                for (const auto& [idx, tp] : lastSmoothedTrajectory.points()
                                                | ranges::views::enumerate)
                {
                    const Eigen::Vector3f target =
                        100.0F * tp.waypoint.pose.linear() * Eigen::Vector3f::UnitY();
                    layer.add(viz::Arrow("theta_" + std::to_string(idx))
                                  .fromTo(tp.waypoint.pose.translation(),
                                          tp.waypoint.pose.translation() + target)
                                  .color(colors[idx]));
                }
            }
            else
            {
                layer.add(viz::Path("path")
                              .points(lastSmoothedTrajectory.positions())
                              .color(simox::Color::blue()));
            }
            vizClient.commit(layer);
        }
 
        // 4) Collision recovery: original (in-collision) start and escape target
        {
            auto layer = vizClient.layer(vizLayerNamePrefix + "_recovery");
            layer.clear();
            if (lastRecoveryOriginalStart.has_value() && lastRecoveryPosition.has_value())
            {
                const Eigen::Vector3f from = lastRecoveryOriginalStart->translation();
                const Eigen::Vector3f to = conv::to3D(lastRecoveryPosition.value());
                layer.add(viz::Ellipsoid("original_start")
                              .pose(Eigen::Isometry3f{Eigen::Translation3f{from}}.matrix())
                              .axisLengths(Eigen::Vector3f{30.F, 30.F, 30.F})
                              .color(simox::Color::red()));
                layer.add(viz::Arrow("to_recovery_position")
                              .fromTo(from, to)
                              .color(simox::Color::red()));
            }
            vizClient.commit(layer);
        }
 
        // 5) Existing collision ellipsoids
        {
            auto layer = vizClient.layer(vizLayerNamePrefix + "_points_in_collision");
            layer.clear();
            for (std::size_t i = 0; i < lastPointsInCollision.size(); ++i)
            {
                const auto& p = lastPointsInCollision[i];
                std::stringstream ss;
                ss << "point_in_collision_" << i;
                viz::Ellipsoid ellipsoid =
                    viz::Ellipsoid(ss.str())
                        .pose(
                            Eigen::Isometry3f{Eigen::Translation3f{p.waypoint.pose.translation()}}
                                .matrix())
                        .axisLengths(Eigen::Vector3f{3.0F, 3.0F, 3.0F})
                        .color(simox::Color::red());
                layer.add(ellipsoid);
            }
            vizClient.commit(layer);
        }
    }
 
} // namespace armarx::navigation::global_planning