Trajectory.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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/>.
 *
 * @package    ArmarX
 * @author     Mirko Waechter( mirko.waechter at kit dot edu)
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <boost/multi_index/hashed_index.hpp>
#include <boost/multi_index/member.hpp>
#include <boost/multi_index/ordered_index.hpp>
#include <boost/multi_index/random_access_index.hpp>
#include <boost/multi_index_container.hpp>
 
#include <Eigen/Core>
 
#include <ArmarXCore/core/exceptions/Exception.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/interface/core/Trajectory.h>
 
namespace armarx::VariantType
{
    // Variant types
    const VariantTypeId Trajectory = Variant::addTypeName("::armarx::TrajectoryBase");
} // namespace armarx::VariantType
 
namespace armarx
{
    using DoubleSeqPtr = std::shared_ptr<Ice::DoubleSeq>;
 
    class Trajectory;
    using TrajectoryPtr = IceInternal::Handle<Trajectory>;
 
    /**
     * @class Trajectory
     * @brief The Trajectory class represents n-dimensional sampled trajectories.
     * @ingroup VariantsGrp
     *
     * The class interpolates all missing values with linear interpolation and calculates derivations.
     * This means trajectories with different timestamps can easily be used.
     * Velocities or accelerations can easily be queried with the *derivation* parameters of many functions.
     * Access complexity for a query with a timestamp is like from an unordered_map.
     *
     * \image html tut-img-traj-1.svg "Internal structure of the Trajectory class"
     *
     * Basic usage:
     * @snippet RobotAPI/libraries/core/test/TrajectoryTest.cpp TrajectoryDocumentation BasicUsage
     *
     * The trajectory offers normal iterator capabilities:
     * @snippet RobotAPI/libraries/core/test/TrajectoryTest.cpp TrajectoryDocumentation IteratorUsage
     * @note Only const iterators are currently available.
     *
     * With the iterators it can be used like a std container:
     * @snippet RobotAPI/libraries/core/test/TrajectoryTest.cpp TrajectoryDocumentation CopyIf
     */
    class Trajectory : public TrajectoryBase
    {
    protected:
        ~Trajectory() override;
 
    public:
        using TrajMap = std::map<double, std::vector<DoubleSeqPtr>>;
 
        struct TrajData
        {
            TrajData()
            {
            }
 
            TrajData(Trajectory* traj);
            DoubleSeqPtr operator[](size_t dim) const;
 
            inline TrajData&
            operator=(const TrajData& s)
            {
                timestamp = s.timestamp;
                data.resize(s.data.size());
                int i = 0;
                for (auto& elem : s.data)
                {
                    data[i++].reset(new Ice::DoubleSeq(*elem));
                }
                trajectory = nullptr;
                return *this;
            }
 
            double getTimestamp() const;
            double getPosition(size_t dim) const;
 
            template <class T = double>
            std::vector<T>
            getPositionsAs() const
            {
                if (!trajectory)
                {
                    throw LocalException("Ptr to trajectory is NULL");
                }
                auto numDim = trajectory->dim();
                std::vector<T> result;
                result.reserve(numDim);
                for (std::size_t i = 0; i < numDim; ++i)
                {
                    result.emplace_back(getPosition(i));
                }
                return result;
            }
 
            std::vector<double>
            getPositions() const
            {
                return getPositionsAs<double>();
            }
 
            Eigen::VectorXf getPositionsAsVectorXf() const;
            Eigen::VectorXd getPositionsAsVectorXd() const;
 
            template <class T>
            void
            writePositionsToNameValueMap(std::map<std::string, T>& map) const
            {
                if (!trajectory)
                {
                    throw LocalException("Ptr to trajectory is NULL");
                }
                auto numDim = trajectory->dim();
                for (std::size_t i = 0; i < numDim; ++i)
                {
                    map[trajectory->getDimensionName(i)] = getPosition(i);
                }
            }
 
            template <class T>
            std::map<std::string, T>
            getPositionsAsNameValueMap() const
            {
                std::map<std::string, T> result;
                writePositionsToNameValueMap(result);
                return result;
            }
 
            double getDeriv(size_t dim, size_t derivation) const;
            const std::vector<DoubleSeqPtr>& getData() const;
 
            friend std::ostream&
            operator<<(std::ostream& stream, const TrajData& rhs)
            {
                stream << rhs.timestamp << ": ";
                for (size_t d = 0; d < rhs.data.size(); ++d)
                {
                    stream << (rhs.data[d] && rhs.data[d]->size() > 0
                                   ? to_string(rhs.data[d]->at(0))
                                   : std::string("-"));
                    if (d <= rhs.data.size() - 1)
                    {
                        stream << ", ";
                    }
                }
                return stream;
            }
 
