Trajectory.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @author     Christian R. G. Dreher ( c dot dreher at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <cstddef>
#include <memory>
#include <utility>
#include <vector>
 
#include <ArmarXCore/core/time/DateTime.h>
 
#include <armarx/navigation/core/basic_types.h>
 
namespace armarx::navigation::core
{
 
    struct GlobalTrajectoryPoint
    {
        Waypoint waypoint;
        float velocity; // [mm/s]
    };
 
    using GlobalTrajectoryPoints = std::vector<GlobalTrajectoryPoint>;
 
    struct Projection
    {
        GlobalTrajectoryPoint projection;
 
        GlobalTrajectoryPoint wayPointBefore;
        GlobalTrajectoryPoint wayPointAfter;
 
        enum class Segment
        {
            FIRST,
            INTERMEDIATE,
            FINAL
        };
 
        Segment segment;
    };
 
    enum class VelocityInterpolation
    {
        LinearInterpolation,
        LastWaypoint
    };
 
    using GlobalTrajectoryPtr = std::shared_ptr<class GlobalTrajectory>;
 
    class GlobalTrajectory
    {
    public:
        GlobalTrajectory() = default;
 
        GlobalTrajectory(const std::vector<GlobalTrajectoryPoint>& points) : pts(points)
        {
        }
 
        Projection getProjection(const Position& point,
                                 const VelocityInterpolation& velocityInterpolation) const;
 
        /**
         * @brief Project point onto the trajectory and return a new trajectory along the old one
         *        from that point for the specified distance.
         * @return The subtrajectory and whether the subtrajectory ends at the same point,
         *         as this trajectory
         */
        std::pair<GlobalTrajectory, bool> getSubTrajectory(const Position& point,
                                                           const float distance) const;
 
        /**
         * @brief Project point onto the trajectory and evaluate the remaining distance from there
         *        on.
         * @param point: current location, from which on to measure
         * @return Remaining translation along the trajectory in mm (ignoring rotations!)
         */
        float getRemainingDistance(const Position& point) const;
 
        /**
         * @brief Project point onto the trajectory and evaluate the remaining duration from there
         *        on.
         * @param point: current location, from which on to measure
         * @param linearVelocity: velocity for translations in mm/s
         * @param angularVelocity: velocity for rotations in rad/s
         * @return Remaining translation along the trajectory in s
         */
        float getRemainingDuration(const Position& point, float linearVelocity = 400,
                                   float angularVelocity = 0.5) const;
 
        [[nodiscard]] std::vector<Position> positions() const noexcept;
 
        [[nodiscard]] std::vector<Pose> poses() const noexcept;
 
        //! Note: the velocity will not be set!
        // currently, only 2D version
        // y is pointing forward
        static GlobalTrajectory FromPath(const Path& path, float velocity);
        static GlobalTrajectory FromPath(const Path& path, const std::vector<float>& velocity);
 
        //! Note: the velocity will not be set!
        // currently, only 2D version
        // y is pointing forward
        static GlobalTrajectory
        FromPath(const Pose& start, const Positions& waypoints, const Pose& goal, float velocity);
 
 
        static GlobalTrajectory FromPath(const Pose& start,
                                         const Positions& waypoints,
                                         const Pose& goal,
                                         const std::vector<float>& velocity);
 
 
        GlobalTrajectory resample(float eps) const;
 
        float length() const;
 
        void setMaxVelocity(float maxVelocity);
 
        bool hasMaxDistanceBetweenWaypoints(float maxDistance) const;
 
        const std::vector<GlobalTrajectoryPoint>& points() const;
 
        std::vector<GlobalTrajectoryPoint>& mutablePoints();
 
        float duration(core::VelocityInterpolation interpolation) const;
 
        bool isValid() const noexcept;
 
        using Indices = std::vector<std::size_t>;
 
        /**
         * @brief get all points within a certain radius that are directly connected to idx 
         * 
         * @return the list without idx
         * 
         */
        Indices allConnectedPointsInRange(std::size_t idx, float radius) const;
 
    private:
        struct InternalProjection
        {
            GlobalTrajectoryPoint projection;
            size_t indexBefore;
        };
 
        InternalProjection
        projectInternal(const Position& point,
                        const VelocityInterpolation& velocityInterpolation) const;
 
 
    private:
        std::vector<GlobalTrajectoryPoint> pts;
    };
 
    struct LocalTrajectoryPoint
    {
        DateTime timestamp;
        core::Pose pose;
    };
 
    using LocalTrajectoryPoints = std::vector<LocalTrajectoryPoint>;
 
    struct Evaluation
    {
        core::Pose pose;
        core::Twist feedforwardTwist; // [mm/s]
    };
 
    class LocalTrajectory
    {
    public:
        LocalTrajectory() = default;
 
        LocalTrajectory(const std::vector<LocalTrajectoryPoint>& points) : pts(points)
        {
        }
 
        const std::vector<LocalTrajectoryPoint>& points() const;
 
        std::vector<LocalTrajectoryPoint>& mutablePoints();
 
        Evaluation evaluate(const DateTime& timestamp) const;
 
 
    private:
        std::vector<LocalTrajectoryPoint> pts;
    };
 
 
} // namespace armarx::navigation::core