TimeGrid.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

//#define DEBUG_TIMEGRID

#include <math.h>
#include <cmath>

namespace ct {
namespace optcon {
namespace tpl {

template <typename SCALAR>
class TimeGrid
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    TimeGrid() = delete;

    TimeGrid(const size_t numberOfShots, const SCALAR timeHorizon)
        : numberOfShots_(numberOfShots), timeHorizon_(timeHorizon), t_(numberOfShots + 1, SCALAR(0.0))
    {
        makeUniformGrid();
    }

    void changeShotCount(const size_t numberOfShots)
    {
        numberOfShots_ = numberOfShots;
        t_.clear();
        t_.resize(numberOfShots_ + 1, SCALAR(0.0));
        makeUniformGrid();
    }

    void changeTimeHorizon(const SCALAR timeHorizon)
    {
        timeHorizon_ = timeHorizon;
        makeUniformGrid();
    }


    void updateTimeGrid(const Eigen::Matrix<SCALAR, Eigen::Dynamic, 1>& h_segment)
    {
        t_[0] = SCALAR(0.0);  //by convention (just for documentation)

        for (size_t i = 0; i < (size_t)h_segment.size(); ++i)
            t_[i + 1] = t_[i] + h_segment(i);

#ifdef DEBUG_TIMEGRID
        std::cout << " ... in updateTimeGrid(). t_ =  ";
        for (size_t i = 0; i < t_.size(); i++)
            std::cout << std::setprecision(10) << t_[i] << "  ";

        std::cout << std::endl;
#endif
    }


    void makeUniformGrid()
    {
        for (size_t i = 0; i < numberOfShots_ + 1; i++)
            t_[i] = i * (SCALAR)(timeHorizon_ / (SCALAR)numberOfShots_);
    }


    const SCALAR getShotStartTime(const size_t shot_index) const { return t_[shot_index]; }
    const SCALAR getShotEndTime(const size_t shot_index) const { return t_[shot_index + 1]; }
    const SCALAR getShotDuration(const size_t shot_index) const { return (t_[shot_index + 1] - t_[shot_index]); }
    const ct::core::tpl::TimeArray<SCALAR>& toImplementation() { return t_; }
    const SCALAR getTimeHorizon() const { return timeHorizon_; }
    const SCALAR getOptimizedTimeHorizon() const { return t_.back(); }
private:
    const size_t numberOfShots_;
    SCALAR timeHorizon_;

    // the individual times of each pair from i=0,..., N
    ct::core::tpl::TimeArray<SCALAR> t_;
};
}

typedef tpl::TimeGrid<double> TimeGrid;

}  // namespace optcon
}  // namespace ct