            double timestamp;
            mutable std::vector<DoubleSeqPtr> data; // mutable so that it can be changed
        protected:
            Trajectory* trajectory = nullptr;
            //            friend class Trajectory;
        };
 
        // structs for indices
        struct TagTimestamp
        {
        };
 
        struct TagOrdered
        {
        };
 
        // Trajectory data container
        typedef boost::multi_index::multi_index_container<
            TrajData,
            boost::multi_index::indexed_by<
                boost::multi_index::hashed_unique<
                    boost::multi_index::tag<TagTimestamp>,
                    boost::multi_index::member<TrajData, double, &TrajData::timestamp>>,
                boost::multi_index::ordered_unique<
                    boost::multi_index::tag<TagOrdered>,
                    boost::multi_index::member<
                        TrajData,
                        double,
                        &TrajData::timestamp>> // this index represents timestamp incremental order
                >>
            TrajDataContainer;
        using timestamp_view =
            typename boost::multi_index::index<TrajDataContainer, TagTimestamp>::type;
        using ordered_view =
            typename boost::multi_index::index<TrajDataContainer, TagOrdered>::type;
 
        Trajectory()
        {
        }
 
        Trajectory(const Trajectory& source);
 
        template <typename T>
        Trajectory(const std::vector<T>& data,
                   const Ice::DoubleSeq timestamps = Ice::DoubleSeq(),
                   const std::string& dimensionName = "",
                   typename std::enable_if_t<std::is_arithmetic_v<T>>* t = 0)
        {
            if (data.size() == 0)
            {
                return;
            }
            Ice::DoubleSeq dvec(data.begin(), data.end());
            addDimension(dvec, timestamps, dimensionName);
        }
 
        /**
         * Constructor to add n-dimensions with m-values.
         * @param data data vector (size n) with positions (size m). Dimension 1 is the joint dimension and Dimension 2 is the position of that joint.
         * @param timestamps timestamps of the trajectory. If not empty, need to be of same size as the inner values of data.
         * If empty, timestamps from 0..1 are generated with a stepsize aligned with the size of the inner vector of data.
         *
         */
        template <typename T>
        Trajectory(const std::vector<std::vector<T>>& data,
                   const Ice::DoubleSeq timestamps = Ice::DoubleSeq(),
                   const Ice::StringSeq& dimensionNames = {})
        {
            if (data.size() == 0)
            {
                return;
            }
            const auto& tempTimestamps =
                timestamps.size() > 0 ? timestamps : GenerateTimestamps(data.at(0));
            size_t i = 0;
            for (const auto& subvec : data)
            {
                Ice::DoubleSeq dvec(subvec.begin(), subvec.end());
                addDimension(
                    dvec, tempTimestamps, i < dimensionNames.size() ? dimensionNames.at(i++) : "");
            }
        }
 
        template <typename T>
        Trajectory(const std::vector<std::vector<std::vector<T>>>& data,
                   const Ice::DoubleSeq timestamps = Ice::DoubleSeq(),
                   const Ice::StringSeq& dimensionNames = {})
        {
            this->dataVec = data;
            this->timestamps = timestamps;
            this->dimensionNames = dimensionNames;
 
            ice_postUnmarshal();
        }
 
        Trajectory(const std::map<double, Ice::DoubleSeq>& data);
 
 
        Trajectory& operator=(const Trajectory& source);
 
        // Basic Manipulators
        size_t addDimension(const Ice::DoubleSeq& values,
                            const Ice::DoubleSeq& timestamps = Ice::DoubleSeq(),
                            const std::string name = "");
        void addPositionsToDimension(size_t dimension,
                                     const Ice::DoubleSeq& values,
                                     const Ice::DoubleSeq& timestamps);
        void
        addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq& derivs);
        Trajectory& operator+=(const Trajectory traj);
        void removeDimension(size_t dimension);
        void removeDerivation(size_t derivation);
        void removeDerivation(size_t dimension, size_t derivation);
 
        // iterators
        /**
         * @brief Iterators that iterates in incremental order of the timestamps through the trajectory.
         * @note Only const versions are available currently.
         * @see end(), rbegin(), rend()
         */
        typename ordered_view::const_iterator begin() const;
        typename ordered_view::const_iterator end() const;
        typename ordered_view::const_reverse_iterator rbegin() const;
        typename ordered_view::const_reverse_iterator rend() const;
 
