Trajectory.cpp

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
 */
#include "Trajectory.h"
#include <ArmarXCore/core/logging/Logging.h>
#include "TrajectoryController.h"
#include "VectorHelpers.h"
#include <ArmarXCore/observers/AbstractObjectSerializer.h>
#include "math/MathUtils.h"
#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
 
#include <memory>
#include <cmath>
 
namespace armarx
{
 
    Trajectory::~Trajectory()
        = default;
 
    void Trajectory::ice_preMarshal()
    {
        dataVec.clear();
        timestamps.clear();
        dataVec.reserve(dim());
        for (const auto& it : *this)
        {
            std::vector< DoubleSeqPtr >& data = it.data;
            DoubleSeqSeq new2DVec;
            new2DVec.reserve(data.size());
            for (DoubleSeqPtr& subVec : data)
            {
                if (subVec)
                {
                    new2DVec.emplace_back(*subVec);
                }
                else
                {
                    new2DVec.emplace_back(Ice::DoubleSeq());
                }
            }
            dataVec.emplace_back(new2DVec);
        }
        timestamps = getTimestamps();
    }
 
    void Trajectory::ice_postUnmarshal()
    {
        int i = 0;
        dataMap.clear();
        for (DoubleSeqSeq& _2DVec : dataVec)
        {
 
            double t = timestamps.at(i);
            int d = 0;
            for (auto& vec : _2DVec)
            {
                setEntries(t, d++, vec);
            }
 
            i++;
        }
    }
 
