Trajectory.cpp

Go to the documentation of this file.

#include "Trajectory.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <iterator>
#include <limits>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <SimoxUtility/shapes/CircleBase.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/math/Helpers.h>
#include <VirtualRobot/math/LinearInterpolatedPose.h>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <armarx/navigation/conversions/eigen.h>
#include <armarx/navigation/core/basic_types.h>
#include <armarx/navigation/core/types.h>
 
#include <range/v3/action/insert.hpp>
#include <range/v3/algorithm/none_of.hpp>
#include <range/v3/numeric/accumulate.hpp>
#include <range/v3/range/conversion.hpp>
#include <range/v3/view/all.hpp>
#include <range/v3/view/concat.hpp>
#include <range/v3/view/for_each.hpp>
#include <range/v3/view/group_by.hpp>
#include <range/v3/view/reverse.hpp>
#include <range/v3/view/sliding.hpp>
#include <range/v3/view/transform.hpp>
#include <range/v3/view/zip.hpp>
 
// FIXME move to simox
 
namespace simox
{
    using Circlef = Circle<float>;
 
    struct Segment2D
    {
        Eigen::Vector2f start;
        Eigen::Vector2f end;
 
        Eigen::Vector2f
        d() const
        {
            return end - start;
        };
    };
 
    // TODO only infinite line implemented. need to check line segment!
    bool
    intersects(const Circlef& circle, const Segment2D& segment)
    {
        // https://mathworld.wolfram.com/Circle-LineIntersection.html
 
        const Segment2D localSegment{.start = segment.start - circle.center(),
                                     .end = segment.end - circle.center()};
 
        const float d_r = localSegment.d().norm();
        const float D = localSegment.start.x() * localSegment.end.y() - localSegment.end.x() -
                        localSegment.start.y();
 
        const auto delta =
            static_cast<float>(std::pow(circle.radius(), 2) * std::pow(d_r, 2) - std::pow(D, 2));
 
        if (delta < 0) // no intersection
        {
            return false;
        }
 
        if (delta == 0) // tangent
        {
            return true;
        }
 
        return true; // intersection
    }
 
    // TODO only infinite line implemented. need to check line segment!
    std::vector<Eigen::Vector2f>
    intersection(const Circlef& circle, const Segment2D& segment)
    {
 
        const Segment2D localSegment{.start = segment.start - circle.center(),
                                     .end = segment.end - circle.center()};
 
        const float d_r = localSegment.d().norm();
        const float D = localSegment.start.x() * localSegment.end.y() - localSegment.end.x() -
                        localSegment.start.y();
 
        const auto delta =
            static_cast<float>(std::pow(circle.radius(), 2) * std::pow(d_r, 2) - std::pow(D, 2));
 
        if (delta < 0) // no intersection
        {
            return std::vector<Eigen::Vector2f>{};
        }
 
        // Hint: + circle.center.x => local to global
 
        if (delta == 0) // tangent
        {
            std::vector<Eigen::Vector2f> v;
            v.emplace_back(D * localSegment.d().y() / std::pow(d_r, 2) + circle.center().x(),
                           -D * localSegment.d().x() / std::pow(d_r, 2) + circle.center().y());
            return v;
        }
 
        // intersection
 
        const float hx =
            std::copysign(localSegment.d().y(), localSegment.d().x()) * std::sqrt(delta);
        const float hy = std::fabs(localSegment.d().y()) * std::sqrt(delta);
 
        std::vector<Eigen::Vector2f> v;
        v.emplace_back((D * localSegment.d().y() + hx) / std::pow(d_r, 2) + circle.center().x(),
                       (-D * localSegment.d().x() + hy) / std::pow(d_r, 2) + circle.center().y());
        v.emplace_back((D * localSegment.d().y() - hx) / std::pow(d_r, 2) + circle.center().x(),
                       (-D * localSegment.d().x() - hy) / std::pow(d_r, 2) + circle.center().y());
        return v;
    }
 
} // namespace simox
 
// end fixme
 
namespace armarx::navigation::core
{
 
    namespace conv
    {
 
