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

Go to the documentation of this file.

#ifndef WALKINGCLIENT_H
#define WALKINGCLIENT_H

#include <ocra-icub/IcubClient.h>
#include <ocra-recipes/TrajectoryThread.h>
#include <ocra-recipes/ControllerClient.h>
#include <ocra/util/EigenUtilities.h>
#include "walking-client/ZmpPreviewController.h"
#include "walking-client/StepController.h"
#include "walking-client/utils.h"
#include "walking-client/MIQPController.h"
#include "walking-client/Interpolator.h"
#include <ocra/util/FileOperations.h>
#include <yarp/os/Time.h>
#include "gurobi_c++.h"
#include "Gurobi.h"

class WalkingClient : public ocra_recipes::ControllerClient
{
DEFINE_CLASS_POINTER_TYPEDEFS(WalkingClient)

public:
    WalkingClient (std::shared_ptr<ocra::Model> modelPtr, const int loopPeriod);
    virtual ~WalkingClient ();

    bool configure(yarp::os::ResourceFinder &rf);


    void printHelp();


    bool queryMIQPSolution(const int miqpPeriod, const int miqpPreviewPeriod, const int clientPeriod, Eigen::VectorXd &preview, Eigen::VectorXd &tPreview);

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

    yarp::os::BufferedPort<yarp::sig::Vector> portWrenchLeftFoot;

    yarp::os::BufferedPort<yarp::sig::Vector> portWrenchRightFoot;

    std::vector<Eigen::Vector2d> generateZMPTrajectoryTEST(const double tTrans,
                                                           const double feetSeparation,
                                                           const double timeStep,
                                                           const int    amplitudeFraction,
                                                           const int N);

    bool getFeetSeparation(Eigen::Vector3d &sep);

    bool publish3dQuantity(yarp::os::BufferedPort<yarp::os::Bottle> &port, Eigen::Vector3d &value);

    void performZMPTest(ZmpTestType type);


    void performZMPPreviewTest(ZmpTestType type);
    
    void performZMPPreviewTestIntCoM(ZmpTestType type);
    
    void performMIQPTest();

    void performSingleStepTest();

    void steppingTest();

    std::string composePortName(std::string portName);

    void findZMPPreviewControllerParams(yarp::os::ResourceFinder &rf);
    void findGeneralTestsParams(yarp::os::ResourceFinder &rf);
    void findCOMLinVelConstRefParams(yarp::os::ResourceFinder &rf);
    void findZMPConstRefParams(yarp::os::ResourceFinder &rf);
    void findSingleStepTestParams(yarp::os::ResourceFinder &rf);
    void findZMPVaryingReferenceParams(yarp::os::ResourceFinder &rf);
    void findMIQPParams(yarp::os::ResourceFinder &rf);
    void findSteppingTestParams(yarp::os::ResourceFinder &rf);

    void transformStdVectorToEigenVector(std::vector< Eigen::Vector2d >& fullTraj, int from, int Nc, VectorXd& output);


    std::vector< Eigen::Vector2d > generateZMPStepTrajectoryTEST(double feetSeparation, double period, double duration, double riseTime, double constantReferenceY);

    std::vector<Eigen::Vector2d> generateZMPSingleStepTrajectory(double period, double feetSeparation);

    void generateStepPattern();

    std::vector<Eigen::Vector2d> generateZMPSteppingTrajectory();

    void startSteppinMotherFucker(int &stepTrigger, double &error);

    void prepareAndsetDesiredCoMTaskState(Eigen::VectorXd comState, bool doSet);



protected:
    virtual bool initialize();
    virtual void release();
    virtual void loop();

private:
    // Variables for STEPPING_TEST.
    std::vector<FOOT> _stepOrder;
    Eigen::MatrixXd _stepTargets;
    Eigen::VectorXd _stepTargetDurations;
    bool _currentlyStepping;
    bool _waitBeforeNextStep;
    double _waitTimeStart;
    int _currentStepIndex;
    std::vector<Eigen::Vector2d> _steppingTrajectory;
    steppingTestParams _steppingTestParams;


    // General Variables
    std::shared_ptr<ZmpPreviewParams> _zmpPreviewParams;
    std::shared_ptr<ZmpPreviewController> _zmpPreviewController;
    std::shared_ptr<ocra_recipes::TaskConnection> _comTask;
    std::shared_ptr<MIQPController> _miqpController;
    std::shared_ptr<StepController> _stepController;
    std::vector<Eigen::Vector2d> _zmpTrajectory;
    std::vector<Eigen::Vector2d> _singleStepTrajectory;
    ocra::TaskState _desiredComState;
    MIQPParameters _miqpParams;
    Eigen::VectorXd _rawLeftFootWrench;
    Eigen::VectorXd _rawRightFootWrench;
    Eigen::Vector2d _globalZMP;
    Eigen::Vector2d _previousCOM;
    Eigen::Vector2d _previousCOMVel;
    std::string _clientName;
    std::string _robot;
    int _period;
    bool _isTestRun;
    std::string _testType;
    ZmpTestType _zmpTestType;
    std::string _homeDataDir;
    double _comYConstVel;
    double _stopTimeConstComVel;
    double _zmpYConstRef;
    double _stopTimeConstZmp;
    double _riseTimeConstZmp;
    double _trajectoryDurationConstZmp;
    double _constantReferenceY;
    singleStepTestParams _singleStepTestParams;
    double _tTrans;
    int _numberOfTransitions;
    int _amplitudeFraction;
    double _stopTimeVaryingZmp;
    int _k;

    yarp::os::BufferedPort<yarp::os::Bottle> _zmpPort;
    yarp::os::BufferedPort<yarp::os::Bottle> _dcomErrorPort;
    yarp::os::BufferedPort<yarp::os::Bottle> _dComDesPort;
    yarp::os::BufferedPort<yarp::os::Bottle> _dComCurPort;
    yarp::os::BufferedPort<yarp::os::Bottle> _zmpDesPort;
    yarp::os::BufferedPort<yarp::os::Bottle> _zmpCurPort;
    yarp::os::BufferedPort<yarp::os::Bottle> _comCurrent;
    yarp::os::BufferedPort<yarp::os::Bottle> _ddcomCurrent;
    yarp::os::BufferedPort<yarp::os::Bottle> _ddcomFromZMP;
    Eigen::VectorXd _hkkPrevious;
    bool _firstLoop;
    Eigen::VectorXd zmpRefInPreviewWindow;
    Eigen::VectorXd comVelRefInPreviewWindow;
    Eigen::VectorXd optimalU;

    // MIQP-related variables
    Eigen::VectorXd _X_kn;
    Eigen::VectorXd _t_kn;
};


#endif // TEST_CLIENT_H