NLOCBackendBase.hpp

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 <atomic>

#include <ct/optcon/costfunction/CostFunctionQuadratic.hpp>
#include <ct/optcon/solver/OptConSolver.h>

#include <ct/optcon/problem/LQOCProblem.hpp>

#include <ct/optcon/solver/lqp/GNRiccatiSolver.hpp>
#include <ct/optcon/solver/lqp/HPIPMInterface.hpp>

#include <ct/optcon/solver/NLOptConSettings.hpp>

#include "NLOCResults.hpp"

#ifdef MATLAB
#include <ct/optcon/matlab.hpp>
#endif

namespace ct {
namespace optcon {


template <size_t STATE_DIM,
    size_t CONTROL_DIM,
    size_t P_DIM,
    size_t V_DIM,
    typename SCALAR = double,
    bool CONTINUOUS = true>
class NLOCBackendBase
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    static const size_t state_dim = STATE_DIM;
    static const size_t control_dim = CONTROL_DIM;

    typedef NLOptConSettings Settings_t;

    typedef typename ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM, SCALAR> Policy_t;

    typedef typename std::conditional<CONTINUOUS,
        ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>,
        DiscreteOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>>::type OptConProblem_t;

    typedef LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR> LQOCProblem_t;
    typedef LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR> LQOCSolver_t;

    typedef ct::core::StateVectorArray<STATE_DIM, SCALAR> StateVectorArray;
    typedef std::shared_ptr<StateVectorArray> StateVectorArrayPtr;

    typedef ct::core::ControlVectorArray<CONTROL_DIM, SCALAR> ControlVectorArray;
    typedef std::shared_ptr<ControlVectorArray> ControlVectorArrayPtr;

    typedef std::vector<StateVectorArrayPtr, Eigen::aligned_allocator<StateVectorArrayPtr>> StateSubsteps;
    typedef std::shared_ptr<StateSubsteps> StateSubstepsPtr;

    typedef std::vector<ControlVectorArrayPtr, Eigen::aligned_allocator<ControlVectorArrayPtr>> ControlSubsteps;
    typedef std::shared_ptr<ControlSubsteps> ControlSubstepsPtr;

    typedef OptconSystemInterface<STATE_DIM, CONTROL_DIM, OptConProblem_t, SCALAR> systemInterface_t;
    typedef std::shared_ptr<systemInterface_t> systemInterfacePtr_t;

    using ControlMatrix = ct::core::ControlMatrix<CONTROL_DIM, SCALAR>;
    using ControlMatrixArray = ct::core::ControlMatrixArray<CONTROL_DIM, SCALAR>;
    using StateMatrixArray = ct::core::StateMatrixArray<STATE_DIM, SCALAR>;
    using StateControlMatrixArray = ct::core::StateControlMatrixArray<STATE_DIM, CONTROL_DIM, SCALAR>;
    using FeedbackArray = ct::core::FeedbackArray<STATE_DIM, CONTROL_DIM, SCALAR>;
    using TimeArray = ct::core::tpl::TimeArray<SCALAR>;

    using state_matrix_t = Eigen::Matrix<SCALAR, STATE_DIM, STATE_DIM>;
    using control_matrix_t = Eigen::Matrix<SCALAR, CONTROL_DIM, CONTROL_DIM>;
    using control_state_matrix_t = Eigen::Matrix<SCALAR, CONTROL_DIM, STATE_DIM>;
    using state_control_matrix_t = Eigen::Matrix<SCALAR, STATE_DIM, CONTROL_DIM>;

    using state_vector_t = core::StateVector<STATE_DIM, SCALAR>;
    using control_vector_t = core::ControlVector<CONTROL_DIM, SCALAR>;
    using feedback_matrix_t = core::FeedbackMatrix<STATE_DIM, CONTROL_DIM, SCALAR>;

    using scalar_t = SCALAR;
    using scalar_array_t = std::vector<SCALAR, Eigen::aligned_allocator<SCALAR>>;

    NLOCBackendBase(const OptConProblem_t& optConProblem, const Settings_t& settings);
    NLOCBackendBase(const OptConProblem_t& optConProblem,
        const std::string& settingsFile,
        bool verbose = true,
        const std::string& ns = "alg");

    NLOCBackendBase(const systemInterfacePtr_t& systemInterface, const Settings_t& settings);
    NLOCBackendBase(const systemInterfacePtr_t& systemInterface,
        const std::string& settingsFile,
        bool verbose = true,
        const std::string& ns = "alg");

    virtual ~NLOCBackendBase();

    template <typename T = OptConProblem_t>  // do not use this template argument
    typename std::enable_if<std::is_same<T, ContinuousOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>>::value,
        systemInterfacePtr_t>::type
    createSystemInterface(const OptConProblem_t& optConProblem, const Settings_t& settings);

    template <typename T = OptConProblem_t>  // do not use this template argument
    typename std::enable_if<std::is_same<T, DiscreteOptConProblem<STATE_DIM, CONTROL_DIM, SCALAR>>::value,
        systemInterfacePtr_t>::type
    createSystemInterface(const OptConProblem_t& optConProblem, const Settings_t& settings);