        GlobalTrajectoryPoint
        toTrajectoryPoint(const Pose& pose, const float velocity)
        {
            return GlobalTrajectoryPoint{.waypoint = {.pose = pose}, .velocity = velocity};
        }
 
        GlobalTrajectoryPoints
        toTrajectoryPoints(const Path& path, const float velocity)
        {
            GlobalTrajectoryPoints trajectoryPoints;
            trajectoryPoints.reserve(path.size());
 
            std::transform(path.begin(),
                           path.end(),
                           std::back_inserter(trajectoryPoints),
                           [&](const auto& p) { return toTrajectoryPoint(p, velocity); });
 
            return trajectoryPoints;
        }
 
        GlobalTrajectoryPoints
        toTrajectoryPoints(const Path& path, const std::vector<float>& velocities)
        {
            ARMARX_CHECK_EQUAL(path.size(), velocities.size());
 
            GlobalTrajectoryPoints trajectoryPoints;
            trajectoryPoints.reserve(path.size());
 
            const auto convert = [](const auto& p)
            {
                const auto& [pose, velocity] = p;
                return toTrajectoryPoint(pose, velocity);
            };
 
            return ranges::views::zip(path, velocities) | ranges::views::transform(convert) |
                   ranges::to_vector;
        }
 
    } // namespace conv
 
    GlobalTrajectory::InternalProjection
    GlobalTrajectory::projectInternal(const Position& point,
                                      const VelocityInterpolation& velocityInterpolation) const
    {
        float distance = std::numeric_limits<float>::max();
 
        InternalProjection bestProj;
 
        for (size_t i = 0; i < pts.size() - 1; i++)
        {
            const auto& wpBefore = pts.at(i);
            const auto& wpAfter = pts.at(i + 1);
 
            /*
            // FIXME remove after finding the bug
            if ((wpBefore.waypoint.pose.translation() - wpAfter.waypoint.pose.translation())
                    .norm() < 1.F)
            {
                // ARMARX_WARNING << "Trajectory segment " << i << " too small ...";
                continue;
            }
            */
 
            const auto closestPoint = VirtualRobot::MathTools::nearestPointOnSegment<Position>(
                wpBefore.waypoint.pose.translation(), wpAfter.waypoint.pose.translation(), point);
 
            const float currentDistance = (closestPoint - point).norm();
 
            // 'less equal' to accept following segment if the closest point is the waypoint
            if (currentDistance <= distance)
            {
                const float d1 = (closestPoint - wpBefore.waypoint.pose.translation()).norm();
                const float d =
                    (wpBefore.waypoint.pose.translation() - wpAfter.waypoint.pose.translation())
                        .norm();
 
                // fraction of distance between segment end points
                const float t = d1 / d;
 
                math::LinearInterpolatedPose ip(
                    wpBefore.waypoint.pose.matrix(), wpAfter.waypoint.pose.matrix(), 0, 1, true);
 
                bestProj.indexBefore = i;
                bestProj.projection.waypoint.pose = ip.Get(t);
 
                switch (velocityInterpolation)
                {
                    case VelocityInterpolation::LinearInterpolation:
                        bestProj.projection.velocity =
                            wpBefore.velocity + (wpAfter.velocity - wpBefore.velocity) * t;
                        break;
                    case VelocityInterpolation::LastWaypoint:
                        bestProj.projection.velocity = wpBefore.velocity;
                        break;
                }
 
                distance = currentDistance;
            }
        }
 
        return bestProj;
    }
 
    Projection
    GlobalTrajectory::getProjection(const Position& point,
                                    const VelocityInterpolation& velocityInterpolation) const
    {
        ARMARX_CHECK_NOT_EMPTY(pts);
 
        if (pts.size() == 1)
        {
            return Projection{.projection = pts.front(),
                              .wayPointBefore = pts.front(),
                              .wayPointAfter = pts.front(),
                              .segment = Projection::Segment::FINAL};
        }
 
        InternalProjection intProj = projectInternal(point, velocityInterpolation);
 
        const auto& wpBefore = pts.at(intProj.indexBefore);
        const auto& wpAfter = pts.at(intProj.indexBefore + 1);
 
        Projection bestProj;
 
        bestProj.wayPointBefore = wpBefore;
        bestProj.wayPointAfter = wpAfter;
        bestProj.projection = intProj.projection;
 
        bestProj.segment = [&]
        {
            // when segment is both first and final, prefer final
            if (intProj.indexBefore == (pts.size() - 2))
            {
                return Projection::Segment::FINAL;
            }
 
            if (intProj.indexBefore == 0)
            {
                return Projection::Segment::FIRST;
            }
 
            return Projection::Segment::INTERMEDIATE;
        }();
 
        return bestProj;
    }
 