        ///// getters
        std::vector<DoubleSeqPtr>& operator[](double timestamp);
        Ice::DoubleSeq getTimestamps() const;
        Ice::FloatSeq getTimestampsFloat() const;
        std::vector<DoubleSeqPtr>& getStates(double t);
        std::vector<DoubleSeqPtr> getStates(double t) const;
        Ice::DoubleSeq getStates(double t, size_t derivation) const;
 
        template <typename T>
        std::map<std::string, T>
        getStatesMap(double t, size_t derivation = 0) const
        {
            std::map<std::string, T> result;
            size_t dimensions = dim();
 
            for (size_t dim = 0; dim < dimensions; dim++)
            {
                result[getDimensionName(dim)] = getState(t, dim, derivation);
            }
 
            return result;
        }
 
        double getState(double t, size_t dim = 0, size_t derivation = 0);
        double getState(double t, size_t dim = 0, size_t derivation = 0) const;
        std::vector<Ice::DoubleSeq> getAllStates(double t, int maxDeriv = 1);
        Ice::DoubleSeq getDerivations(double t, size_t dimension, size_t derivations) const;
        std::string getDimensionName(size_t dim) const;
        const Ice::StringSeq& getDimensionNames() const;
 
        TrajDataContainer& data();
 
        /**
         * @brief getDimensionData gets all entries for one dimensions with order of increasing timestamps
         * @param dimension
         * @param derivation
         * @return
         */
        Ice::DoubleSeq getDimensionData(size_t dimension, size_t derivation = 0) const;
        Eigen::VectorXd getDimensionDataAsEigen(size_t dimension, size_t derivation) const;
        Eigen::VectorXd getDimensionDataAsEigen(size_t dimension,
                                                size_t derivation,
                                                double startTime,
                                                double endTime) const;
        Eigen::MatrixXd toEigen(size_t derivation, double startTime, double endTime) const;
        Eigen::MatrixXd toEigen(size_t derivation = 0) const;
        TrajectoryPtr
        getPart(double startTime, double endTime, size_t numberOfDerivations = 0) const;
        /**
         * @brief dim returns the trajectory dimension count for this trajectory (e.g. taskspace w/o orientation would be 3)
         * @return
         */
        size_t dim() const;
 
        /**
         * @brief Returns number of data entries (i.e. positions) in the trajectory.
         */
        size_t size() const;
 
        /**
         * @brief Difference between biggest and smallest timestamp. If size() <=1 -> return 0.
         */
        double getTimeLength() const;
 
        /**
         * @brief Returns the sum of a all subsequent distances of the entries in the trajectories over all dimensions
         * @param derivation Derivation for which the length is calculated
         */
        double getLength(size_t derivation = 0) const;
        double getLength(size_t derivation, double startTime, double endTime) const;
        double getLength(size_t dimension, size_t derivation) const;
        double
        getLength(size_t dimension, size_t derivation, double startTime, double endTime) const;
        double getSquaredLength(size_t dimension, size_t derivation) const;
        double getSquaredLength(size_t dimension,
                                size_t derivation,
                                double startTime,
                                double endTime) const;
        double getMax(size_t dimension, size_t derivation, double startTime, double endTime) const;
        double getMin(size_t dimension, size_t derivation, double startTime, double endTime) const;
        double getWithFunc(const double& (*foo)(const double&, const double&),
                           double initValue,
                           size_t dimension,
                           size_t derivation,
                           double startTime,
                           double endTime) const;
        double
        getAmplitude(size_t dimension, size_t derivation, double startTime, double endTime) const;
 
        /**
         * @brief Calculate *all* minima.
         * @param dimension
         * @param derivation
         * @param startTime
         * @param endTime
         * @return
         */
        Ice::DoubleSeq
        getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const;
        Ice::DoubleSeq
        getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const;
        Ice::DoubleSeq getMinimaTimestamps(size_t dimension,
                                           size_t derivation,
                                           double startTime,
                                           double endTime) const;
        Ice::DoubleSeq getMaximaTimestamps(size_t dimension,
                                           size_t derivation,
                                           double startTime,
                                           double endTime) const;
 
        // calculations
 
        Ice::DoubleSeq getDiscreteDifferentiationForDim(size_t trajDimension,
                                                        size_t derivation) const;
        static Ice::DoubleSeq DifferentiateDiscretly(const Ice::DoubleSeq& timestamps,
                                                     const Ice::DoubleSeq& values,
                                                     int derivationCount = 1);
        double
        getDiscretDifferentiationForDimAtT(double t, size_t trajDimension, size_t derivation) const;
 