    virtual void configure(const Settings_t& settings);

    const Settings_t& getSettings() const;

    void setInitialGuess(const Policy_t& initialGuess);


    void changeTimeHorizon(const SCALAR& tf);
    void changeTimeHorizon(int numStages);

    SCALAR getTimeHorizon();

    int getNumSteps();

    int getNumStepsPerShot() const;

    void changeInitialState(const core::StateVector<STATE_DIM, SCALAR>& x0);

    void changeCostFunction(const typename OptConProblem_t::CostFunctionPtr_t& cf);

    void changeNonlinearSystem(const typename OptConProblem_t::DynamicsPtr_t& dyn);

    void changeLinearSystem(const typename OptConProblem_t::LinearPtr_t& lin);

    void changeInputBoxConstraints(const typename OptConProblem_t::ConstraintPtr_t& con);

    void changeStateBoxConstraints(const typename OptConProblem_t::ConstraintPtr_t& con);

    void changeGeneralConstraints(const typename OptConProblem_t::ConstraintPtr_t& con);

    std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances();

    const std::vector<typename OptConProblem_t::DynamicsPtr_t>& getNonlinearSystemsInstances() const;

    std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances();

    const std::vector<typename OptConProblem_t::LinearPtr_t>& getLinearSystemsInstances() const;

    std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances();

    const std::vector<typename OptConProblem_t::CostFunctionPtr_t>& getCostFunctionInstances() const;

    std::vector<typename OptConProblem_t::ConstraintPtr_t>& getInputBoxConstraintsInstances();
    std::vector<typename OptConProblem_t::ConstraintPtr_t>& getStateBoxConstraintsInstances();

    const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getInputBoxConstraintsInstances() const;
    const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getStateBoxConstraintsInstances() const;

    std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances();

    const std::vector<typename OptConProblem_t::ConstraintPtr_t>& getGeneralConstraintsInstances() const;


    bool testConsistency();



    void logToMatlab(const size_t& iteration);

    void logInitToMatlab();

    SCALAR getCost() const;

    SCALAR getTotalDefect() const;

    void reset();

    const core::StateTrajectory<STATE_DIM, SCALAR> getStateTrajectory() const;

    const core::ControlTrajectory<CONTROL_DIM, SCALAR> getControlTrajectory() const;


    const Policy_t& getSolution();

    const TimeArray& getTimeArray();

    bool isConfigured();

    bool isInitialized();



    void retrieveLastAffineModel(StateMatrixArray& A, StateControlMatrixArray& B, StateVectorArray& b);

    virtual void prepareSolveLQProblem(size_t startIndex);

    virtual void finishSolveLQProblem(size_t endIndex);

    virtual void solveFullLQProblem();

    void extractSolution();

    void updateCosts();

    bool nominalRollout();

    void checkProblem();

    size_t& iteration();


    void printSummary();

    bool lineSearch();

    virtual void computeLQApproximation(size_t firstIndex, size_t lastIndex) = 0;

    void setInputBoxConstraintsForLQOCProblem();
    void setStateBoxConstraintsForLQOCProblem();

    void resetDefects();

    void computeDefectsNorm();

    virtual void rolloutShots(size_t firstIndex, size_t lastIndex) = 0;

    bool rolloutShotsSingleThreaded(size_t threadId,
        size_t firstIndex,
        size_t lastIndex,
        ControlVectorArray& u_ff_local,
        StateVectorArray& x_local,
        const StateVectorArray& x_ref_lqr,
        StateVectorArray& xShot,
        StateVectorArray& d,
        StateSubsteps& substepsX,
        ControlSubsteps& substepsU,
        std::atomic_bool* terminationFlag = nullptr) const;

    virtual SCALAR performLineSearch() = 0;

    void doFullStepUpdate();

    void logSummaryToMatlab(const std::string& fileName);

    const SummaryAllIterations<SCALAR>& getSummary() const;

protected:
    bool rolloutSingleShot(const size_t threadId,
        const size_t k,
        ControlVectorArray& u_ff_local,
        StateVectorArray& x_local,
        const StateVectorArray& x_ref_lqr,
        StateVectorArray& xShot,
        StateSubsteps& substepsX,
        ControlSubsteps& substepsU,
        std::atomic_bool* terminationFlag = nullptr) const;



    void computeSingleDefect(size_t k,
        const StateVectorArray& x_local,
        const StateVectorArray& xShot,
        StateVectorArray& d) const;



    void executeLQApproximation(size_t threadId, size_t k);



    void computeLinearizedConstraints(size_t threadId, size_t k);


    void initializeCostToGo();


    void computeCostToGo(size_t k);


    void designController(size_t k);


    void computeCostsOfTrajectory(size_t threadId,
        const core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
        const core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
        scalar_t& intermediateCost,
        scalar_t& finalCost) const;

    void computeBoxConstraintErrorOfTrajectory(size_t threadId,
        const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
        const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
        scalar_t& e_tot) const;