    std::pair<GlobalTrajectory, bool>
    GlobalTrajectory::getSubTrajectory(const Position& point, float distance) const
    {
        const InternalProjection intProj =
            projectInternal(point, VelocityInterpolation::LinearInterpolation);
 
        GlobalTrajectory subTraj{};
        subTraj.mutablePoints().push_back(intProj.projection);
 
        float remainingDistance = distance;
        GlobalTrajectoryPoint lastWp = intProj.projection;
        for (size_t i = intProj.indexBefore + 1; i < pts.size(); i++)
        {
            const auto& nextWp = pts.at(i);
 
            const float segmentDistance =
                (nextWp.waypoint.pose.translation() - lastWp.waypoint.pose.translation()).norm();
 
            if (segmentDistance < remainingDistance)
            {
                subTraj.mutablePoints().push_back(nextWp);
                lastWp = nextWp;
                remainingDistance -= segmentDistance;
            }
            else
            {
                // fraction of distance between segment end points
                const float t = remainingDistance / segmentDistance;
 
                math::LinearInterpolatedPose ip(
                    lastWp.waypoint.pose.matrix(), nextWp.waypoint.pose.matrix(), 0, 1, true);
                const float velocity = lastWp.velocity + (nextWp.velocity - lastWp.velocity) * t;
 
                subTraj.mutablePoints().push_back({{static_cast<Pose>(ip.Get(t))}, velocity});
 
                return {subTraj, false};
            }
        }
 
        // remaining trajectory is shorter than the specified distance
        return {subTraj, true};
    }
 
    std::vector<Eigen::Vector3f>
    GlobalTrajectory::positions() const noexcept
    {
        const auto toPosition = [](const GlobalTrajectoryPoint& wp)
        { return wp.waypoint.pose.translation(); };
 
        std::vector<Eigen::Vector3f> positions;
        positions.reserve(pts.size());
 
        std::transform(pts.begin(), pts.end(), std::back_inserter(positions), toPosition);
 
        return positions;
    }
 
    std::vector<Pose>
    GlobalTrajectory::poses() const noexcept
    {
        std::vector<Pose> poses;
        poses.reserve(pts.size());
 
        std::transform(pts.begin(),
                       pts.end(),
                       std::back_inserter(poses),
                       [](const core::GlobalTrajectoryPoint& pt) -> core::Pose
                       { return pt.waypoint.pose; });
 
        return poses;
    }
 
    GlobalTrajectory
    GlobalTrajectory::FromPath(const Path& path, const float velocity)
    {
        return {conv::toTrajectoryPoints(path, velocity)};
    }
 
    GlobalTrajectory
    GlobalTrajectory::FromPath(const Path& path, const std::vector<float>& velocity)
    {
        return conv::toTrajectoryPoints(path, velocity);
    }
 
