MPC.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 <type_traits>

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

#include "MpcSettings.h"
#include "MpcTimeKeeper.h"

#include "policyhandler/PolicyHandler.h"
#include "timehorizon/MpcTimeHorizon.h"

#include <ct/optcon/solver/NLOptConSolver.hpp>
#include "policyhandler/default/StateFeedbackPolicyHandler.h"

//#define DEBUG_PRINT_MPC   //! use this flag to enable debug printouts in the MPC implementation


namespace ct {
namespace optcon {


template <typename OPTCON_SOLVER>
class MPC
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    static const size_t STATE_DIM = OPTCON_SOLVER::STATE_D;
    static const size_t CONTROL_DIM = OPTCON_SOLVER::CONTROL_D;

    static const size_t P_DIM = OPTCON_SOLVER::POS_DIM;
    static const size_t V_DIM = OPTCON_SOLVER::VEL_DIM;

    using Scalar_t = typename OPTCON_SOLVER::Scalar_t;
    using Policy_t = typename OPTCON_SOLVER::Policy_t;
    using OptConProblem_t = ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, Scalar_t>;



    MPC(const OptConProblem_t& problem,
        const typename OPTCON_SOLVER::Settings_t& solverSettings,
        const mpc_settings& mpcsettings = mpc_settings(),
        std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>> customPolicyHandler = nullptr,
        std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> customTimeHorizon = nullptr);



    OPTCON_SOLVER& getSolver();



    void setTimeHorizonStrategy(std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> timeHorizonStrategy);


    void setInitialGuess(const Policy_t& initGuess);

    bool timeHorizonReached();



    const Scalar_t timeSinceFirstSuccessfulSolve(const Scalar_t& extTime);



    void doForwardIntegration(const Scalar_t& t_forward_start,
        const Scalar_t& t_forward_stop,
        core::StateVector<STATE_DIM, Scalar_t>& x_start,
        const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController =
            nullptr);


    void prepareIteration(const Scalar_t& ext_ts);



    bool finishIteration(const core::StateVector<STATE_DIM, Scalar_t>& x,
        const Scalar_t x_ts,
        Policy_t& newPolicy,
        Scalar_t& newPolicy_ts,
        const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>> forwardIntegrationController =
            nullptr);


    void resetMpc(const Scalar_t& newTimeHorizon);



    void updateSettings(const mpc_settings& settings);


    void printMpcSummary();


private:

    void integrateForward(const Scalar_t startTime,
        const Scalar_t stopTime,
        core::StateVector<STATE_DIM, Scalar_t>& state,
        const std::shared_ptr<core::Controller<STATE_DIM, CONTROL_DIM, Scalar_t>>& controller);

    void checkSettings(const mpc_settings& settings);

    Scalar_t t_forward_start_;
    Scalar_t t_forward_stop_;

    OPTCON_SOLVER solver_;

    mpc_settings mpc_settings_;

    typename OPTCON_SOLVER::OptConProblem_t::DynamicsPtr_t dynamics_;

    ct::core::Integrator<STATE_DIM, Scalar_t> forwardIntegrator_;

    bool firstRun_;

    size_t runCallCounter_;

    std::shared_ptr<PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>> policyHandler_;

    Policy_t currentPolicy_;

    std::shared_ptr<tpl::MpcTimeHorizon<Scalar_t>> timeHorizonStrategy_;

    tpl::MpcTimeKeeper<Scalar_t> timeKeeper_;
};


}  //namespace optcon
}  //namespace ct