        void differentiateDiscretlyForDim(size_t trajDimension, size_t derivation);
        void differentiateDiscretly(size_t derivation);
        void reconstructFromDerivativeForDim(double valueAtFirstTimestamp,
                                             size_t trajDimension,
                                             size_t sourceDimOfSystemState,
                                             size_t targetDimOfSystemState);
 
        /**
         * @brief negateDim changes the sign of all values of the given dimension.
         */
        void negateDim(size_t trajDimension);
 
        /**
         * @brief gaussianFilter smoothes the trajectory
         * @param filterRadius, in easy words: time range to left and right from center that
         * influences the resulting center value
         */
        void gaussianFilter(double filterRadius);
 
        static Trajectory NormalizeTimestamps(const Trajectory& traj,
                                              const double startTime = 0.0,
                                              const double endTime = 1.0);
        TrajectoryPtr normalize(const double startTime = 0.0, const double endTime = 1.0);
        TrajectoryPtr calculateTimeOptimalTrajectory(double maxVelocity,
                                                     double maxAcceleration,
                                                     double maxDeviation,
                                                     IceUtil::Time const& timestep);
        TrajectoryPtr calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities,
                                                     const Eigen::VectorXd& maxAccelerations,
                                                     double maxDeviation,
                                                     IceUtil::Time const& timestep);
        static Ice::DoubleSeq NormalizeTimestamps(const Ice::DoubleSeq& timestamps,
                                                  const double startTime = 0.0,
                                                  const double endTime = 1.0);
        static Ice::DoubleSeq
        GenerateTimestamps(double startTime = 0.0, double endTime = 1.0, double stepSize = 0.001);
 
        template <typename T>
        static Ice::DoubleSeq
        GenerateTimestamps(const std::vector<T>& values)
        {
            return GenerateTimestamps(0, 1, 1.0 / (values.size() - 1));
        }
 
        void shiftTime(double shift);
        void shiftValue(const Ice::DoubleSeq& shift);
        void scaleValue(const Ice::DoubleSeq& factor);
 
 
        static void CopyData(const Trajectory& source, Trajectory& destination);
        static void CopyMetaData(const Trajectory& source, Trajectory& destination);
 
        void clear(bool keepMetaData = false);
        void setPositionEntry(const double t, const size_t dimIndex, const double& y);
        void setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq& y);
        bool dataExists(double t, size_t dimension = 0, size_t derivation = 0) const;
 
 
        std::map<double, Ice::DoubleSeq>
        getStatesAround(double t, size_t derivation, size_t extend) const;
 
 
        // Object interface
        void ice_preMarshal() override;
        void ice_postUnmarshal() override;
 
        // Serializable interface
        void serialize(const ObjectSerializerBasePtr& obj,
                       const Ice::Current& = Ice::emptyCurrent) const override;
        void deserialize(const ObjectSerializerBasePtr&,
                         const Ice::Current& = Ice::emptyCurrent) override;
 
        // VariantDataClass interface
        VariantDataClassPtr clone(const Ice::Current& c = Ice::emptyCurrent) const override;
        TrajectoryPtr cloneMetaData() const;
        std::string output(const Ice::Current& c = Ice::emptyCurrent) const override;
        Ice::Int getType(const Ice::Current& c = Ice::emptyCurrent) const override;
        bool validate(const Ice::Current& c = Ice::emptyCurrent) override;
 
        Ice::ObjectPtr ice_clone() const override;
 
        void
        setDimensionNames(const Ice::StringSeq dimNames)
        {
            dimensionNames = dimNames;
        }
 
        void setLimitless(const LimitlessStateSeq& limitlessStates);
        LimitlessStateSeq getLimitless() const;
 
    protected:
        void __addDimension();
        void __fillAllEmptyFields();
        double __interpolate(typename ordered_view::const_iterator itMap,
                             size_t dimension,
                             size_t derivation) const;
        double __gaussianFilter(double filterRadius,
                                typename ordered_view::iterator centerIt,
                                size_t trajNum,
                                size_t dim);
        timestamp_view::iterator __fillBaseDataAtTimestamp(const double& t);
        std::vector<DoubleSeqPtr> __calcBaseDataAtTimestamp(const double& t) const;
 
 
        TrajDataContainer dataMap;
 
        virtual double __interpolate(double t,
                                     ordered_view::const_iterator itPrev,
                                     ordered_view::const_iterator itNext,
                                     size_t dimension,
                                     size_t derivation) const;
        double __interpolate(double t, size_t dimension, size_t derivation) const;
    };
 
} // namespace armarx