MpcTimeKeeper.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 "MpcSettings.h"
#include "timehorizon/MpcTimeHorizon.h"

#include <ct/optcon/problem/ContinuousOptConProblem.h>

//#define DEBUG_PRINT_TIMEKEEPER

namespace ct {
namespace optcon {
namespace tpl {



template <typename SCALAR = double>
class MpcTimeKeeper
{
public:

    MpcTimeKeeper() {}

    MpcTimeKeeper(std::shared_ptr<MpcTimeHorizon<SCALAR>> timeHorizonStrategy, const mpc_settings& mpc_settings)
        : mpc_settings_(mpc_settings),
          initialized_(false),
          finalPointReached_(false),
          lastMeasuredDelay_(0.0),
          maxDelayMeasured_(0.0),
          minDelayMeasured_(std::numeric_limits<SCALAR>::max()),
          summedDelay_(0.0),
          timeHorizonStrategy_(timeHorizonStrategy),
          firstRun_(true)
    {
    }



    void initialize()
    {
        firstRun_ = true;

        timer_.reset();
        lastSolveTimer_.reset();
        firstSolveTimer_.reset();

        ext_timer_.reset();
        ext_lastSolveTimer_.reset();
        ext_firstSolveTimer_.reset();

        lastMeasuredDelay_ = 0.0;
        maxDelayMeasured_ = 0.0;
        minDelayMeasured_ = std::numeric_limits<SCALAR>::max();
        summedDelay_ = 0.0;
        finalPointReached_ = false;

        initialized_ = true;
    }



    void computeNewTimings(const SCALAR externalTime,
        const SCALAR current_T,
        SCALAR& new_T,
        SCALAR& t_forw_start,
        SCALAR& t_forw_stop)
    {
        if (initialized_ == false)
            throw std::runtime_error(
                "Error in MPC time keeper: cannot update timings if MpcTimeKeeper not properly initialized.");


        SCALAR timeSinceEndedLastSolve;
        SCALAR timeSinceEndedFirstSolve;

        if (!firstRun_)
        {
            if (mpc_settings_.useExternalTiming_)
            {
                ext_firstSolveTimer_.stop(externalTime);
                ext_lastSolveTimer_.stop(externalTime);
                timeSinceEndedFirstSolve = ext_firstSolveTimer_.getElapsedTime();
                timeSinceEndedLastSolve = ext_lastSolveTimer_.getElapsedTime();
            }
            else
            {
                lastSolveTimer_.stop();
                firstSolveTimer_.stop();
                timeSinceEndedLastSolve = lastSolveTimer_.getElapsedTime();
                timeSinceEndedFirstSolve = firstSolveTimer_.getElapsedTime();
            }
        }
        else
        {
            // set zero for the first mpc run
            timeSinceEndedLastSolve = 0.0;
            timeSinceEndedFirstSolve = 0.0;
        }


        // starting time of forward prediction, relative to previous controller
        t_forw_start = timeSinceEndedLastSolve;


        SCALAR delayToApply = computeDelayToApply();


        // stopping time relative to previous controller/run
        if (!firstRun_)
            t_forw_stop = t_forw_start + delayToApply;
        else
            t_forw_stop = t_forw_start;


        if (t_forw_start > current_T)
        {
            std::cerr << "WARNING: forward integration start time cannot be bigger than last time horizon. Truncating "
                         "forward integration time."
                      << std::endl;
            t_forw_start = current_T;
        }
        if (t_forw_stop > current_T)
        {
            std::cerr << "WARNING: forward integration stop time cannot be bigger than last time horizon. Truncating "
                         "forward integration time."
                      << std::endl;
            t_forw_stop = current_T;
        }


        finalPointReached_ = timeHorizonStrategy_->computeNewTimeHorizon(timeSinceEndedFirstSolve, t_forw_stop, new_T);


#ifdef DEBUG_PRINT_TIMEKEEPER
        std::cout << "DEBUG_PRINT_TIMEKEEPER: Time since first solve(): " << timeSinceEndedFirstSolve << std::endl;
        std::cout << "DEBUG_PRINT_TIMEKEEPER: Time since last solve(): " << timeSinceEndedLastSolve << std::endl;
        std::cout << "DEBUG_PRINT_TIMEKEEPER: t_forward_start: " << t_forw_start << std::endl;
        std::cout << "DEBUG_PRINT_TIMEKEEPER: t_forward_stop: " << t_forw_stop << std::endl;
        std::cout << "DEBUG_PRINT_TIMEKEEPER: New Time Horizon: " << new_T << std::endl;
        std::cout << "DEBUG_PRINT_TIMEKEEPER: final point reached_: " << finalPointReached_ << std::endl;
#endif
    }



    const bool finalPointReached() const { return finalPointReached_; }

