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

Go to the documentation of this file.

#ifndef _ZMPPREVIEWCONTROLLER_
#define _ZMPPREVIEWCONTROLLER_

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


struct ZmpPreviewParams {
    int Nc;
    int Np;
    double cz;
    double nu;
    double nw;
    double nb;
//     std::vector<Eigen::Vector2d> Pr;
//     std::vector<Eigen::Vector2d> Hr;

    ZmpPreviewParams(int    Np,
                     int    Nc,
                     double cz,
                     double nu,
                     double nw,
                     double nb):
    Np(Np),
    Nc(Nc),
    cz(cz),
    nu(nu),
    nw(nw),
    nb(nb){}

};

class ZmpPreviewController
{
public:
    ZmpPreviewController(const double period, std::shared_ptr<ZmpPreviewParams> parameters);

    virtual ~ZmpPreviewController();


    bool initialize();

    template <typename Derived>
    void computeOptimalInput(const Eigen::MatrixBase<Derived>& zmpRef,
                             const Eigen::MatrixBase<Derived>& comVelRef,
                             const Eigen::MatrixBase<Derived>& hk,
                             Eigen::MatrixBase<Derived>& optimalU) {
//     AOptimal = Hp.transpose()*Nb*Hp + Nu + Hh.transpose()*Nw*Hh;
//     bOptimal = Hp.transpose()*Nb*(zmpRef - Gp*hk) + Hh.transpose()*Nw*(comVelRef - Gh*hk);
//     optimalU = AOptimal.colPivHouseholderQr().solve(bOptimal);

//         double start = yarp::os::Time::now();
        //TODO: Remember that the bOptimal I have to use is actually the next line, not Weiber's because it doesn't take into account com velocity references.
//         bOptimal = Hp.transpose()*Nb*(zmpRef - Gp*hk) + Hh.transpose()*Nw*(comVelRef - Gh*hk);
//         OCRA_INFO("Time to compute bOptimal is: " << yarp::os::Time::now() - start);
        bOptimal = Hp.transpose() * (zmpRef - Gp * hk);
//         start = yarp::os::Time::now();
//         optimalU = AOptimal.colPivHouseholderQr().solve(Hp.transpose() * (bOptimal));
        // Using Cholesky decomposition
        optimalU = (AOptimal).llt().solve(bOptimal);
//         OCRA_INFO("Time to compute optimalU using Cholesky decomposition : " << yarp::os::Time::now() - start);
    }

    void integrateCom(Eigen::VectorXd comJerk, Eigen::VectorXd hk, Eigen::VectorXd &hkk);


    void tableCartModel(Eigen::Vector2d hk, Eigen::VectorXd ddhk, Eigen::Vector2d& p);

    void tableCartModel(Eigen::VectorXd hkk, Eigen::Vector2d& p);

    Eigen::MatrixXd buildAh(const double dt);

    Eigen::MatrixXd buildBh(const double dt);

    Eigen::MatrixXd buildCp(const double cz, const double g);

    Eigen::MatrixXd buildGp(Eigen::MatrixXd Cp, Eigen::MatrixXd Ah, const int Np);

    Eigen::MatrixXd buildHp(Eigen::MatrixXd Cp, Eigen::MatrixXd Bh, Eigen::MatrixXd Ah, const int Nc, const int Np);

    Eigen::MatrixXd buildCh();

    Eigen::MatrixXd buildGh(Eigen::MatrixXd Ch, Eigen::MatrixXd Ah, const int Np);

    Eigen::MatrixXd buildHh(Eigen::MatrixXd Ch, Eigen::MatrixXd Bh, Eigen::MatrixXd Ah, const int Nc, const int Np);

    Eigen::MatrixXd buildNu(const double nu, const int Nc);

    Eigen::MatrixXd buildNw(const double nw, const int Np);

    Eigen::MatrixXd buildNb(const double nb, const int Np);

    bool computeFootZMP(FOOT whichFoot,
                        Eigen::VectorXd wrench,
                        Eigen::Vector2d &footZMP,
                        Eigen::VectorXd &wrenchInWorldRef,
                        ocra::Model::Ptr model,
                        const double tolerance=1e-3);


    bool computeGlobalZMPFromSensors(Eigen::VectorXd rawLeftFootWrench,
                                     Eigen::VectorXd rawRightFootWrench,
                                     ocra::Model::Ptr model,
                                     Eigen::Vector2d &globalZMP);

    void getFTSensorAdjointMatrix(FOOT whichFoot, Eigen::MatrixXd &T, Eigen::Vector3d &sensorPosition, ocra::Model::Ptr model);


private:
    const double cz;
    const double g = 9.8;
    const double dt;
    const int Nc;
    const int Np;
    const double nu;
    const double nw;
    const double nb;
    const Eigen::MatrixXd Nu;
    const Eigen::MatrixXd Nw;
    const Eigen::MatrixXd Nb;
    const Eigen::MatrixXd Ah;
    const Eigen::MatrixXd Bh;
    const Eigen::MatrixXd Cp;
    const Eigen::MatrixXd Gp;
    const Eigen::MatrixXd Hp;
    const Eigen::MatrixXd Ch;
    const Eigen::MatrixXd Gh;
    const Eigen::MatrixXd Hh;

    Eigen::MatrixXd AOptimal;

    Eigen::MatrixXd bOptimal;



};

#endif