Point2Point.cpp

Go to the documentation of this file.

#include "Point2Point.h"
 
#include <algorithm>
#include <cmath>
#include <optional>
#include <vector>
 
#include <range/v3/algorithm/all_of.hpp>
 
#include <VirtualRobot/Robot.h> // IWYU pragma: keep
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/serialization/Eigen/Eigen_fdi.h>
 
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.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/Point2PointParams.aron.generated.h>
#include <armarx/navigation/global_planning/aron_conversions.h>
#include <armarx/navigation/global_planning/core.h>
 
namespace armarx::navigation::global_planning
{
 
    namespace
    {
 
        constexpr float maxWaypointSpacing = 100.F;
        constexpr int minWaypointSegments = 9;
        constexpr float minWaypointSpacing = 5.F;
 
        std::vector<core::GlobalTrajectoryPoint>
        resampleStraightLine(const core::Pose& start,
                             const core::Pose& goal,
                             float eps,
                             float velocity)
        {
            const float distance =
                (goal.translation().head<2>() - start.translation().head<2>()).norm();
 
            // A fixed spacing leaves a short line with fewer than the four waypoints TOPP-RA
            // needs for its spline, and the navigator then falls back to ramping, which pins a
            // short move to the goal velocity. Sample short lines finer instead.
            const float spacing =
                distance > 0.F
                    ? std::clamp(distance / static_cast<float>(minWaypointSegments),
                                 minWaypointSpacing,
                                 eps)
                    : eps;
 
            if (distance < spacing)
            {
                return {core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = start},
                                                    .velocity = velocity},
                        core::GlobalTrajectoryPoint{.waypoint = core::Waypoint{.pose = goal},
                                                    .velocity = velocity}};
            }
 
            const int numSegments = static_cast<int>(std::ceil(distance / spacing));
            const int numPoints = numSegments + 1;
 
            const Eigen::Quaternionf startQ(start.linear());
            const Eigen::Quaternionf goalQ(goal.linear());
 
            std::vector<core::GlobalTrajectoryPoint> points;
            points.reserve(numPoints);
 
            for (int i = 0; i < numPoints; i++)
            {
                const float t = static_cast<float>(i) / static_cast<float>(numSegments);
 
                const Eigen::Vector3f position =
                    (1.F - t) * start.translation() + t * goal.translation();
 
                const Eigen::Quaternionf orientation = startQ.slerp(t, goalQ);
 
                core::Pose pose = core::Pose::Identity();
                pose.translation() = position;
                pose.linear() = orientation.toRotationMatrix();
 
                points.push_back(core::GlobalTrajectoryPoint{
                    .waypoint = core::Waypoint{.pose = pose}, .velocity = velocity});
            }
 
            return points;
        }
 
        bool
        checkCollisions(const std::vector<core::GlobalTrajectoryPoint>& trajectory,
                        const navigation::algorithms::Costmap& costmap)
        {
            const auto isCollisionFree =
                [&costmap](const core::GlobalTrajectoryPoint& point) -> bool
            {
                const auto idx =
                    costmap.toVertex(point.waypoint.pose.translation().head<2>()).index;
                return costmap.isValid(idx);
            };
 
            return ranges::all_of(trajectory, isCollisionFree);
        }
 
    } // namespace
 
    // Point2PointParams
 
    Algorithms
    Point2PointParams::algorithm() const
    {
        return Algorithms::Point2Point;
    }
 
    aron::data::DictPtr
    Point2PointParams::toAron() const
    {
        arondto::Point2PointParams dto;
 
        Point2PointParams bo;
        aron_conv::toAron(dto, bo);
 
        return dto.toAron();
    }
 
    Point2PointParams
    Point2PointParams::FromAron(const aron::data::DictPtr& dict)
    {
        ARMARX_CHECK_NOT_NULL(dict);
 
        arondto::Point2PointParams dto;
        dto.fromAron(dict);
 
        Point2PointParams bo;
        aron_conv::fromAron(dto, bo);
 
        return bo;
    }
 
    // Point2Point
 
    Point2Point::Point2Point(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,
              ctx.robot)
    {
    }
 
    std::optional<GlobalPlannerResult>
    Point2Point::plan(const core::Pose& goal)
    {
        return plan(core::Pose(scene.robot->getGlobalPose()), goal);
    }
 
    std::optional<GlobalPlannerResult>
    Point2Point::plan(const core::Pose& start, const core::Pose& goal)
    {
        if (scene.staticScene.has_value())
        {
            impl_.updateCostmap(scene.staticScene->distanceToObstaclesCostmap);
        }
        return impl_.plan(start, goal);
    }
 
    // Point2PointImpl
 
    Point2PointImpl::Point2PointImpl(const Params& params,
                                     const core::GeneralConfig& generalConfig,
                                     const std::optional<navigation::algorithms::Costmap>& costmap,
                                     VirtualRobot::RobotPtr robot) :
        params_(params), generalConfig_(generalConfig), costmap_(costmap), robot_(robot)
    {
    }
 
    void
    Point2PointImpl::updateCostmap(const std::optional<navigation::algorithms::Costmap>& costmap)
    {
        if (costmap.has_value())
        {
            costmap_.emplace(costmap.value());
        }
        else
        {
            costmap_.reset();
        }
    }
 
    std::optional<GlobalPlannerResult>
    Point2PointImpl::plan(const core::Pose& start, const core::Pose& goal)
    {
        const auto trajectory =
            resampleStraightLine(start, goal, maxWaypointSpacing, generalConfig_.maxVel.linear);
 
        if (params_.checkCollisionsAlongTrajectory)
        {
            ARMARX_INFO << "Checking collision along trajectory";
            ARMARX_CHECK(costmap_.has_value());
 
            if (not checkCollisions(trajectory, costmap_.value()))
            {
                ARMARX_WARNING << "Planned trajectory is in collision!";
                return std::nullopt;
            }
        }
 
        return GlobalPlannerResult{.trajectory = trajectory, .helperTrajectory = std::nullopt};
    }
 
} // namespace armarx::navigation::global_planning