    void updateSettings(const mpc_settings& settings) { mpc_settings_ = settings; }
    void startDelayMeasurement(const SCALAR& externalTime)
    {
        if (mpc_settings_.measureDelay_)
        {
            if (mpc_settings_.useExternalTiming_)
                ext_timer_.start(externalTime);
            else
                timer_.start();
        }
    }


    void stopDelayMeasurement(const SCALAR& externalTime)
    {
        if (mpc_settings_.useExternalTiming_)
        {
            ext_timer_.stop(externalTime);
            lastMeasuredDelay_ = ext_timer_.getElapsedTime();
            // measure how much time passed since last successful solve()
            ext_lastSolveTimer_.start(externalTime);
        }
        else
        {
            timer_.stop();
            lastMeasuredDelay_ = timer_.getElapsedTime();
            // measure how much time passed since last successful solve()
            lastSolveTimer_.start();
        }

        maxDelayMeasured_ = std::max(maxDelayMeasured_, lastMeasuredDelay_);
        minDelayMeasured_ = std::min(minDelayMeasured_, lastMeasuredDelay_);
        summedDelay_ += lastMeasuredDelay_;

        if (lastMeasuredDelay_ < 0)
            throw std::runtime_error("Fatal: measured delay cannot be < 0");

#ifdef DEBUG_PRINT_TIMEKEEPER
        std::cout << "Measured delay during Solution: " << lastMeasuredDelay_ << " seconds" << std::endl;
        std::cout << "Max. measured delay during Solution: " << maxDelayMeasured_ << " seconds" << std::endl;
        std::cout << "Min. measured delay during Solution: " << minDelayMeasured_ << " seconds" << std::endl;
#endif

        if (firstRun_)
        {
            // start timer for measuring how much time elapsed since the first successful plan
            if (mpc_settings_.useExternalTiming_)
            {
                ext_firstSolveTimer_.start(externalTime);
            }
            else
            {
                firstSolveTimer_.start();
            }

            firstRun_ = false;
        }
    }



    SCALAR timeSincePreviousSuccessfulSolve(const SCALAR& externalTime)
    {
        if (firstRun_)
            return 0.0;
        else
        {
            if (mpc_settings_.useExternalTiming_)
            {
                ext_lastSolveTimer_.stop(externalTime);
                return ext_lastSolveTimer_.getElapsedTime();
            }
            else
            {
                lastSolveTimer_.stop();
                return lastSolveTimer_.getElapsedTime();
            }
        }
    }



    const SCALAR timeSinceFirstSuccessfulSolve(const SCALAR& externalTime)
    {
        if (firstRun_)
            return 0.0;
        else
        {
            if (mpc_settings_.useExternalTiming_)
            {
                ext_firstSolveTimer_.stop(externalTime);
                return ext_firstSolveTimer_.getElapsedTime();
            }
            else
            {
                firstSolveTimer_.stop();
                return firstSolveTimer_.getElapsedTime();
            }
        }
    }

    const SCALAR& getMeasuredDelay() const { return lastMeasuredDelay_; }
    const SCALAR& getMaxMeasuredDelay() const { return maxDelayMeasured_; }
    const SCALAR& getMinMeasuredDelay() const { return minDelayMeasured_; }
    const SCALAR& getSummedDelay() const { return summedDelay_; }
private:

    SCALAR computeDelayToApply()
    {
        SCALAR fixedDelay = 1e-6 * mpc_settings_.additionalDelayUs_;
        SCALAR variableDelay = 0.0;

        if (mpc_settings_.measureDelay_)  // use variable, measured delay
            variableDelay = mpc_settings_.delayMeasurementMultiplier_ * lastMeasuredDelay_;
        else  // using fixed delay
            fixedDelay += 1e-6 * mpc_settings_.fixedDelayUs_;

#ifdef DEBUG_PRINT_TIMEKEEPER
        std::cout << "Accumulated delay to apply: " << fixedDelay + variableDelay << " seconds" << std::endl;
#endif

        return fixedDelay + variableDelay;
    }


    mpc_settings mpc_settings_;

    bool initialized_;

    bool finalPointReached_;

    core::tpl::Timer<SCALAR> timer_;
    core::tpl::Timer<SCALAR> lastSolveTimer_;
    core::tpl::Timer<SCALAR> firstSolveTimer_;

    core::tpl::ExternallyDrivenTimer<SCALAR> ext_timer_;
    core::tpl::ExternallyDrivenTimer<SCALAR> ext_lastSolveTimer_;
    core::tpl::ExternallyDrivenTimer<SCALAR> ext_firstSolveTimer_;

    SCALAR lastMeasuredDelay_;  
    SCALAR maxDelayMeasured_;   
    SCALAR minDelayMeasured_;   
    SCALAR summedDelay_;        

    std::shared_ptr<MpcTimeHorizon<SCALAR>> timeHorizonStrategy_;

    bool firstRun_;  
};


}  // namespace tpl

typedef tpl::MpcTimeKeeper<double> MpcTimeKeeper;

}  // namespace optcon
}  // namespace ct