    GlobalTrajectory
    GlobalTrajectory::FromPath(const Pose& start,
                               const Positions& waypoints,
                               const Pose& goal,
                               const float velocity)
    {
        // currently, only 2D version
 
        Path path;
        path.reserve(waypoints.size());
 
        if (waypoints.empty())
        {
            return FromPath({start, goal}, velocity);
        }
 
        path.push_back(start);
 
        const auto directedPose = [](const Position& position, const Position& target) -> Pose
        {
            const Direction direction = target - position;
 
            const float yaw = math::Helpers::Angle(navigation::conv::to2D(direction));
 
            // our robot is moving into y direction ( y is forward ... )
            const Eigen::Rotation2Df pose_R_robot =
                Eigen::Rotation2Df(yaw) * Eigen::Rotation2Df(-M_PI_2f32);
 
            return navigation::conv::to3D(Eigen::Translation2f(navigation::conv::to2D(position)) *
                                          pose_R_robot);
        };
 
        // insert waypoints pointing to next waypoint
        // for (size_t i = 0; i < waypoints.size() - 1; i++)
        // {
        //     path.emplace_back(directedPose(waypoints.at(i), waypoints.at(i + 1)));
        // }
        ranges::insert(path,
                       path.end(),
                       waypoints | ranges::views::sliding(2) |
                           ranges::views::transform(
                               [&directedPose](const auto& p) -> Pose
                               {
                                   const auto& [position, _] = p;
                                   const auto target = position + 1;
 
                                   return directedPose(*position, *target);
                               }));
 
        path.emplace_back(directedPose(waypoints.back(), goal.translation()));
        path.push_back(goal);
 
        return FromPath(path, velocity);
    }
 
    GlobalTrajectory
    GlobalTrajectory::FromPath(const Pose& start,
                               const Positions& waypoints,
                               const Pose& goal,
                               const std::vector<float>& velocity)
    {
 
        // currently, only 2D version
 
        Path path;
        path.reserve(waypoints.size());
 
        if (waypoints.empty())
        {
            return FromPath({start, goal}, velocity);
        }
 
        path.push_back(start);
 
        const auto directedPose = [](const Position& position, const Position& target) -> Pose
        {
            const Direction direction = target - position;
 
            const float yaw = math::Helpers::Angle(navigation::conv::to2D(direction));
 
            // our robot is moving into y direction ( y is forward ... )
            const Eigen::Rotation2Df pose_R_robot =
                Eigen::Rotation2Df(yaw) * Eigen::Rotation2Df(-M_PI_2f32);
 
            return navigation::conv::to3D(Eigen::Translation2f(navigation::conv::to2D(position)) *
                                          pose_R_robot);
        };
 
        // insert waypoints pointing to next waypoint
        // for (size_t i = 0; i < waypoints.size() - 1; i++)
        // {
        //     path.emplace_back(directedPose(waypoints.at(i), waypoints.at(i + 1)));
        // }
        ranges::insert(path,
                       path.end(),
                       waypoints | ranges::views::sliding(2) |
                           ranges::views::transform(
                               [&directedPose](const auto& p) -> Pose
                               {
                                   const auto& [position, _] = p;
                                   const auto target = position + 1;
 
                                   return directedPose(*position, *target);
                               }));
 
        path.emplace_back(directedPose(waypoints.back(), goal.translation()));
        path.push_back(goal);
 
        return FromPath(path, velocity);
    }
 