    void computeGeneralConstraintErrorOfTrajectory(size_t threadId,
        const ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_local,
        const ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_local,
        scalar_t& e_tot) const;

    void executeLineSearch(const size_t threadId,
        const scalar_t alpha,
        ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_recorded,
        ct::core::StateVectorArray<STATE_DIM, SCALAR>& x_shot_recorded,
        ct::core::StateVectorArray<STATE_DIM, SCALAR>& defects_recorded,
        ct::core::ControlVectorArray<CONTROL_DIM, SCALAR>& u_recorded,
        scalar_t& intermediateCost,
        scalar_t& finalCost,
        scalar_t& defectNorm,
        scalar_t& e_box_norm,
        scalar_t& e_gen_norm,
        StateSubsteps& substepsX,
        ControlSubsteps& substepsU,
        std::atomic_bool* terminationFlag = nullptr) const;


    bool acceptStep(
        const SCALAR alpha,
        const SCALAR intermediateCost,
        const SCALAR finalCost,
        const SCALAR defectNorm,
        const SCALAR e_box_norm,
        const SCALAR e_gen_norm,
        const SCALAR lowestMeritPrevious,
        SCALAR& new_merit);


    void updateFFController(size_t k);

    template <typename ARRAY_TYPE, size_t ORDER = 1>
    SCALAR computeDiscreteArrayNorm(const ARRAY_TYPE& d) const;

    template <typename ARRAY_TYPE, size_t ORDER = 1>
    SCALAR computeDiscreteArrayNorm(const ARRAY_TYPE& a, const ARRAY_TYPE& b) const;


    template <size_t ORDER = 1>
    SCALAR computeDefectsNorm(const StateVectorArray& d) const;

    bool initialized_;
    bool configured_;

    size_t iteration_; 
    bool firstRollout_;

    Settings_t settings_;

    int K_;                               
    ct::core::tpl::TimeArray<SCALAR> t_;  
    StateVectorArray x_;                  
    StateVectorArray xShot_;              
    StateVectorArray d_;                  
    StateVectorArray x_prev_;             
    StateVectorArray x_ref_lqr_;          

    ControlVectorArray u_ff_;       
    ControlVectorArray u_ff_prev_;  
    FeedbackArray L_;               

    ControlVectorArray delta_u_ff_;     
    StateVectorArray delta_x_;          
    StateVectorArray delta_x_ref_lqr_;  

    StateSubstepsPtr substepsX_;    
    ControlSubstepsPtr substepsU_;  

    SCALAR d_norm_;      
    SCALAR e_box_norm_;  
    SCALAR e_gen_norm_;  
    SCALAR lx_norm_;     
    SCALAR lu_norm_;     

    scalar_t intermediateCostBest_;
    scalar_t finalCostBest_;
    scalar_t lowestCost_;

    scalar_t intermediateCostPrevious_;
    scalar_t finalCostPrevious_;

    scalar_t alphaBest_;

    std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM, SCALAR>> lqocProblem_;

    std::shared_ptr<LQOCSolver<STATE_DIM, CONTROL_DIM, SCALAR>> lqocSolver_;

    systemInterfacePtr_t systemInterface_;

    std::vector<typename OptConProblem_t::CostFunctionPtr_t> costFunctions_;
    std::vector<typename OptConProblem_t::ConstraintPtr_t> inputBoxConstraints_;
    std::vector<typename OptConProblem_t::ConstraintPtr_t> stateBoxConstraints_;
    std::vector<typename OptConProblem_t::ConstraintPtr_t> generalConstraints_;

    size_t lqpCounter_;

    NLOCBackendBase::Policy_t policy_;

    SummaryAllIterations<SCALAR> summaryAllIterations_;

#ifdef MATLAB
    matlab::MatFile matFile_;
#endif  //MATLAB
};


template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
template <typename ARRAY_TYPE, size_t ORDER>
SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeDiscreteArrayNorm(
    const ARRAY_TYPE& d) const
{
    SCALAR norm = 0.0;

    for (size_t k = 0; k < d.size(); k++)
    {
        norm += d[k].template lpNorm<ORDER>();
    }
    return norm;
}


template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
template <typename ARRAY_TYPE, size_t ORDER>
SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeDiscreteArrayNorm(
    const ARRAY_TYPE& a,
    const ARRAY_TYPE& b) const
{
    assert(a.size() == b.size());

    SCALAR norm = 0.0;

    for (size_t k = 0; k < a.size(); k++)
    {
        norm += (a[k] - b[k]).template lpNorm<ORDER>();
    }
    return norm;
}

template <size_t STATE_DIM, size_t CONTROL_DIM, size_t P_DIM, size_t V_DIM, typename SCALAR, bool CONTINUOUS>
template <size_t ORDER>
SCALAR NLOCBackendBase<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR, CONTINUOUS>::computeDefectsNorm(
    const StateVectorArray& d) const
{
    return computeDiscreteArrayNorm<StateVectorArray, ORDER>(d);
}


}  // namespace optcon
}  // namespace ct