HPIPMInterface.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 "LQOCSolver.hpp"

#ifdef HPIPM

#include <blasfeo_target.h>
#include <blasfeo_common.h>
#include <blasfeo_d_aux.h>
#include <blasfeo_d_aux_ext_dep.h>

extern "C" {
#include <hpipm_d_ocp_qp.h>
#include <hpipm_d_ocp_qp_sol.h>
#include <hpipm_d_ocp_qp_ipm.h>
#include <hpipm_d_ocp_qp_dim.h>
#include <hpipm_timing.h>
}

#include <unsupported/Eigen/MatrixFunctions>


namespace ct {
namespace optcon {

template <int STATE_DIM, int CONTROL_DIM>
class HPIPMInterface : public LQOCSolver<STATE_DIM, CONTROL_DIM>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    static const int state_dim = STATE_DIM;
    static const int control_dim = CONTROL_DIM;
    static const int max_box_constr_dim = STATE_DIM + CONTROL_DIM;

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

    using StateVectorArray = ct::core::StateVectorArray<STATE_DIM>;
    using ControlVectorArray = ct::core::ControlVectorArray<CONTROL_DIM>;

    // definitions for variable-size constraints
    using constr_vec_t = Eigen::Matrix<double, -1, 1>;
    using constr_vec_array_t = ct::core::DiscreteArray<constr_vec_t>;

    using box_constr_sparsity_t = Eigen::Matrix<int, max_box_constr_dim, 1>;

    HPIPMInterface();

    virtual ~HPIPMInterface();

    virtual void configure(const NLOptConSettings& settings) override;

    void solve() override;

    virtual void computeStatesAndControls() override;
    virtual void computeFeedbackMatrices() override;
    virtual void compute_lv() override;

    void printSolution();

    virtual bool configureInputBoxConstraints(
        std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;
    virtual bool configureStateBoxConstraints(
        std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;

    virtual bool configureGeneralConstraints(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;

    virtual void initializeAndAllocate() override;

    virtual const ct::core::ControlVectorArray<CONTROL_DIM>& get_lv() override;

private:
    void setSolverDimensions(const int N, const int nbu = 0, const int nbx = 0, const int ng = 0);

    void setProblemImpl(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem) override;

    void setupCostAndDynamics(StateMatrixArray& A,
        StateControlMatrixArray& B,
        StateVectorArray& b,
        FeedbackArray& P,
        StateVectorArray& qv,
        StateMatrixArray& Q,
        ControlVectorArray& rv,
        ControlMatrixArray& R);

    bool changeProblemSize(std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem);

    void computeLrInvArray();
    bool isLrInvComputed_;

    void d_print_mat(int m, int n, double* A, int lda);

    void d_print_e_tran_mat(int row, int col, double* A, int lda);

    int N_;

    std::vector<int> nx_;  
    std::vector<int> nu_;  

    std::vector<int> nbu_;  
    std::vector<int> nbx_;  

    std::vector<int> ng_;  

    std::vector<int> nsbx_;  // number of softed constraints on state box constraints
    std::vector<int> nsbu_;  // number of softed constraints on input box constraints
    std::vector<int> nsg_;   // number of softed constraints on general constraints

    std::vector<double*> hA_;                  
    std::vector<double*> hB_;                  
    std::vector<double*> hb_;                  
    Eigen::Matrix<double, state_dim, 1> hb0_;  

    std::vector<double*> hQ_;                    
    std::vector<double*> hS_;                    
    std::vector<double*> hR_;                    
    std::vector<double*> hq_;                    
    std::vector<double*> hr_;                    
    Eigen::Matrix<double, control_dim, 1> hr0_;  

    std::vector<double*> hlbx_;  
    std::vector<double*> hubx_;  
    std::vector<double*> hlbu_;  
    std::vector<double*> hubu_;  

    std::vector<int*> hidxbx_;  
    std::vector<int*> hidxbu_;  

    std::vector<double*> hlg_;  
    std::vector<double*> hug_;  
    std::vector<double*> hC_;   
    std::vector<double*> hD_;   

    //  local vars for constraint bounds for statge k=0, which need to be different by HPIPM convention
    Eigen::VectorXd hd_lg_0_Eigen_;
    Eigen::VectorXd hd_ug_0_Eigen_;

    std::vector<double*> hZl_;  // todo what are those quantities? (related to soft constraints?)
    std::vector<double*> hZu_;
    std::vector<double*> hzl_;
    std::vector<double*> hzu_;
    std::vector<int*> hidxs_;
    std::vector<double*> hlls_;
    std::vector<double*> hlus_;

    // cached data for efficiency
    ct::core::DiscreteArray<ct::core::ControlMatrix<control_dim>> Lr_inv_;  // inv of cholesky matrix of hessian H


    NLOptConSettings settings_;

    int dim_size_;
    void* dim_mem_;
    struct d_ocp_qp_dim dim_;

    void* qp_mem_;
    struct d_ocp_qp qp_;

    void* qp_sol_mem_;
    struct d_ocp_qp_sol qp_sol_;

    void* ipm_arg_mem_;
    struct d_ocp_qp_ipm_arg arg_;

    // workspace
    void* ipm_mem_;
    struct d_ocp_qp_ipm_ws workspace_;
    int hpipm_status_;  // status code after solving

    // todo make this a setting
    ::hpipm_mode mode_ = ::hpipm_mode::SPEED;  // ROBUST/BALANCED; see also hpipm_common.h
};

}  // namespace optcon
}  // namespace ct

#endif  // HPIPM