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

Go to the documentation of this file.

#ifndef _BASE_OF_SUPPORT_H_
#define _BASE_OF_SUPPORT_H_

// Boost headers
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include "unsupported/Eigen/MatrixFunctions"
// Eigen headers
#include <Eigen/Core>
#include <walking-client/MIQPState.h>
#include <walking-client/StepController.h>
#include <walking-client/utils.h>
#include <ocra-recipes/TaskConnection.h>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)
typedef boost::tuple<double, double> point;
typedef boost::geometry::model::polygon<point> Polygon;
typedef boost::geometry::model::box<point> Box;

class BaseOfSupport {
private:
    Eigen::MatrixXd _Ab;
    
    Eigen::VectorXd _b;

    Eigen::MatrixXd _Cp;

    Eigen::MatrixXd _Ci;
    
    Eigen::VectorXd _f;

    Eigen::MatrixXd _A;
    
    Eigen::VectorXd _fbar;
    
    Eigen::MatrixXd _B;
    
    Eigen::VectorXd _rhs;
    
    Polygon _poly;
    Box _bbox;
    std::shared_ptr<StepController> _stepController;
    
    Eigen::MatrixXd _Q;
    
    Eigen::MatrixXd _T;
    
    MIQPParameters _miqpParams;
    
public:
    BaseOfSupport(std::shared_ptr<StepController> stepController, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& T, MIQPParameters miqpParams);
    
    virtual ~BaseOfSupport ();
    
    bool update(const Eigen::VectorXd& xi_k);
    
    void computeBoundingBox(const Eigen::MatrixXd &feetCorners, Eigen::Matrix2d &minMaxBoundingBox);
    
    void getA(Eigen::MatrixXd& A);
    
    void getrhs(Eigen::VectorXd& rhs);
    
protected:
    
    
    void buildAb();
    
    void buildb(const Eigen::Matrix2d& minMaxBoundingBox);
    
    void buildCp(double cz, double g);
    
    void buildCi(const Eigen::MatrixXd& Ab, const Eigen::MatrixXd& Cp);
    
    void buildf();
    
    void buildfbar(const Eigen::VectorXd& f);
    
    void buildB(const Eigen::MatrixXd& Ci, const Eigen::MatrixXd& Q);
    
    void buildA(const Eigen::MatrixXd& Ci, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& T);
};

#endif