DiscreteTrajectoryBase.h

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include "TrajectoryBase.h"
#include "../arrays/TimeArray.h"
#include "../arrays/DiscreteArray.h"

#include <ct/core/common/Interpolation.h>


namespace ct {
namespace core {


template <class T, class Alloc = Eigen::aligned_allocator<T>, typename SCALAR = double>
class DiscreteTrajectoryBase : public TrajectoryBase<T, SCALAR>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    DiscreteTrajectoryBase(const InterpolationType& type = ZOH) : time_(), data_(), interp_(type) {}

    DiscreteTrajectoryBase(const tpl::TimeArray<SCALAR>& time,
        const DiscreteArray<T, Alloc>& data,
        const InterpolationType& type = ZOH)
        : time_(time), data_(data), interp_(type)
    {
    }


    DiscreteTrajectoryBase(const DiscreteArray<T, Alloc>& data,
        const SCALAR& deltaT,
        const SCALAR& t0,
        const InterpolationType& type = ZOH)
        : time_(), data_(data), interp_(type)
    {
        time_ = tpl::TimeArray<SCALAR>(deltaT, data.size(), t0);
    }

    DiscreteTrajectoryBase(const DiscreteTrajectoryBase<T, Alloc>& other)
        : time_(other.time_), data_(other.data_), interp_(other.interp_.getInterpolationType())
    {
    }


    DiscreteTrajectoryBase(DiscreteTrajectoryBase<T, Alloc, SCALAR>& other,
        const size_t startIndex,
        const size_t endIndex)
        : time_(), data_(), interp_(other.interp_.getInterpolationType())
    {
        tpl::TimeArray<SCALAR> time_temp;
        DiscreteArray<T, Alloc> data_temp;

        for (size_t i = startIndex; i <= endIndex; i++)
        {
            time_temp.push_back(other.time_[i]);
            data_temp.push_back(other.data_[i]);
        }

        time_ = time_temp;
        data_ = data_temp;
    }

    virtual ~DiscreteTrajectoryBase(){};



    void setData(const DiscreteArray<T, Alloc>& data) { data_ = data; }

    void setInterpolationType(const InterpolationType& type) { interp_.changeInterpolationType(type); }

    void setTime(const tpl::TimeArray<SCALAR>& time) { time_ = time; }

    void shiftTime(const SCALAR& dt) { time_.addOffset(-dt); }

    virtual T eval(const SCALAR& evalTime) override
    {
        T result;
        interp_.interpolate(time_, data_, evalTime, result);
        return result;
    }


    size_t size() const
    {
        if (data_.size() != time_.size())
            throw std::runtime_error("DiscreteTrajectoryBase: size inconsistent.");

        return this->data_.size();
    }

    T& front() { return data_.front(); }
    const T& front() const { return data_.front(); }
    T& back() { return data_.back(); }
    const T& back() const { return data_.back(); }
    T& operator[](const size_t i) { return data_[i]; }
    const T& operator[](const size_t i) const { return data_[i]; }
    const SCALAR startTime() const { return time_.front(); }
    const SCALAR finalTime() const { return time_.back(); }
    const SCALAR duration() const { return time_.back() - time_.front(); }

    void push_back(const T& data, const SCALAR& time, const bool timeIsAbsolute)
    {
        if (timeIsAbsolute)
            time_.push_back(time);
        else
            time_.push_back(time + time_.back());

        data_.push_back(data);
    }

    void pop_back()
    {
        time_.pop_back();
        data_.pop_back();
    }


    void eraseFront(const size_t& N, const SCALAR& dt = 0.0)
    {
        time_.eraseFront(N);
        data_.eraseFront(N);
        shiftTime(dt);
    }

    void clear()
    {
        data_.clear();
        time_.clear();
    }


    void swapData(DiscreteTrajectoryBase& other)
    {
        time_.swap(other.time_);
        data_.swap(other.data_);
    }

    DiscreteTrajectoryBase& operator=(const DiscreteTrajectoryBase& other)
    {
        if (this == &other)
            return *this;

        time_ = other.time_;
        data_ = other.data_;
        interp_.changeInterpolationType(other.interp_.getInterpolationType());

        return *this;
    }

    const SCALAR& getTimeFromIndex(const size_t& ind) const { return time_[ind]; }

    size_t getIndexFromTime(const SCALAR& t) { return interp_.findIndex(time_, t); }
    DiscreteArray<T, Alloc>& getDataArray() { return data_; }
    const DiscreteArray<T, Alloc>& getDataArray() const { return data_; }
    tpl::TimeArray<SCALAR>& getTimeArray() { return time_; }
    const tpl::TimeArray<SCALAR>& getTimeArray() const { return time_; }

    void print()
    {
        assert(time_.size() == data_.size());
        for (size_t i = 0; i < time_.size(); i++)
        {
            std::cout << "time: \t " << time_[i] << std::endl << "data-point: \t" << data_[i] << std::endl;
        }
    }

protected:
    tpl::TimeArray<SCALAR> time_;  

    DiscreteArray<T, Alloc> data_;  

    Interpolation<T, Alloc, SCALAR> interp_;  
};

} /* namespace core */
} /* namespace ct */