    void Trajectory::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
        Ice::StringSeq columnContent;
        Eigen::MatrixXd m = toEigen();
        auto cols = m.cols();
        auto rows = m.rows();
        for (int col = 0; col < cols; col++)
        {
            std::stringstream ss;
 
            for (int row = 0; row < rows; row++)
            {
                ss << m(row, col) << (row < rows - 1 ? "," : "");
            }
 
            columnContent.push_back(ss.str());
        }
        obj->setDoubleArray("timestamps", getTimestamps());
        obj->setStringArray("dimensionData", columnContent);
        obj->setStringArray("dimensionNames", dimensionNames);
    }
 
    void Trajectory::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
    {
        clear();
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
        Ice::StringSeq rowContent;
        obj->getStringArray("dimensionData", rowContent);
 
        Ice::DoubleSeq timestamps;
        obj->getDoubleArray("timestamps", timestamps);
        for (const auto& row : rowContent)
        {
            Ice::StringSeq values = Split(row, ",");
            Ice::DoubleSeq newRow;
            newRow.reserve(values.size());
            for (std::string v : values)
            {
                newRow.push_back(atof(v.c_str()));
            }
            addDimension(newRow, timestamps);
        }
        obj->getStringArray("dimensionNames", dimensionNames);
    }
 
    VariantDataClassPtr Trajectory::clone(const Ice::Current&) const
    {
        return new Trajectory(*this);
    }
 
    TrajectoryPtr Trajectory::cloneMetaData() const
    {
        TrajectoryPtr t = new Trajectory();
        CopyMetaData(*this, *t);
        return t;
    }
 
    std::string Trajectory::output(const Ice::Current&) const
    {
 
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        typename ordered_view::const_iterator itMap = ordv.begin();
        std::stringstream s;
        s << "Dimensions names: \n";
        for (size_t i = 0; i < dimensionNames.size(); i++)
        {
            s << dimensionNames.at(i) << (i < limitless.size() && limitless.at(i).enabled ? " Limitless " : "") << "\n";
        }
 
        for (; itMap != ordv.end(); itMap++)
        {
            s << *itMap << std::endl;
        }
        return s.str();
    }
 
    Ice::Int Trajectory::getType(const Ice::Current&) const
    {
        return VariantType::Trajectory;
    }
 
    bool Trajectory::validate(const Ice::Current&)
    {
        return true;
    }
 
    Ice::ObjectPtr Trajectory::ice_clone() const
    {
        return clone();
    }
 
    Trajectory::Trajectory(const Trajectory& source) :
        IceUtil::Shared(source),
        armarx::Serializable(source),
        armarx::VariantDataClass(source),
        TrajectoryBase(source)
    {
        CopyData(source, *this);
    }
 
 
 
    Trajectory::Trajectory(const std::map<double, Ice::DoubleSeq >& data)
    {
        if (data.size() == 0)
        {
            return;
        }
 
        typename  std::map<double, Ice::DoubleSeq >::const_iterator it = data.begin();
 
        for (; it != data.end(); it++)
        {
            TrajData dataEntry(this);
            dataEntry.timestamp = it->first;
            const Ice::DoubleSeq& dataVec = it->second;
 
            for (double i : dataVec)
            {
                checkValue(i);
                dataEntry.data.push_back(std::make_shared<Ice::DoubleSeq>(1, i));
            }
            dataMap.insert(std::move(dataEntry));
        }
    }
 
    //
    //    Trajectory::Trajectory(const TrajMap &map)
    //    {
    //        copyMap(map, dataMap);
    //    }
 
    Trajectory& Trajectory::operator=(const Trajectory& source)
    {
        CopyData(source, *this);
        return *this;
    }
 
 
    double Trajectory::getState(double t, size_t dim, size_t derivation)
    {
        if (dim >= this->dim())
        {
            throw LocalException() << "dimension is to big: " << dim << " max: " << this->dim();
        }
 
        typename timestamp_view::iterator it = dataMap.find(t);
 
        if (it == dataMap.end() || dim >= it->data.size() || !it->data.at(dim) || it->data.at(dim)->size() <= derivation)
        {
            //            ARMARX_INFO << "interpolating for " << VAROUT(t) << VAROUT(dim);
            __fillBaseDataAtTimestamp(t);// interpolates and retrieves
            it = dataMap.find(t);
        }
 
        if (it->data.size() <= dim)
        {
            //            ARMARX_ERROR << "FAILED!";
            //            ARMARX_INFO << VAROUT(t) << VAROUT(dim) << VAROUT(it->data.size()) << this->output();
            throw LocalException() << "std::vector ptr is not the correct size!? " << VAROUT(dim) << VAROUT(it->data.size());
        }
 
        if (!it->data.at(dim))
            //        it->data.at(dim).reset(new Ice::DoubleSeq());
        {
            throw LocalException() << "std::vector ptr is NULL!?";
        }
 
 
 
        std::vector<DoubleSeqPtr>& vec = it->data;
        ARMARX_CHECK_GREATER(vec.size(), dim);
        if (derivation != 0 && vec.at(dim)->size() <= derivation)
        {
            //resize and calculate derivations
            ARMARX_CHECK_GREATER(vec.size(), dim);
            ARMARX_CHECK_EXPRESSION(vec.at(dim));
 
            size_t curDeriv = vec.at(dim)->size();
            //            ARMARX_INFO << VAROUT(curDeriv) << VAROUT(dim);
            vec.at(dim)->resize(derivation + 1);
 
            while (curDeriv <= derivation)
            {
                double derivValue = getDiscretDifferentiationForDimAtT(t, dim, curDeriv);
                checkValue(curDeriv);
                vec.at(dim)->at(curDeriv) = derivValue;
                curDeriv++;
            }
 
        }
 
        ARMARX_CHECK_GREATER(vec.at(dim)->size(), derivation) << VAROUT(t) << VAROUT(dim) << VAROUT(*this);
        //    std::cout << "dimensions: " <<it->data.size() << " derivations: " <<  it->data.at(dim)->size() << std::endl;
        double result = vec.at(dim)->at(derivation);
        //    checkValue(result);
        return result;
    }
 
    double Trajectory::getState(double t, size_t dim, size_t derivation) const
    {
 
        if (dim >= this->dim())
        {
            throw LocalException() << "dimension is to big: " << dim << " max: " << this->dim();
        }
 
        typename timestamp_view::iterator it = dataMap.find(t);
 
        if (!dataExists(t, dim, derivation))
        {
            if (derivation == 0)
            {
                return __interpolate(t, dim, derivation);
            }
            else
            {
                return getDiscretDifferentiationForDimAtT(t, dim, derivation);
            }
        }
        else
        {
            //                std::cout << "dimensions: " <<it->data.size() << " derivations: " <<  it->data.at(dim)->size() << std::endl;
            double result = it->data.at(dim)->at(derivation);
 
            checkValue(result);
 
 
            return result;
        }
    }
 
    std::vector<Ice::DoubleSeq> Trajectory::getAllStates(double t, int maxDeriv)
    {
        std::vector<Ice::DoubleSeq> res;
 
        for (size_t i = 0; i < dim(); i++)
        {
            Ice::DoubleSeq curdata;
 
            for (int deri = 0; deri <= maxDeriv; deri++)
            {
                curdata.push_back(getState(t, i, deri));
            }
 
            res.push_back(curdata);
        }
 
        return res;
    }
 
    Ice::DoubleSeq Trajectory::getDerivations(double t, size_t dimension, size_t derivations) const
    {
        if (dimension >= dim())
        {
            throw LocalException() << "Dimension out of bounds: requested: " << dimension << " available: " << dim();
        }
 
        Ice::DoubleSeq result;
 
        for (size_t i = 0; i <= derivations; i++)
        {
            result.push_back(getState(t, dimension, i));
        }
 
        return result;
    }
 
    std::string Trajectory::getDimensionName(size_t dim) const
    {
        return dimensionNames.at(dim);
    }
 
    const Ice::StringSeq& Trajectory::getDimensionNames() const
    {
        return dimensionNames;
    }
 
    Trajectory::TrajDataContainer& Trajectory::data()
    {
        return dataMap;
    }
 
    Ice::DoubleSeq
    Trajectory::getDimensionData(size_t dimension, size_t derivation) const
    {
        if (dimension >= dim())
        {
            throw LocalException("dimension is out of range: ") << dimension << " actual dimensions: " << dim();
        }
 
        Ice::DoubleSeq result;
 
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        typename ordered_view::const_iterator itMap = ordv.begin();
 
        for (; itMap != ordv.end(); itMap++)
        {
            DoubleSeqPtr dataPtr = itMap->data[dimension];
            result.push_back(itMap->getDeriv(dimension, derivation));
            //        if(dataPtr && dataPtr->size() > derivation)
            //            result.push_back(itMap->getDeriv(dimension, derivation));
            //        else result.push_back(getState(itMap->timestamp, dimension, derivation));
        }
 
        return result;
 
    }
 
    Eigen::VectorXd Trajectory::getDimensionDataAsEigen(size_t dimension, size_t derivation) const
    {
        return getDimensionDataAsEigen(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
    }
 
    Eigen::VectorXd Trajectory::getDimensionDataAsEigen(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        if (dimension >= dim())
        {
            throw LocalException("dimension is out of range: ") << dimension << " actual dimensions: " << dim();
        }
 
        Ice::DoubleSeq result;
 
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
        //    typename ordered_view::const_iterator itMap = ordv.begin();
 
        size_t i = 0;
        for (; itO != itOEnd && i < size(); itO++, i++)
        {
            DoubleSeqPtr dataPtr = itO->data[dimension];
            result.push_back(itO->getDeriv(dimension, derivation));
            //        if(dataPtr && dataPtr->size() > derivation)
            //            result.push_back(itMap->getDeriv(dimension, derivation));
            //        else result.push_back(getState(itMap->timestamp, dimension, derivation));
        }
        Eigen::VectorXd resultVec(result.size());
        for (size_t i = 0; i < result.size(); i++)
        {
            resultVec(i) = result[i];
        }
        return resultVec;
    }
 
    Eigen::MatrixXd Trajectory::toEigen(size_t derivation, double startTime, double endTime) const
    {
        Eigen::MatrixXd result(size(), dim());
 
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
        //    typename ordered_view::const_iterator itMap = ordv.begin();
 
        size_t i = 0;
        for (; itO != itOEnd; itO++, i++)
        {
            //        DoubleSeqPtr dataPtr = itO->data[dimension];
            for (size_t d = 0; d < itO->data.size(); d++)
            {
                result(i, d) = (itO->getDeriv(d, derivation));
            }
        }
 
        return result;
    }
 
    Eigen::MatrixXd Trajectory::toEigen(size_t derivation) const
    {
        if (size() == 0 || dim() == 0)
        {
            return Eigen::MatrixXd();
        }
        return toEigen(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
    }
 
    TrajectoryPtr Trajectory::getPart(double startTime, double endTime, size_t numberOfDerivations) const
    {
        TrajectoryPtr result(new Trajectory());
 
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
        //    typename ordered_view::const_iterator itMap = ordv.begin();
 
        size_t i = 0;
        for (; itO != itOEnd; itO++, i++)
        {
            //        DoubleSeqPtr dataPtr = itO->data[dimension];
            for (size_t d = 0; d < itO->data.size(); d++)
            {
                Ice::DoubleSeq derivs;
                for (size_t i = 0; i < numberOfDerivations + 1; i++)
                {
                    derivs.push_back(itO->getDeriv(d, i));
                }
 
                result->addDerivationsToDimension(d, itO->getTimestamp(), derivs);
            }
            //            result.addPositionsToDimension(d, itO->getTimestamp(), itO->getDeriv(d, 0));
        }
        result->setDimensionNames(getDimensionNames());
        return result;
    }
 
    size_t Trajectory::dim() const
    {
        if (dataMap.size() == 0)
        {
            return 0;
        }
        else
        {
            return dataMap.begin()->data.size();
        }
    }
 
    size_t Trajectory::size() const
    {
        return dataMap.size();
    }
 
 
 
    double Trajectory::getTimeLength() const
    {
        const ordered_view& ordView = dataMap.get<TagOrdered>();
 
        if (ordView.begin() != ordView.end())
        {
            return ordView.rbegin()->timestamp - ordView.begin()->timestamp;
        }
        else
        {
            return 0;
        }
    }
 
    double Trajectory::getLength(size_t derivation) const
    {
        if (size() == 0)
        {
            return 0;
        }
        return getLength(derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
    }
 
    double Trajectory::getLength(size_t derivation, double startTime, double endTime) const
    {
        double length = 0.0;
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::const_iterator itO = ordv.lower_bound(startTime);
        ordered_view::const_iterator itOEnd = ordv.upper_bound(endTime);
        if (itO == ordv.end())
        {
            return 0.0;
        }
        size_t dimensions = dim();
        Ice::DoubleSeq prevValue(dimensions);
        for (size_t d = 0; d < dimensions; ++d)
        {
            prevValue[d] = getState(startTime, d, derivation);
        }
 
        itO++;
 
        double segmentLength = 0;
        for (;
             itO != itOEnd && itO != ordv.end();
             itO++
            )
        {
            if (itO->getTimestamp() >= endTime)
            {
                break;
            }
            segmentLength = 0;
            double value;
            for (size_t d = 0; d < dimensions; ++d)
            {
                value = itO->getDeriv(d, derivation);
                double diff = value - prevValue[d];
                segmentLength += diff * diff;
                prevValue[d] = value;
            }
            length += sqrt(segmentLength);
 
        }
        segmentLength = 0;
        // interpolate end values
        for (size_t d = 0; d < dimensions; ++d)
        {
            double value = getState(endTime, d, derivation);
            double diff = value - prevValue[d];
            segmentLength += diff * diff;
            prevValue[d] = value;
        }
        length += sqrt(segmentLength);
        return length;
    }
 
    double Trajectory::getLength(size_t dimension, size_t derivation) const
    {
        if (size() == 0)
        {
            return 0;
        }
        return getLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
    }
 
    double Trajectory::getLength(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        double length = 0.0;
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
        if (itO == ordv.end())
        {
            return 0.0;
        }
        double prevValue = getState(startTime, dimension, derivation);
 
        for (;
             itO != itOEnd && itO != ordv.end();
             itO++
            )
        {
            if (itO->getTimestamp() >= endTime)
            {
                break;
            }
            length += fabs(prevValue - itO->getDeriv(dimension, derivation));
            prevValue = itO->getDeriv(dimension, derivation);
        }
        length += fabs(prevValue - getState(endTime, dimension, derivation));
        return length;
    }
 
    double Trajectory::getSquaredLength(size_t dimension, size_t derivation) const
    {
        return getSquaredLength(dimension, derivation, begin()->getTimestamp(), rbegin()->getTimestamp());
    }
 
    double Trajectory::getSquaredLength(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        double length = 0.0;
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
        if (itO == ordv.end())
        {
            return 0.0;
        }
 
        double prevValue = itO->getDeriv(dimension, derivation);
 
        for (;
             itO != itOEnd;
             itO++
            )
        {
            length += fabs(pow(prevValue, 2.0) - pow(itO->getDeriv(dimension, derivation), 2.0));
            prevValue = itO->getDeriv(dimension, derivation);
        }
 
        return length;
    }
 
    double Trajectory::getMax(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        return getWithFunc(&std::max<double>, -std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
    }
 
    double Trajectory::getMin(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        return getWithFunc(&std::min<double>, std::numeric_limits<double>::max(), dimension, derivation, startTime, endTime);
    }
 
    double Trajectory::getWithFunc(const double & (*foo)(const double&, const double&), double initValue, size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        double bestValue = initValue;
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
 
        if (itO == ordv.end())
        {
            return bestValue;
        }
        for (;
             itO != itOEnd;
             itO++
            )
        {
            bestValue = foo(bestValue, itO->getDeriv(dimension, derivation));
        }
 
        return bestValue;
    }
 
    double Trajectory::getAmplitude(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        return getMax(dimension, derivation, startTime, endTime) - getMin(dimension, derivation, startTime, endTime);
    }
 
    Ice::DoubleSeq Trajectory::getMinima(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        Ice::DoubleSeq result;
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
 
        if (itO == ordv.end())
        {
            return result;
        }
        double preValue = itO->getDeriv(dimension, derivation);
        for (;
             itO != itOEnd;
 
            )
        {
            //            double t = itO->getTimestamp();
            double cur = itO->getDeriv(dimension, derivation);
            itO++;
            if (itO == ordv.end())
            {
                break;
            }
            double next = itO->getDeriv(dimension, derivation);
            if (cur <= preValue && cur <= next)
            {
                result.push_back(cur);
            }
            preValue = cur;
        }
 
        return result;
    }
 
    Ice::DoubleSeq Trajectory::getMinimaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        Ice::DoubleSeq result;
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
 
        if (itO == ordv.end())
        {
            return result;
        }
        double preValue = itO->getDeriv(dimension, derivation);
        for (;
             itO != itOEnd;
 
            )
        {
            //            double t = itO->getTimestamp();
            double cur = itO->getDeriv(dimension, derivation);
            itO++;
            if (itO == ordv.end())
            {
                break;
            }
            double next = itO->getDeriv(dimension, derivation);
            if (cur <= preValue && cur <= next)
            {
                result.push_back(itO->getTimestamp());
            }
            preValue = cur;
        }
 
        return result;
    }
 
    Ice::DoubleSeq Trajectory::getMaxima(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        Ice::DoubleSeq result;
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
 
        if (itO == ordv.end())
        {
            return result;
        }
        double preValue = itO->getDeriv(dimension, derivation);
        for (;
             itO != itOEnd;
 
            )
        {
            double cur = itO->getDeriv(dimension, derivation);
            itO++;
            if (itO == ordv.end())
            {
                break;
            }
            double next = itO->getDeriv(dimension, derivation);
            if (cur >= preValue && cur >= next)
            {
                result.push_back(cur);
            }
            preValue = cur;
        }
 
        return result;
    }
 
    Ice::DoubleSeq Trajectory::getMaximaTimestamps(size_t dimension, size_t derivation, double startTime, double endTime) const
    {
        Ice::DoubleSeq result;
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        ordered_view::iterator itO = ordv.lower_bound(startTime);
        ordered_view::iterator itOEnd = ordv.upper_bound(endTime);
 
        if (itO == ordv.end())
        {
            return result;
        }
        double preValue = itO->getDeriv(dimension, derivation);
        for (;
             itO != itOEnd;
 
            )
        {
            double cur = itO->getDeriv(dimension, derivation);
            itO++;
            if (itO == ordv.end())
            {
                break;
            }
            double next = itO->getDeriv(dimension, derivation);
 
            if (cur >= preValue && cur >= next)
            {
                result.push_back(itO->getTimestamp());
            }
            preValue = cur;
        }
 
        return result;
    }
 
 
    std::vector<DoubleSeqPtr> Trajectory::__calcBaseDataAtTimestamp(const double& t) const
    {
        //        ARMARX_INFO << "calcBaseDataAtTimestamp for " << t;
        //    typename timestamp_view::const_iterator it = dataMap.find(t);
        //    if(it != dataMap.end())
        //        return it->data;
        std::vector<DoubleSeqPtr> result;
 
        for (size_t dimension = 0; dimension < dim(); dimension++)
        {
            double newValue = __interpolate(t, dimension, 0);
            checkValue(newValue);
            result.push_back(std::make_shared<Ice::DoubleSeq>(1, newValue));
        }
 
        return result;
 
    }
 
    bool Trajectory::dataExists(double t, size_t dimension, size_t derivation) const
    {
        typename timestamp_view::iterator it = dataMap.find(t);
 
        if (it == dataMap.end() || !it->data.at(dimension) || it->data.at(dimension)->size() <= derivation)
        {
            return false;
        }
        else
        {
            return true;
        }
    }
 
 
    Trajectory::timestamp_view::iterator Trajectory::__fillBaseDataAtTimestamp(const double& t)
    {
        typename timestamp_view::const_iterator it = dataMap.find(t);
 
        if (it != dataMap.end() && it->data.size() == dim())
        {
            bool foundEmpty = false;
 
            for (auto& i : it->data)
            {
                if (!i || i->empty())
                {
                    foundEmpty = true;
                    break;
                }
            }
 
            if (!foundEmpty)
            {
                //                ARMARX_INFO << "Was not empty for " << t;
                return it;
            }
        }
 
        TrajData entry(this);
        entry.timestamp = t;
        entry.data.resize(dim());
        dataMap.insert(entry);
        it = dataMap.find(t);
 
        ARMARX_CHECK_EXPRESSION(it != dataMap.end());
        //        const ordered_view& ordv = dataMap.get<TagOrdered>();
        //        typename ordered_view::iterator itOrdered = ordv.iterator_to(*it);
        it->data = __calcBaseDataAtTimestamp(t);
 
        return it;
    }
 
    std::vector<DoubleSeqPtr>& Trajectory::getStates(double t)
    {
        typename timestamp_view::const_iterator it = dataMap.find(t);
 
        if (it == dataMap.end())
        {
            // interpolate and insert entry
            it = __fillBaseDataAtTimestamp(t);
        }
 
 
        return it->data;
    }
 
    std::vector<DoubleSeqPtr> Trajectory::getStates(double t) const
    {
        typename timestamp_view::const_iterator it = dataMap.find(t);
 
        if (it == dataMap.end())
        {
            // interpolate and return data
            return __calcBaseDataAtTimestamp(t);
        }
 
 
        return it->data;
    }
 
 
 
    Ice::DoubleSeq Trajectory::getStates(double t, size_t derivation) const
    {
        size_t dimensions = dim();
        Ice::DoubleSeq result;
        result.reserve(dimensions);
        for (size_t i = 0; i < dimensions; i++)
        {
            result.push_back(getState(t, i, derivation));
        }
 
        return result;
    }
 
    std::map<double, Ice::DoubleSeq> Trajectory::getStatesAround(double t, size_t derivation, size_t extend) const
    {
 
        std::map<double, Ice::DoubleSeq> res;
        typename timestamp_view::iterator itMap = dataMap.find(t);
 
        if (itMap != dataMap.end())
        {
            for (size_t i = 0; i < dim(); i++)
            {
                res.insert(std::pair<double, Ice::DoubleSeq>(t, {itMap->data[i]->at(derivation)}));
            }
 
            return res;
        }
 
        const ordered_view& ordv = dataMap.get<TagOrdered>();
 
        typename ordered_view::iterator itNext = ordv.upper_bound(t);
 
        typename ordered_view::iterator itPrev = itNext;
 
        itPrev--;
 
        if (itPrev == ordv.end())
        {
 
            throw LocalException("Cannot find value at timestamp ") << t;
        }
 
        for (size_t i = 0; i < extend; i++)
        {
            Ice::DoubleSeq preData = getStates(itPrev->timestamp, derivation);
            Ice::DoubleSeq nexData = getStates(itNext->timestamp, derivation);
 
            res.insert(std::pair<double, Ice::DoubleSeq>(itPrev->timestamp, preData));
            res.insert(std::pair<double, Ice::DoubleSeq>(itNext->timestamp, nexData));
 
            if (itPrev == ordv.begin() || itNext == ordv.end())
            {
                std::cout << "Warning: the timestamp is out of the range. " <<
                          "The current result will be returned" << std::endl;
                break;
            }
            itPrev--;
            itNext++;
        }
 
        return res;
 
    }
 
 
    Trajectory& Trajectory::operator +=(const Trajectory traj)
    {
        size_t dims = traj.dim();
        Ice::DoubleSeq timestamps = traj.getTimestamps();
 
        for (size_t d = 0; d < dims; ++d)
        {
            addPositionsToDimension(d, timestamps, traj.getDimensionData(d));
        }
 
        return *this;
    }
 
 
    void Trajectory::__addDimension()
    {
        size_t newDim = dim() + 1;
 
        typename timestamp_view::iterator itMap = dataMap.begin();
 
        for (; itMap != dataMap.end(); itMap++)
        {
            itMap->data.resize(newDim);
        }
    }
 
    void Trajectory::setEntries(const double t, const size_t dimIndex, const Ice::DoubleSeq& y)
    {
        typename timestamp_view::iterator itMap = dataMap.find(t);
 
        if (itMap == dataMap.end() || dim() == 0)
        {
            //            double dura = getTimeLength();
            TrajData newData(this);
            newData.timestamp = t;
            newData.data =  std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
            newData.data[dimIndex] = std::make_shared<Ice::DoubleSeq>(y);
            dataMap.insert(std::move(newData));
        }
        else
        {
            assert(dim() > 0);
 
            while (dim() <= dimIndex)
            {
                __addDimension();
            }
 
            itMap->data.resize(dim());
            ARMARX_CHECK_GREATER(itMap->data.size(), dimIndex);
            itMap->data.at(dimIndex) = std::make_shared<Ice::DoubleSeq>(y);
        }
 
    }
 
 
    void Trajectory::setPositionEntry(const double t, const size_t dimIndex, const double& y)
    {
        typename timestamp_view::iterator itMap = dataMap.find(t);
 
        if (itMap == dataMap.end() || dim() == 0)
        {
            TrajData newData(this);
            newData.timestamp = t;
            newData.data =  std::vector<DoubleSeqPtr>(std::max((size_t)1, dim()));
            newData.data[dimIndex] = std::make_shared<Ice::DoubleSeq>(1, y);
            dataMap.insert(newData);
        }
        else
        {
            assert(dim() > 0);
 
            while (dim() <= dimIndex)
            {
                __addDimension();
            }
 
            itMap->data.resize(dim());
            itMap->data.at(dimIndex) = std::make_shared<Ice::DoubleSeq>(1, y);
        }
 
    }
 
 
    void Trajectory::__fillAllEmptyFields()
    {
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        typename ordered_view::const_iterator itMap = ordv.begin();
 
        for (; itMap != ordv.end(); itMap++)
        {
            for (size_t dimension = 0; dimension < dim(); dimension++)
            {
                if (!itMap->data[dimension])
                {
                    itMap->data[dimension] = std::make_shared<Ice::DoubleSeq>(__interpolate(itMap, dimension, 0));
                }
            }
        }
    }
 
 
 
 
    Ice::DoubleSeq Trajectory::getTimestamps() const
    {
        Ice::DoubleSeq result;
        result.reserve(size());
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        typename ordered_view::const_iterator itMap = ordv.begin();
 
        for (; itMap != ordv.end(); itMap++)
        {
            result.push_back(itMap->timestamp);
        }
 
        return result;
    }
 
    Ice::FloatSeq Trajectory::getTimestampsFloat() const
    {
        Ice::FloatSeq result;
        result.reserve(size());
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        typename ordered_view::const_iterator itMap = ordv.begin();
 
        for (; itMap != ordv.end(); itMap++)
        {
            result.push_back(itMap->timestamp);
        }
 
        return result;
    }
 
 
 
    Ice::DoubleSeq
    Trajectory::getDiscreteDifferentiationForDim(size_t trajDimension, size_t derivation) const
    {
        if (trajDimension >= dim())
        {
            throw LocalException("dimension is out of range: ") << trajDimension << " actual dimensions: " << dim();
        }
 
 
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        typename ordered_view::const_iterator itPrev = ordv.begin();
 
        typename ordered_view::const_iterator itCurrent = itPrev;
        //        itCurrent++;
        typename ordered_view::const_iterator itNext = itCurrent;
        itNext++;
 
        Ice::DoubleSeq result;
 
        for (; itCurrent != ordv.end();)
        {
            if (itNext == ordv.end()) // last step-> differentiate between current and prev
            {
                itNext = itCurrent;
            }
 
 
            // get current data
            DoubleSeqPtr prevStatePtr = itPrev->data[trajDimension];
            DoubleSeqPtr nextStatePtr = itNext->data[trajDimension];
            // differentiateDiscretly now
            result.push_back((nextStatePtr->at(derivation) - prevStatePtr->at(derivation)) /
                             (itNext->timestamp - itPrev->timestamp));
            checkValue(*result.rbegin());
 
 
            itCurrent++, itPrev++, itNext++;
 
            if (itPrev == itCurrent)
            {
                // first step-> reestablish that current is > prev
                itPrev--;
            }
 
        }
 
        return result;
    }
 
 
    Ice::DoubleSeq Trajectory::DifferentiateDiscretly(const Ice::DoubleSeq& timestamps, const Ice::DoubleSeq& values, int derivationCount)
    {
        if (derivationCount < 0)
        {
            throw LocalException("Negative derivation value is not allowed!");
        }
 
        if (derivationCount == 0)
        {
            return values;
        }
 
        Ice::DoubleSeq result;
        int size = std::min(timestamps.size(), values.size());
 
        if (size < 2)
        {
            return values;
        }
 
        result.resize(size);
 
        // boundaries
        result[0] = (values.at(1) - values.at(0)) / (timestamps.at(1) - timestamps.at(0)) ;
        result[size - 1] = (values.at(size - 1) - values.at(size - 2)) / (timestamps.at(size - 1) - timestamps.at(size - 2)) ;
 
        //#pragma omp parallel for
        for (int i = 1; i < size - 1; ++i)
        {
            result[i] = (values.at(i + 1) - values.at(i - 1)) / (timestamps.at(i + 1) - timestamps.at(i - 1)) ;
            checkValue(result[i]);
        }
 
        if (derivationCount > 1) // recursivly differentiate
        {
            result = DifferentiateDiscretly(timestamps, result, derivationCount - 1);
        }
 
        return result;
 
    }
 
 
    double Trajectory::getDiscretDifferentiationForDimAtT(double t, size_t trajDimension, size_t derivation) const
    {
        if (derivation == 0)
        {
            return getState(t, trajDimension, derivation);
        }
 
        typename timestamp_view::iterator it = dataMap.find(t);
 
        const ordered_view& ordV = dataMap.get<TagOrdered>();
        typename ordered_view::iterator itCurrent = ordV.end();
 
        if (it != dataMap.end())
        {
            itCurrent = ordV.iterator_to(*it);
        }
 
        typename ordered_view::iterator itNext = ordV.upper_bound(t); // first element after t
 
        if (it != dataMap.end() && itNext == ordV.end())
        {
            itNext = itCurrent;    // current item is last, set next to current
        }
        else if (itNext == ordV.end() && it == dataMap.end())
        {
            throw LocalException() << "Cannot interpolate for t " << t << " no data in trajectory";
        }
 
        typename ordered_view::iterator itPrev = itNext;
        itPrev--; // now equal to current (if current exists) or before current
 
        if (itCurrent != ordV.end()) // check if current item exists
        {
            itPrev--;    //element in front of t
        }
 
        if (itPrev == ordV.end())
        {
            //element in front of t does not exist
            if (itCurrent != ordV.end())
            {
                itPrev = itCurrent;    // set prev element to current
            }
            else
            {
                throw LocalException() << "Cannot interpolate for t " << t;
            }
        }
 
        if (itNext == ordV.end())
        {
            //element after t does not exist
            if (itCurrent != ordV.end())
            {
                itNext = itCurrent;    // set next element to current
            }
            else
            {
                throw LocalException() << "Cannot interpolate for t " << t;
            }
        }
 
        if (itNext == itPrev)
        {
            throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << " " << VAROUT(size());
        }
 
        //        double diff = itNext->data[trajDimension]->at(derivation-1) - itPrev->data[trajDimension]->at(derivation-1);
        double tNext;
 
        if (dataExists(itNext->timestamp, trajDimension, derivation - 1) || derivation > 1)
        {
            tNext = itNext->timestamp;
        }
        else
        {
            tNext = t;
        }
 
        double next = getState(tNext, trajDimension, derivation - 1);
 
        double tBefore;
 
        if (dataExists(itPrev->timestamp, trajDimension, derivation - 1) || derivation > 1)
        {
            tBefore = itPrev->timestamp;
        }
        else
        {
            tBefore = t;
        }
 
        double before = getState(tBefore, trajDimension, derivation - 1);
 
        if (fabs(tNext - tBefore) < 1e-10)
        {
            throw LocalException() << "Interpolation failed: the next data and the previous are missing.\nInfo:\n" << VAROUT(tNext) << VAROUT(tBefore) << VAROUT(t) << VAROUT(trajDimension) << (getDimensionName(trajDimension)) << VAROUT(size()) << VAROUT(getTimestamps());
        }
 
        double duration = tNext - tBefore;
 
        double delta;
        if (trajDimension < limitless.size() && limitless.at(trajDimension).enabled)
        {
            double range = limitless.at(trajDimension).limitHi - limitless.at(trajDimension).limitLo;
 
            double dist1 = next - before;
            double dist2 = next - (before + range);
            double dist3 = next - (before - range);
 
            if (fabs(dist1) <= fabs(dist2) && fabs(dist1) <= fabs(dist3))
            {
                // no issue with bounds
                //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
                delta = dist1;
            }
            else if (fabs(dist2) <= fabs(dist3) && fabs(dist2) <= fabs(dist3))
            {
                // go over hi bound
                //ARMARX_IMPORTANT << "LIMITLESS deriv HIGH: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
                delta = dist2;
            }
            else
            {
                // go over lo bound
                //ARMARX_IMPORTANT << "LIMITLESS deriv LOW: checking dim " << trajDimension << ", prev:" << before << ", next:" << next << ", range:" << range << ", dist1:" << dist1 << ", dist2:" << dist2 << ", dist3:" << dist3;
                delta = dist3;
            }
 
            //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ": delta is " << delta;
        }
        else
        {
            // no limitless joint
            delta = next - before;
        }
 
        delta = delta / duration;
        //ARMARX_IMPORTANT << "LIMITLESS deriv: checking dim " << trajDimension << ": delta/duration is " << delta;
        checkValue(delta);
        return delta;
    }
 
    void Trajectory::differentiateDiscretly(size_t derivation)
    {
 
        for (size_t d = 0; d < dim(); d++)
        {
            differentiateDiscretlyForDim(d, derivation);
        }
    }
 
    void Trajectory::differentiateDiscretlyForDim(size_t trajDimension, size_t derivation)
    {
        removeDerivation(trajDimension, derivation);
        typename ordered_view::iterator itOrd = begin();
 
        for (; itOrd != end(); itOrd++)
        {
            getState(itOrd->timestamp, trajDimension, derivation);
        }
    }
 
 
    void Trajectory::reconstructFromDerivativeForDim(double valueAtFirstTimestamp, size_t trajDimension, size_t sourceDimOfSystemState, size_t targetDimOfSystemState)
    {
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        typename ordered_view::iterator it = ordv.begin();
 
        if (it == ordv.end())
        {
            return;
        }
 
        it->data[trajDimension]->at(targetDimOfSystemState) = valueAtFirstTimestamp;
        double previousValue = valueAtFirstTimestamp;
        double previousTimestamp = it->timestamp;
 
        //        size_t derivation = double().size();
        for (it++; it != ordv.end(); it++)
        {
            double slope;
            if (it->data[trajDimension]->size() > sourceDimOfSystemState)
            {
                slope = it->data[trajDimension]->at(sourceDimOfSystemState);
            }
            else
            {
                slope = 0;
            }
 
            double diff = it->timestamp - previousTimestamp;
            it->data[trajDimension]->at(targetDimOfSystemState) = previousValue + diff * slope;
            previousTimestamp = it->timestamp;
            previousValue = it->data[trajDimension]->at(targetDimOfSystemState);
        }
 
    }
 
 
    void Trajectory::negateDim(size_t trajDimension)
    {
        if (trajDimension >= dim())
        {
            throw LocalException("dimension is out of range: ") << trajDimension << " actual dimensions: " << dim();
        }
 
        const ordered_view& ordv = dataMap.get<TagOrdered>();
 
        Trajectory filteredTraj;
 
        for (const auto& it : ordv)
        {
            (*it.data.at(trajDimension)) *= -1;
        }
    }
 
 
 
 
 
    double
    Trajectory::__interpolate(double t, size_t dimension, size_t derivation) const
    {
        typename timestamp_view::const_iterator it = dataMap.find(t);
 
        if (it != dataMap.end() && it->data.size() > dimension && it->data.at(dimension) && it->data.at(dimension)->size() > derivation)
        {
            return it->data.at(dimension)->at(derivation);
        }
 
        const ordered_view& ordv = dataMap.get<TagOrdered>();
 
        typename ordered_view::iterator itNext = ordv.upper_bound(t);
 
        typename ordered_view::iterator itPrev = itNext;
 
        itPrev--;
 
        double result = __interpolate(t, itPrev, itNext, dimension, derivation);
 
        checkValue(result);
 
        return result;
    }
 
    double
    Trajectory::__interpolate(typename ordered_view::const_iterator itMap, size_t dimension, size_t derivation) const
    {
        typename ordered_view::iterator itPrev = itMap;
        itPrev--;
        typename ordered_view::iterator itNext = itMap;
        itNext++;
        return __interpolate(itMap->timestamp, itPrev, itNext, dimension, derivation);
    }
 
    double
    Trajectory::__interpolate(double t, typename ordered_view::const_iterator itPrev, typename ordered_view::const_iterator itNext, size_t dimension, size_t derivation) const
    {
        const ordered_view& ordView = dataMap.get<TagOrdered>();
        double previous = 0;
        double next = 0;
 
        // find previous SystemState that exists for that dimension
        while (itPrev != ordView.end() && (itPrev->data.at(dimension) == nullptr || itPrev->data.at(dimension)->size() <= derivation))
        {
            itPrev--;
        }
 
        if (itPrev != ordView.end())
        {
            //            ARMARX_INFO << "Found prev state at " << itPrev->timestamp;
            ARMARX_CHECK_NOT_EQUAL(t, itPrev->timestamp) << VAROUT(t) << VAROUT(itPrev->timestamp) << VAROUT(*this);
            previous = getState(itPrev->timestamp, dimension, derivation);
        }
 
        // find next SystemState that exists for that dimension
        while (itNext != ordView.end() && (!itNext->data.at(dimension) || itNext->data.at(dimension)->size() <= derivation))
        {
            itNext++;
        }
 
        if (itNext != ordView.end())
        {
            //            ARMARX_INFO << "Found next state at " << itNext->timestamp;
            ARMARX_CHECK_NOT_EQUAL(t, itNext->timestamp) << VAROUT(t) << VAROUT(itNext->timestamp);
            next = getState(itNext->timestamp, dimension, derivation);
        }
 
 
 
        if (itNext == ordView.end() && itPrev == ordView.end())
        {
            throw LocalException() << "Cannot find next or prev values in dim " << dimension << " at timestamp " << t;
        }
 
        if (itNext == ordView.end())
        {
            //            ARMARX_INFO << "Extrapolating to the right from " << itPrev->timestamp;
            return getState(itPrev->timestamp, dimension, derivation) + getState(itPrev->timestamp, dimension, derivation + 1) * (t - itPrev->timestamp);
        }
        else if (itPrev == ordView.end())
        {
            //            ARMARX_INFO << "Extrapolating to the left from " << itNext->timestamp;
            return getState(itNext->timestamp, dimension, derivation) - getState(itNext->timestamp, dimension, derivation + 1) * (itNext->timestamp - t);
        }
        else
        {
            // linear interpolation
 
            float f = math::MathUtils::ILerp(itPrev->timestamp, itNext->timestamp, t);
 
            if (dimension < limitless.size() && limitless.at(dimension).enabled)
            {
                return math::MathUtils::AngleLerp(previous, next, f);
            }
            else
            {
                return math::MathUtils::Lerp(previous, next, f);
            }
        }
    }
 
    void Trajectory::gaussianFilter(double filterRadius)
    {
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        Trajectory filteredTraj;
        filteredTraj.setDimensionNames(getDimensionNames());
        filteredTraj.setLimitless(getLimitless());
        Ice::DoubleSeq timestamps = getTimestamps();
 
        for (size_t d = 0; d < dim(); d++)
        {
            Ice::DoubleSeq entries;
 
            for (typename ordered_view::iterator it = ordv.begin(); it != ordv.end(); it++)
            {
                entries.push_back(__gaussianFilter(filterRadius, it, d, 0));
            }
 
            filteredTraj.addDimension(entries, timestamps);
        }
 
        CopyData(filteredTraj, *this);
    }
 
    double
    Trajectory::__gaussianFilter(double filterRadiusInTime, typename ordered_view::iterator centerIt, size_t trajNum, size_t dim)
    {
        const ordered_view& ordView = dataMap.get<TagOrdered>();
        const double sigma = filterRadiusInTime / 2.5;
 
        double weightedSum = 0;
        double sumOfWeight = 0;
        ordered_view::iterator start = centerIt;
 
        const double sqrt2PI = sqrt(2 * M_PI);
 
        for (int sign = -1; sign < 2; sign += 2)
        {
            for (ordered_view::iterator it = start;
                 it != ordView.end()
                 && fabs(it->timestamp - centerIt->timestamp) <= fabs(filterRadiusInTime * 2);
                )
            {
                double value;
                value = it->getDeriv(trajNum, dim);//data[trajNum]->at(dim);
                double diff = (it->timestamp - centerIt->timestamp);
                double squared = diff * diff;
                const double gaussValue = exp(-squared / (2 * sigma * sigma)) / (sigma * sqrt2PI);
                sumOfWeight += gaussValue;
                weightedSum += gaussValue * value;
 
                if (sign > 0)
                {
                    it++;
                }
                else
                {
                    it--;
                }
 
                checkValue(weightedSum);
            }
 
            start++;// skip center value in second loop iteration
        }
 
        double result;
        result = weightedSum / sumOfWeight;
        checkValue(result);
        return result;
 
    }
 
 
    void Trajectory::CopyData(const Trajectory& source, Trajectory& destination)
    {
        if (&source == &destination)
        {
            return;
        }
 
        destination.clear();
 
        Ice::DoubleSeq timestamps = source.getTimestamps();
 
        for (size_t dim = 0; dim < source.dim(); dim++)
        {
            destination.addDimension(source.getDimensionData(dim), timestamps
                                    );
        }
        CopyMetaData(source, destination);
    }
 
    void Trajectory::CopyMetaData(const Trajectory& source, Trajectory& destination)
    {
        destination.setDimensionNames(source.getDimensionNames());
 
        destination.setLimitless(source.getLimitless());
    }
 
 
    void Trajectory::clear(bool keepMetaData)
    {
        dataMap.erase(dataMap.begin(), dataMap.end());
        if (!keepMetaData)
        {
            dimensionNames.clear();
            limitless.clear();
        }
    }
 
 
 
    Ice::DoubleSeq Trajectory::GenerateTimestamps(double startTime, double endTime, double stepSize)
    {
        if (startTime >= endTime)
        {
            throw LocalException("startTime must be smaller than endTime.");
        }
 
        Ice::DoubleSeq result;
        size_t size = int((endTime - startTime) / stepSize) + 1;
        stepSize = (endTime - startTime) / (size - 1);
        result.reserve(size);
 
        double currentTimestamp = startTime;
        size_t i = 0;
 
        while (i < size)
        {
            result.push_back(currentTimestamp);
            currentTimestamp += stepSize;
            i++;
        }
        ARMARX_CHECK_EQUAL(result.size(), size) << VAROUT(startTime) << VAROUT(endTime) << VAROUT(stepSize);
        return result;
    }
 
 
    Ice::DoubleSeq Trajectory::NormalizeTimestamps(const Ice::DoubleSeq& timestamps, const double startTime, const double endTime)
    {
        Ice::DoubleSeq normTimestamps;
        normTimestamps.resize(timestamps.size());
        const double minValue = *timestamps.begin();
        const double maxValue = *timestamps.rbegin();
        const double duration = maxValue - minValue;
 
        for (size_t i = 0; i < timestamps.size(); i++)
        {
            normTimestamps[i] = startTime + (timestamps.at(i) - minValue) / duration * (endTime - startTime);
        }
 
        return normTimestamps;
    }
 
 
    Trajectory Trajectory::NormalizeTimestamps(const Trajectory& traj, const double startTime, const double endTime)
    {
 
        if (traj.size() <= 1 || (traj.begin()->timestamp == startTime && traj.rbegin()->timestamp == endTime))
        {
            return traj;    // already normalized
        }
 
 
        Ice::DoubleSeq timestamps = traj.getTimestamps();
 
 
        Ice::DoubleSeq normTimestamps = NormalizeTimestamps(timestamps, startTime, endTime);
        Trajectory normExampleTraj;
 
        for (size_t dim = 0; dim < traj.dim(); dim++)
        {
            Ice::DoubleSeq dimensionData  =  traj.getDimensionData(dim);
            normExampleTraj.addDimension(dimensionData, normTimestamps);
        }
        normExampleTraj.setDimensionNames(traj.getDimensionNames());
        normExampleTraj.setLimitless(traj.getLimitless());
        return normExampleTraj;
 
 
    }
 
    TrajectoryPtr Trajectory::normalize(const double startTime, const double endTime)
    {
        Trajectory normTraj = NormalizeTimestamps(*this, startTime, endTime);
        TrajectoryPtr newTraj = new Trajectory(normTraj);
        return newTraj;
    }
 
    TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(double maxVelocity, double maxAcceleration, double maxDeviation, const IceUtil::Time& timestep)
    {
        Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(dim(), maxVelocity);
        Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(dim(), maxAcceleration);
        return calculateTimeOptimalTrajectory(maxVelocities, maxAccelerations, maxDeviation, timestep);
    }
 
    TrajectoryPtr Trajectory::calculateTimeOptimalTrajectory(const Eigen::VectorXd& maxVelocities, const Eigen::VectorXd& maxAccelerations, double maxDeviation, IceUtil::Time const& timestep)
    {
 
        bool hasLimitlessDimension = false;
        for (auto l : limitless)
        {
            hasLimitlessDimension |= l.enabled;
        }
 
        TrajectoryPtr unfoldedTraj;
        if (hasLimitlessDimension)
        {
            unfoldedTraj = new Trajectory(*this);
            TrajectoryController::UnfoldLimitlessJointPositions(unfoldedTraj);
        }
 
 
        auto timestepValue = timestep.toSecondsDouble();
        std::list<Eigen::VectorXd> waypointList;
        auto dimensions = dim();
        for (auto& waypoint : hasLimitlessDimension ? *unfoldedTraj : *this)
        {
            auto positions = waypoint.getPositions();
            waypointList.push_back(Eigen::Map<Eigen::VectorXd>(positions.data(), dimensions));
        }
 
        VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(VirtualRobot::Path(waypointList, maxDeviation),
                maxVelocities,
                maxAccelerations, timestepValue);
        ARMARX_CHECK_EXPRESSION(timeOptimalTraj.isValid());
 
 
        TrajectoryPtr newTraj = new Trajectory();
 
        Ice::DoubleSeq newTimestamps;
        double duration = timeOptimalTraj.getDuration();
        newTimestamps.reserve(duration / timestepValue + 1);
        for (double t = 0.0; t < duration; t += timestepValue)
        {
            newTimestamps.push_back(t);
        }
        newTimestamps.push_back(duration);
 
        for (size_t d = 0; d < dimensionNames.size(); d++)
        {
            Ice::DoubleSeq position;
            position.reserve(newTimestamps.size());
            for (double t = 0.0; t < duration; t += timestepValue)
            {
                position.push_back(timeOptimalTraj.getPosition(t)[d]);
            }
            position.push_back(timeOptimalTraj.getPosition(duration)[d]);
            newTraj->addDimension(position, newTimestamps, dimensionNames.at(d));
 
            Ice::DoubleSeq derivs;
            derivs.reserve(newTimestamps.size());
 
            for (double t = 0.0; t < duration; t += timestepValue)
            {
                derivs.clear();
                derivs.push_back(timeOptimalTraj.getPosition(t)[d]);
                derivs.push_back(timeOptimalTraj.getVelocity(t)[d]);
                newTraj->addDerivationsToDimension(d, t, derivs);
            }
            derivs.clear();
            derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
            derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
            newTraj->addDerivationsToDimension(d, duration, derivs);
        }
        newTraj->setLimitless(limitless);
        if (hasLimitlessDimension)
        {
            TrajectoryController::FoldLimitlessJointPositions(newTraj);
        }
        return newTraj;
    }
 
 
 
 
 
 
 
 
    size_t Trajectory::addDimension(const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps, const std::string name)
    {
 
        const auto& tempTimestamps = timestamps.size() > 0 ? timestamps : GenerateTimestamps(values);
 
        size_t newDimIndex = dim();
 
        __addDimension();
 
        addPositionsToDimension(newDimIndex, values, tempTimestamps);
        if (newDimIndex < dimensionNames.size())
        {
            dimensionNames.at(newDimIndex) = name;
        }
        else
        {
            dimensionNames.push_back(name);
        }
        return newDimIndex;
    }
 
    void Trajectory::removeDimension(size_t dimension)
    {
        typename timestamp_view::iterator itMap = dataMap.begin();
 
        for (; itMap != dataMap.end(); itMap++)
        {
            std::vector< DoubleSeqPtr >& data = itMap->data;
 
            if (dimension < data.size())
            {
                data.erase(data.begin() + dimension);
            }
        }
        if (dimension < dimensionNames.size())
        {
            dimensionNames.erase(dimensionNames.begin() + dimension);
        }
    }
 
    void Trajectory::removeDerivation(size_t derivation)
    {
        typename timestamp_view::iterator itMap = dataMap.begin();
 
        for (; itMap != dataMap.end(); itMap++)
        {
            std::vector< DoubleSeqPtr >& data = itMap->data;
 
            for (auto& vec : data)
            {
                if (derivation + 1 <  vec->size())
                {
                    vec->resize(derivation);
                }
            }
        }
    }
 
    void Trajectory::removeDerivation(size_t dimension, size_t derivation)
    {
        typename timestamp_view::iterator itMap = dataMap.begin();
 
        for (; itMap != dataMap.end(); itMap++)
        {
            std::vector< DoubleSeqPtr >& data = itMap->data;
 
            if (data.size() > dimension && derivation + 1 < data.at(dimension)->size())
            {
                data.at(dimension)->resize(derivation);
            }
        }
    }
 
    Trajectory::ordered_view::const_iterator Trajectory::begin() const
    {
        return dataMap.get<TagOrdered>().begin();
    }
 
    Trajectory::ordered_view::const_iterator Trajectory::end() const
    {
        return dataMap.get<TagOrdered>().end();
    }
 
    Trajectory::ordered_view::const_reverse_iterator Trajectory::rbegin() const
    {
        return dataMap.get<TagOrdered>().rbegin();
    }
 
    Trajectory::ordered_view::const_reverse_iterator Trajectory::rend() const
    {
        return dataMap.get<TagOrdered>().rend();
    }
 
    std::vector<DoubleSeqPtr>& Trajectory::operator[](double timestamp)
    {
        return getStates(timestamp);
    }
 
 
 
    void Trajectory::addPositionsToDimension(size_t dimension, const Ice::DoubleSeq& values, const Ice::DoubleSeq& timestamps)
    {
        if (dimension >= dim() && dim() > 0)
        {
            addDimension(values, timestamps);
        }
        else
        {
            ARMARX_CHECK_EXPRESSION(timestamps.size() ==  values.size()) << timestamps.size() << ", " <<  values.size();
 
            for (size_t i = 0; i < timestamps.size(); ++i)
            {
                checkValue(timestamps[i]);
                checkValue(values[i]);
                setPositionEntry(timestamps[i], dimension, values[i]);
            }
        }
    }
 
 
    void Trajectory::addDerivationsToDimension(size_t dimension, const double t, const Ice::DoubleSeq& derivs)
    {
        setEntries(t, dimension, derivs);
    }
 
    Trajectory::TrajData::TrajData(Trajectory* traj)
    {
        trajectory = traj;
    }
 
    DoubleSeqPtr Trajectory::TrajData::operator[](size_t dim) const
    {
        return data.at(dim);
    }
 
    double Trajectory::TrajData::getTimestamp() const
    {
        return timestamp;
    }
 
    double Trajectory::TrajData::getPosition(size_t dim) const
    {
        return getDeriv(dim, 0);
    }
 
    Eigen::VectorXf Trajectory::TrajData::getPositionsAsVectorXf() const
    {
        if (!trajectory)
        {
            throw LocalException("Ptr to trajectory is NULL");
        }
        size_t numDim = trajectory->dim();
        Eigen::VectorXf result(numDim);
        for (std::size_t i = 0; i < numDim; ++i)
        {
            result(i) = getPosition(i);
        }
        return result;
    }
 
    Eigen::VectorXd Trajectory::TrajData::getPositionsAsVectorXd() const
    {
        if (!trajectory)
        {
            throw LocalException("Ptr to trajectory is NULL");
        }
        size_t numDim = trajectory->dim();
        Eigen::VectorXd result(numDim);
        for (std::size_t i = 0; i < numDim; ++i)
        {
            result(i) = getPosition(i);
        }
        return result;
    }
 
    double Trajectory::TrajData::getDeriv(size_t dim, size_t derivation) const
    {
        if (!trajectory)
        {
            throw LocalException("Ptr to trajectory is NULL");
        }
        return trajectory->getState(timestamp, dim, derivation);
    }
 
    const std::vector<DoubleSeqPtr>& Trajectory::TrajData::getData() const
    {
        return data;
    }
 
 
 
 
    void Trajectory::shiftTime(double shift)
    {
        const ordered_view& ordv = dataMap.get<TagOrdered>();
        typename ordered_view::const_iterator itMap = ordv.begin();
        Trajectory shiftedTraj;
        CopyMetaData(*this, shiftedTraj);
        auto d = dim();
        for (; itMap != ordv.end(); itMap++)
        {
            for (size_t i = 0; i < d; ++i)
            {
                shiftedTraj.setEntries(itMap->timestamp + shift, i, *itMap->getData().at(i));
            }
        }
        //        dataMap.swap(shiftedTraj.dataMap);
        *this = shiftedTraj;
    }
 
    void Trajectory::shiftValue(const Ice::DoubleSeq& shift)
    {
        if (shift.size() > dim())
        {
            throw LocalException("dimension is out of range: ") << shift.size() << " actual dimensions: " << dim();
        }
 
 
        for (size_t dimension = 0; dimension < dim(); dimension++)
        {
            const ordered_view& ordv = dataMap.get<TagOrdered>();
            typename ordered_view::const_iterator itMap = ordv.begin();
 
            for (; itMap != ordv.end(); itMap++)
            {
                itMap->data[dimension]->at(0) += shift[dimension];
            }
        }
 
    }
 
    void Trajectory::scaleValue(const Ice::DoubleSeq& factor)
    {
        if (factor.size() > dim())
        {
            throw LocalException("dimension is out of range: ") << factor.size() << " actual dimensions: " << dim();
        }
 
 
        for (size_t dimension = 0; dimension < dim(); dimension++)
        {
            const ordered_view& ordv = dataMap.get<TagOrdered>();
            typename ordered_view::const_iterator itMap = ordv.begin();
 
            for (; itMap != ordv.end(); itMap++)
            {
                itMap->data[dimension]->at(0) *= factor[dimension];
            }
        }
 
    }
 
 
    void Trajectory::setLimitless(const LimitlessStateSeq& limitlessStates)
    {
        limitless = limitlessStates;
    }
 
    LimitlessStateSeq Trajectory::getLimitless() const
    {
        return limitless;
    }
 
 
} // namespace armarx