    core::Positions
    resamplePath(const auto& pts, const float eps)
    {
        core::Positions resampledPath;
 
        const auto toPoint = [](const GlobalTrajectoryPoint& wp) -> Pose
        { return wp.waypoint.pose; };
 
        const core::Path originalPath = pts | ranges::views::transform(toPoint) | ranges::to_vector;
 
        // resampledPath.push_back(originalPath.front().translation());
 
        // the current / last added point to the resampled path
        Position pointOnPath = originalPath.front().translation();
 
        for (const auto& wp : originalPath | ranges::views::sliding(2))
        {
            const auto& [wp1, _] = wp;
            const auto wp2 = wp1 + 1;
 
            ARMARX_TRACE;
 
            ARMARX_DEBUG << "sliding ...";
            ARMARX_DEBUG << "End: " << navigation::conv::to2D(wp2->translation());
 
            const simox::Segment2D lineSegment{.start = navigation::conv::to2D(wp1->translation()),
                                               .end = navigation::conv::to2D(wp2->translation())};
 
            // easy to add points along line
            while ((navigation::conv::to2D(pointOnPath) - lineSegment.end).norm() >= eps)
            {
                const auto dir =
                    (navigation::conv::to3D(lineSegment.end) - pointOnPath).normalized();
 
                pointOnPath += dir * eps;
                resampledPath.push_back(pointOnPath);
            }
 
            // find the segment that is sufficiently far away from the current pointOnPath
            // skip those segments that are too close
            // => find the first segment that penetrates the epsilon-region
            const float distanceEnd =
                (lineSegment.end - navigation::conv::to2D(pointOnPath)).norm();
            if (distanceEnd < eps)
            {
                continue;
            }
 
            // add the intersection between the circle defining the epsilon region and the
            // line segment
 
            ARMARX_TRACE;
            const auto isect =
                intersection(simox::Circlef(navigation::conv::to2D(pointOnPath), eps), lineSegment);
 
            if (not(isect.size() == 2))
            {
                ARMARX_WARNING << "intersection failure";
                continue;
            }
 
            for (const auto& pt : isect)
            {
                ARMARX_DEBUG << "Intersection " << VAROUT(pt) << " distance "
                             << (pt - navigation::conv::to2D(pointOnPath)).norm();
            }
 
            // true intersection hypotheses
            const auto d0 = (isect.at(0) - lineSegment.start).normalized();
            // const auto d1 = isect.at(1) - lineSegment.start;
 
            const auto d = (lineSegment.end - lineSegment.start).normalized();
 
            // find out which point lies between start and end
            // here, we check if the point is into end's direction from start (cosine similarity)
            if (d0.dot(d) > 0)
            {
                pointOnPath = navigation::conv::to3D(isect.at(0));
            }
            else
            {
                pointOnPath = navigation::conv::to3D(isect.at(1));
            }
 
            resampledPath.push_back(pointOnPath);
        }
 
        // if (resampledPath.size() >= 2)
        // {
        //     // remove first "waypoint" => this is the start
        //     resampledPath.erase(resampledPath.begin());
 
        //     // remove last "waypoint" => this is the end
        //     resampledPath.pop_back();
        // }
 
        return resampledPath;
    }
 
    GlobalTrajectory
    GlobalTrajectory::resample(const float eps) const
    {
        // can only resample if at least a line segment is available
        if (pts.size() < 2)
        {
            return *this;
        }
 
        // ARMARX_CHECK_GREATER_EQUAL(pts.size(), 2);
 
        const core::Positions resampledPathForward = resamplePath(pts, eps);
        const core::Positions resampledPathBackward =
            resamplePath(pts | ranges::views::reverse, eps);
 
        if (resampledPathForward.empty() or resampledPathBackward.empty())
        {
            ARMARX_DEBUG << "The resampled path contains no points. This means that it is likely "
                            "very short.";
 
            const core::GlobalTrajectory testTrajectory = core::GlobalTrajectory::FromPath(
                pts.front().waypoint.pose, resampledPathForward, pts.back().waypoint.pose, 0.F);
 
            ARMARX_CHECK_LESS_EQUAL(testTrajectory.length(), eps)
                << "The resampled trajectory is only allowed to contains no points if it is "
                   "shorter than eps";
 
            return GlobalTrajectory({pts.front(), pts.back()});
        }
 
 
        ARMARX_DEBUG << "Resampled path forward contains " << resampledPathForward.size()
                     << " waypoints";
        ARMARX_DEBUG << "Resampled path backwards contains " << resampledPathBackward.size()
                     << " waypoints";
 
        core::Position pivot = resampledPathBackward.front();
 
        core::Positions segmentForward;
        core::Positions segmentBackward;
 
        // Find the first two elements that are close enough to each other.
        // The loop works as follows:
        // - loop over the resampled paths
        // - if an element is far enough away from the pivot element (the last element inserted into one of the segments),
        //   add it to the corresponding list
        // - if the new element is close enough to the pivot element, we are done => solution found
        for (const auto& [wpForward, wpBackward] :
             ranges::views::zip(resampledPathForward, resampledPathBackward))
        {
            segmentForward.push_back(wpForward);
            if ((pivot - wpForward).norm() < eps) // term. cond?
            {
                break;
            }
            pivot = wpForward;
 
            segmentBackward.push_back(wpBackward);
            if ((pivot - wpBackward).norm() < eps) // term. cond?
            {
                break;
            }
            pivot = wpBackward;
        }
 
        core::Positions resampledPath =
            ranges::views::concat(segmentForward, segmentBackward | ranges::views::reverse) |
            ranges::to_vector;
 
 
        auto resampledTrajectory = GlobalTrajectory::FromPath(
            pts.front().waypoint.pose, resampledPath, pts.back().waypoint.pose, 0.F);
 
        // set velocity based on original trajectory
        {
            const auto setVelocityInPlace = [&](GlobalTrajectoryPoint& pt)
            {
                const auto projection = getProjection(pt.waypoint.pose.translation(),
                                                      VelocityInterpolation::LinearInterpolation);
                pt.velocity = projection.projection.velocity;
            };
 
            std::for_each(resampledTrajectory.mutablePoints().begin(),
                          resampledTrajectory.mutablePoints().end(),
                          setVelocityInPlace);
        }
 
 
        // sanity check: make sure that resampling was successful
        // number of samples required for source trajectory
        // const float minRequiredSamples = length() / eps;
        // const size_t samples = resampledTrajectory.points().size();
 
        // ARMARX_CHECK_GREATER_EQUAL(samples, minRequiredSamples);
 
        // this is a rough approximation of the upper bound
        // ARMARX_CHECK_LESS_EQUAL(samples, 2 * minRequiredSamples);
 
        return resampledTrajectory;
    }
 
