AStarWithOrientation.cpp

Go to the documentation of this file.

#include "AStarWithOrientation.h"
 
#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/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/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/io.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, ctx.staticScene->orientationAwareCostmap.value(), 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 std::optional<navigation::algorithms::orientation_aware::Costmap3D>& costmap,
        VirtualRobot::RobotPtr robot) :
        params(params), 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 =
            algorithms::orientation_aware::io::loadAStarWithOrientationParams();
        algorithms::orientation_aware::AStarPlanner planner{costmap.value(), aStarParams};
 
 
        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;
 
        const auto plan = planner.plan(conv::to2D(start), conv::to2D(goal));
 
        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 = {start * root_used_T_root}, .velocity = 300.F});
        trajBeforeTransform.push_back({.waypoint = {start}, .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,
            });
        }
 
        trajectory.back().velocity = 0;
        trajBeforeTransform.back().velocity = 0;
 
        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
 
        algorithms::orientation_aware::smoothing::Smoothing smoother(
            core::GlobalTrajectory{trajBeforeTransform},
            costmap.value(),
            algorithms::orientation_aware::smoothing::io::loadSmoothingParams());
        const auto optimizationResult = smoother.optimize();
        auto traj_smoothed = optimizationResult.trajectory.value();
        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;
 
        lastPointsInCollision = smoother.checkCenterTrajectory(traj_smoothed);
 
        if (lastPointsInCollision.empty())
        {
            ARMARX_INFO << "Smoothed trajectory is collision-free.";
            return GlobalPlannerResult{.trajectory = traj_smoothed_converted,
                                       .helperTrajectory = traj_smoothed};
        }
        // return GlobalPlannerResult{.trajectory = traj_smoothed_converted,
        //                            .helperTrajectory = traj_smoothed};
 
        ARMARX_WARNING
            << "Smoothed trajectory is NOT collision-free. Using original trajectory instead";
        return GlobalPlannerResult{.trajectory = trajectory,
                                   .helperTrajectory = trajBeforeTransform};
    }
 
    void
    AStarWithOrientationImpl::visualizeDebugInfo(viz::Client& vizClient,
                                                 const std::string& vizLayerNamePrefix)
    {
        // visualize points in collision
        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{100.0F, 100.0F, 100.0F})
                    .color(simox::Color::red());
            layer.add(ellipsoid);
        }
        vizClient.commit(layer);
    }
 
} // namespace armarx::navigation::global_planning