ocra-wbi-plugins/ocra-icub-clients/walking-client/include/walking-client/constraints/MIQPLinearConstraints.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_LINEAR_CONSTRAINTS_H_
#define _MIQP_LINEAR_CONSTRAINTS_H_

#include "walking-client/constraints/ShapeConstraints.h"
#include "walking-client/constraints/AdmissibilityConstraints.h"
#include "walking-client/StepController.h"
#include "walking-client/BaseOfSupport.h"
#include "walking-client/utils.h"

class MIQPLinearConstraints {
private:
    std::shared_ptr<ShapeConstraints> _shapeCnstr;
    std::shared_ptr<AdmissibilityConstraints> _admissibilityCnstr;
    std::shared_ptr<StepController> _stepController;
    Eigen::MatrixXd _AShapeAdmiss;
    Eigen::MatrixXd _BShapeAdmiss;
    Eigen::VectorXd _fcbarShapeAdmiss;
    Eigen::MatrixXd _A;
    // f_c - B*Xi_k
    Eigen::VectorXd _rhs;
    Eigen::MatrixXd _Acr;
    Eigen::MatrixXd _Acl;
    
    Eigen::MatrixXd _Q;
    
    Eigen::MatrixXd _T;

    Eigen::MatrixXd _Bh;
    
    Eigen::MatrixXd _Ah;
    
    bool _addShapeCtrs;
    
    bool _addAdmissibilityCtrs;
    
    bool _addCoPConstraints;
    
    bool _addWalkingCtrs;

    unsigned int _dt;
    
    unsigned int _N;
    
    unsigned int _nConstraints;
    
    std::shared_ptr<BaseOfSupport> _baseOfSupport;
    
    MIQPParameters _miqpParams;
public:

    MIQPLinearConstraints(std::shared_ptr<StepController> stepController, MIQPParameters &miqpParams);
    
    virtual ~MIQPLinearConstraints ();
    
    void updateRHS(const Eigen::VectorXd& xi_k);
    
    void getConstraintsMatrixA(Eigen::MatrixXd &A);
    
    unsigned int getTotalNumberOfConstraints();

    void getRHS(Eigen::VectorXd &rhs);

protected:
    void buildShapeAndAdmissibilityInPreviewWindow();
    void buildAShapeAdmiss();
    void buildBShapeAdmiss();
    void buildFcBarShapeAdmiss();
    void setMatrixAcr();
    void setMatrixAcl();
    void buildMatrixQ();
    void buildMatrixT();
    void buildBh();
    void buildAh();
};
#endif