    float
    GlobalTrajectory::length() const
    {
        namespace r = ::ranges;
 
        const auto distanceBetweenWaypoints = [](const auto& p) -> float
        {
            const auto& [p1, _] = p;
            const auto p2 = p1 + 1;
            return (p1->waypoint.pose.translation() - p2->waypoint.pose.translation()).norm();
        };
 
        auto rng = pts | r::views::sliding(2) | r::views::transform(distanceBetweenWaypoints);
 
        const float length = r::accumulate(rng, 0.F);
 
        ARMARX_CHECK(std::isfinite(length));
 
        return length;
    }
 
    void
    GlobalTrajectory::setMaxVelocity(float maxVelocity)
    {
        ARMARX_CHECK_GREATER_EQUAL(maxVelocity, 0.F) << "maxVelocity must be positive!";
 
        std::for_each(pts.begin(),
                      pts.end(),
                      [&maxVelocity](GlobalTrajectoryPoint& pt)
                      { pt.velocity = std::clamp(pt.velocity, -maxVelocity, maxVelocity); });
    }
 
    bool
    GlobalTrajectory::hasMaxDistanceBetweenWaypoints(const float maxDistance) const
    {
        namespace r = ::ranges;
 
        const auto distanceBetweenWaypoints = [](const auto& p) -> float
        {
            const auto& [p1, _] = p;
            const auto p2 = p1 + 1;
            return (p1->waypoint.pose.translation() - p2->waypoint.pose.translation()).norm();
        };
 
        auto rng = pts | r::views::sliding(2) | r::views::transform(distanceBetweenWaypoints);
 
        return ranges::none_of(
            rng, [&maxDistance](const auto& dist) -> bool { return dist > maxDistance; });
    }
 
    const std::vector<GlobalTrajectoryPoint>&
    GlobalTrajectory::points() const
    {
        return pts;
    }
 
    std::vector<GlobalTrajectoryPoint>&
    GlobalTrajectory::mutablePoints()
    {
        return pts;
    }
 
    float
    GlobalTrajectory::duration(const core::VelocityInterpolation interpolation) const
    {
 
        float dur = 0;
 
        for (int i = 0; i < static_cast<int>(pts.size() - 1); i++)
        {
            const auto& start = pts.at(i);
            const auto& goal = pts.at(i + 1);
 
            const float distance =
                (goal.waypoint.pose.translation() - start.waypoint.pose.translation()).norm();
 
            const float startVelocity = start.velocity;
            const float goalVelocity = goal.velocity;
 
            constexpr int nSamples = 100;
 
            for (int j = 0; j < nSamples; j++)
            {
                const float progress = static_cast<float>(j) / nSamples;
 
                const float v = startVelocity + progress * (goalVelocity - startVelocity);
                const float s = distance / nSamples;
 
                const float t = s / v;
 
                dur += t;
            }
        }
 
        return dur;
    }
 
