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

Go to the documentation of this file.

#ifndef _MIQP_STATE_H_
#define _MIQP_STATE_H_

#include <Eigen/Dense>
#include <ocra-icub/OcraWbiModel.h>
#include <ocra/util/ErrorsHelper.h>
#include "walking-client/utils.h"
#include <ocra-icub/Utilities.h>

namespace MIQP{
    enum StateVectorIndex{
        A_X=0,
        A_Y,
        B_X,
        B_Y,
        ALPHA_X,
        ALPHA_Y,
        BETA_X,
        BETA_Y,
        DELTA,
        GAMMA,
        H_X,
        H_Y,
        DH_X,
        DH_Y,
        DDH_X,
        DDH_Y
    };


    class MIQPState {
    private:
        Eigen::VectorXd _xi_k;
        Eigen::Vector2d _a;
        Eigen::Vector2d _b;
        Eigen::Vector2d _alpha;
        Eigen::Vector2d _beta;
        unsigned int          _delta;

        unsigned int          _gamma;

        Eigen::VectorXd _hk;

        ocra::Model::Ptr _robotModel;

        std::string _robot;

        /*
         * Right foot coordinates
         */
        Eigen::Vector3d _r_foot_coord;

        /*
         * Left foot coordinates
         */
        Eigen::Vector3d _l_foot_coord;

        /*
         * Port that connects to the left foot F/T sensor.
         */
        yarp::os::BufferedPort<yarp::sig::Vector> _portWrenchLeftFoot;

        /*
         * Port that connect to the right F/T sensor.
         */
        yarp::os::BufferedPort<yarp::sig::Vector> _portWrenchRightFoot;

        /*
         * Normal force threshold
         */
        double _FzThreshold; // N

        /*
         * Normal distance threshold
         */
        double _PzThreshold; // m

    public:

        MIQPState (ocra::Model::Ptr robotModel, std::string robot);

        virtual ~MIQPState ();

        bool initialize();

        void updateStateVector();

        void updateBaseOfSupportDescriptors(Eigen::Vector2d &a,
                                            Eigen::Vector2d &b,
                                            Eigen::Vector2d &alpha,
                                            Eigen::Vector2d &beta,
                                            unsigned int &delta,
                                            unsigned int &gamma,
                                            double thresholdChange);

        void updateHorizontalCoMState(Eigen::VectorXd &hk);

        bool isRobotInSS(FOOT &footInSS);

        bool isFootInContact(FOOT whichFoot, double FzThreshold, double PzThreshold);

        bool readFootWrench(FOOT whichFoot, Eigen::VectorXd &rawWrench);

        void getFullState(Eigen::VectorXd &xi);

        
        Eigen::Vector3d getLeftFootPosition();
        
        
        Eigen::Vector3d getRightFootPosition();
        
        friend std::ostream& operator<<(std::ostream &out, const MIQPState &state) {
            out << "----- State Vector xi_k ----- \n";
            out << "\ta    : [" << state._a.transpose()               << "]\n";
            out << "\tb    : [" << state._b.transpose()               << "]\n";
            out << "\talpha: [" << state._alpha.transpose()           << "]\n";
            out << "\tbeta : [" << state._beta.transpose()            << "]\n";
            out << "\tdelta:  " << state._delta                       << "]\n";
            out << "\tgamma:  " << state._gamma                       << " \n";
            out << "\th    : [" << state._hk.head(2).transpose()       << " \n";
            out << "\tdh   : [" << state._hk.segment<2>(2).transpose()<< "]\n";
            out << "\tddh  : [" << state._hk.tail(2).transpose()      << "]\n";
            return out;
        }

    };
} // end of namespace
#endif