ocra-wbi-plugins/ocra-icub-clients/walking-client/include/walking-client/MIQPController.h

Go to the documentation of this file.

/*
 *  This file is part of ocra-recipes.
 *  Copyright (C) 2016 Institut des Systèmes Intelligents et de Robotique (ISIR)
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef _MIQP_CONTROLLER_H_
#define _MIQP_CONTROLLER_H_

#include "gurobi_c++.h"
#include <walking-client/utils.h>
#include <ocra-icub/OcraWbiModel.h>
#include <ocra/util/FileOperations.h>
#include <yarp/os/RateThread.h>
#include <yarp/os/Semaphore.h>
#include <Eigen/Dense>
#include <Eigen/Lgsm>
#include "unsupported/Eigen/MatrixFunctions"
#include <walking-client/constraints/MIQPLinearConstraints.h>
#include <walking-client/MIQPState.h>
#include "Gurobi.h" // eigen-gurobi

namespace MIQP{
    enum InputVectorIndex{
        A_X_IN=0,
        A_Y_IN,
        B_X_IN,
        B_Y_IN,
        ALPHA_X_IN,
        ALPHA_Y_IN,
        BETA_X_IN,
        BETA_Y_IN,
        DELTA_IN,
        GAMMA_IN,
        U_X_IN,
        U_Y_IN
    };
}

class MIQPController : public yarp::os::RateThread {
public:
    MIQPController(MIQPParameters params, ocra::Model::Ptr robotModel, std::shared_ptr<StepController> stepController, const Eigen::MatrixXd &comStateRef);

    virtual ~MIQPController();

    virtual bool threadInit();

    virtual void threadRelease();

    virtual void run();

    void setCOMStateRefInPreviewWindow(unsigned int k, Eigen::VectorXd &comStateRef);
        
    yarp::os::Semaphore semaphore;
    
    void getSolution(Eigen::VectorXd &X_kn);

protected:
    // MARK: - PROTECTED METHODS
    void setLinearPartObjectiveFunction();

    void setLowerAndUpperBounds();

    void updateStateVector();

    void buildAh(int dt, Eigen::MatrixXd &output);

    void buildBh(int dt, Eigen::MatrixXd &output);

    void buildQ(Eigen::MatrixXd &output);

    void buildT(Eigen::MatrixXd &output);

    void buildC_H(Eigen::MatrixXd &output);

    void buildC_P(Eigen::MatrixXd &output);

    void buildC_B(Eigen::MatrixXd &output);

    void buildH_N(Eigen::MatrixXd &output);
    
    
    void buildNb(Eigen::MatrixXd &output, double wb);
    
    void buildNx(Eigen::MatrixXd &output);

    void buildGenericRegMat(MIQP::InputVectorIndex whichVariable, double weight, Eigen::MatrixXd &output);
    
    void buildSw(Eigen::MatrixXd &output, MIQPParameters miqpParams);

    
    void buildPreviewStateMatrix(const Eigen::MatrixXd &C, Eigen::MatrixXd &P);

    
    void buildPreviewInputMatrix(const Eigen::MatrixXd &C, Eigen::MatrixXd &R);
    
    void buildEqualityConstraintsMatrices(const Eigen::VectorXd &x_k, Eigen::MatrixXd& Aeq, Eigen::VectorXd& Beq);

    void updateEqualityConstraints(const Eigen::VectorXd &x_k, Eigen::VectorXd &Beq);
    
    void buildRegularizationTerms(MIQPParameters &miqpParams);
    
    void buildCoMJerkReg(MIQPParameters &miqpParams);
    
    void buildAvoidOneFootRestReg(MIQPParameters &miqpParams);
    
    void buildMinimizeSteppingReg(MIQPParameters &miqpParams);
    
    void setBinaryVariables();
    
    void writeToFile(const double& time, const Eigen::VectorXd& X_kn, std::string& home);
    
private:
    // MARK: - PRIVATE VARIABLES
    /*
     * Pointer to robot model which will be used by MIQPController::updateStateVector() to update the state vector by retrieving robot information set up by the hosting client (`walking-client`) and coordinated by the controller server.
     */
    ocra::Model::Ptr _robotModel;
    
    /*
     * Pointer to step controller started by the walking-client
     * 
     * @todo This is really NOT desireable at all. Find another way to get this object down to MIQPLinearConstraints.
     */
    std::shared_ptr<StepController> _stepController;

    MIQPParameters _miqpParams;

    Eigen::MatrixXd _comStateRef;

    int _period;
    
    bool _addRegularization;

    Eigen::VectorXd _linearTermTransObjFunc;

    Eigen::VectorXd _lb;

    Eigen::VectorXd _ub;

    // TODO: Document
    std::string _variablesNames[INPUT_VECTOR_SIZE];

    Eigen::VectorXd _xi_k;

    Eigen::VectorXd _X_kn;

    Eigen::MatrixXd _Ah;

    Eigen::MatrixXd _Bh;

    Eigen::MatrixXd _Q;

    Eigen::MatrixXd _T;

    Eigen::MatrixXd _C_H;

    Eigen::MatrixXd _C_P;

    Eigen::MatrixXd _C_B;

    Eigen::MatrixXd _P_H;

    Eigen::MatrixXd _P_P;

    Eigen::MatrixXd _P_B;

    Eigen::MatrixXd _R_H;

    Eigen::MatrixXd _R_P;

    Eigen::MatrixXd _R_B;

    Eigen::MatrixXd _Sw;

    Eigen::MatrixXd _Nb;
    
    Eigen::MatrixXd _Nx;

    Eigen::MatrixXd _H_N;


    Eigen::MatrixXd _Aineq;

    Eigen::VectorXd _Bineq;
        
    Eigen::MatrixXd _Aeq;
    
    Eigen::VectorXd _Beq;
    
    Eigen::MatrixXd _Ci_eq;
    
    Eigen::VectorXd _fcbar_eq;
    
    Eigen::MatrixXd _rhs_2_eq;

    Eigen::GurobiDense _eigGurobi;

    std::shared_ptr<MIQPLinearConstraints> _constraints;
    
    std::shared_ptr<MIQP::MIQPState> _state;

    Eigen::VectorXd _H_N_r;

    unsigned int _k;   
    
    Eigen::MatrixXd _S_wu;
    Eigen::MatrixXd _S_gamma;
    Eigen::MatrixXd _P_Gamma;
    Eigen::MatrixXd _R_Gamma;
    Eigen::VectorXd _One_Gamma;
    Eigen::MatrixXd _S_alpha;
    Eigen::MatrixXd _S_beta;
    Eigen::MatrixXd _P_Alpha;
    Eigen::MatrixXd _R_Alpha;
    Eigen::MatrixXd _P_Beta;
    Eigen::MatrixXd _R_Beta;
};

#endif