    bool
    GlobalTrajectory::isValid() const noexcept
    {
        const auto isValid = [](const GlobalTrajectoryPoint& pt) -> bool
        { return std::isfinite(pt.velocity) and pt.velocity < 3000; };
 
        return std::all_of(pts.begin(), pts.end(), isValid);
    }
 
    GlobalTrajectory::Indices
    GlobalTrajectory::allConnectedPointsInRange(std::size_t idx, float radius) const
    {
        const core::Position referencePosition = pts.at(idx).waypoint.pose.translation();
 
        const auto isInRange = [&](const std::size_t i) -> bool
        {
            const float posDiff =
                (pts.at(i).waypoint.pose.translation() - referencePosition).norm();
            // ARMARX_INFO << VAROUT(posDiff);
            return posDiff <= radius;
        };
 
        GlobalTrajectory::Indices indices;
 
 
        // traverse from query point to start
        for (int i = static_cast<int>(idx) - 1; i >= 0; i--)
        {
            if (isInRange(i))
            {
                indices.push_back(i);
            }
            else
            {
                // the first one outside the radius is the termination condition
                break;
            }
        }
 
        // traverse from query point to end
        for (int i = idx + 1; i < static_cast<int>(pts.size()); i++)
        {
            if (isInRange(i))
            {
                indices.push_back(i);
            }
            else
            {
                // the first one outside the radius is the termination condition
                break;
            }
        }
 
        ARMARX_DEBUG << indices.size() << " points in range";
 
        return indices;
    }
 
    const std::vector<LocalTrajectoryPoint>&
    LocalTrajectory::points() const
    {
        return pts;
    }
 
    std::vector<LocalTrajectoryPoint>&
    LocalTrajectory::mutablePoints()
    {
        return pts;
    }
 
    Evaluation
    LocalTrajectory::evaluate(const armarx::core::time::DateTime& ts) const
    {
        const auto& finalTimestamp = points().back().timestamp;
        const auto timestamp =
            ts > finalTimestamp ? finalTimestamp - Duration::MilliSeconds(1) : ts;
 
        const auto cmp = [](const core::LocalTrajectoryPoint& lhs,
                            const DateTime& timestamp) -> bool
        { return lhs.timestamp < timestamp; };
 
        const auto lower = std::lower_bound(pts.begin(), pts.end(), timestamp, cmp);
 
        if (lower == pts.end() - 1)
        {
            // if we arrived at the last point, the velocity is 0
            return {lower->pose, core::Twist::Zero()};
        }
 
        const Duration d1 = timestamp - lower->timestamp;
        const Duration dT = lower[1].timestamp - lower->timestamp;
 
        // the waypoint before this timestamp
        const auto& global_T_wp_before = lower->pose;
 
        // the waypoint after this timestamp
        const auto& global_T_wp_after = lower[1].pose;
 
        // fraction of time between segment end points
        const float t = d1 / dT;
        math::LinearInterpolatedPose ip(
            global_T_wp_before.matrix(), global_T_wp_after.matrix(), 0, 1, true);
 
        const core::Pose wp_before_T_wp_after = global_T_wp_before.inverse() * global_T_wp_after;
 
        const Eigen::Matrix3f& global_R_wp_before = global_T_wp_before.linear();
 
        // the movement direction in the global frame
        const Eigen::Vector3f global_V_linear_movement_direction =
            global_R_wp_before * wp_before_T_wp_after.translation().normalized();
        const Eigen::Vector3f distance = lower[1].pose.translation() - lower->pose.translation();
        const float linearVelocity = static_cast<float>(distance.norm()) / dT.toSecondsDouble();
 
 
        // angular velocity
 
        Eigen::AngleAxisf angleAx(wp_before_T_wp_after.linear());
 
        const core::Twist feedforwardTwist{
            .linear = linearVelocity * global_V_linear_movement_direction,
            .angular = angleAx.axis() * angleAx.angle() / dT.toSecondsDouble()};
 
        return {static_cast<core::Pose>(ip.Get(t)), feedforwardTwist};
    }
 
 
} // namespace armarx::navigation::core