WMG.h

Go to the documentation of this file.

#ifndef WMG_H
#define WMG_H


/****************************************
 * INCLUDES 
 ****************************************/

#include <string>
#include <vector>

#include "smpc_solver.h"

/****************************************
 * DEFINES
 ****************************************/



/****************************************
 * TYPEDEFS 
 ****************************************/
class footstep;




enum WMGret
{
    WMG_OK,
    WMG_HALT
};

enum fs_type
{
    FS_TYPE_AUTO,   // let the library decide
    FS_TYPE_SS_L,   // left single support
    FS_TYPE_SS_R,   // right single support
    FS_TYPE_DS      // double support
};



class smpc_parameters
{
    public:
        smpc_parameters (
                const unsigned int, 
                const double, 
                const double gravity_ = 9.81);

        ~smpc_parameters();



// variables
        double hCoM;    

        double gravity; 

        double *T;      

        double *h;      
        double h0;      

        double *angle;

        double *fp_x;
        double *fp_y;
        
        double *lb;
        double *ub;

        double *zref_x;
        double *zref_y;


        smpc::state_com init_state;

        double *X;
};



class defConstraints
{
    public:
        double ss_left[4];
        double ss_right[4];

        double ds[4];

        double auto_ds[4];

        double support_distance_y;


        defConstraints()
        {
            // 2*HipOffsetY = 2*0.05
            support_distance_y = 0.1;
        }

        void init()
        {
            // measured using ruler =)
            ss_left[0] = ss_right[0] = 0.09;
            ss_left[1] = ss_right[1] = 0.025;
            ss_left[2] = ss_right[2] = 0.03;
            ss_left[3] = ss_right[3] = 0.025;

            ds[0] = 0.09;
            ds[1] = 0.025 + support_distance_y/2;
            ds[2] = 0.03;
            ds[3] = 0.025 + support_distance_y/2;

            // SS constraints with increased safety margin
            auto_ds[0] = 0.07;
            auto_ds[1] = 0.025;
            auto_ds[2] = 0.025;
            auto_ds[3] = 0.025;
        };


        void init_FSR()
        {
            /*
               Positions of forse sensitve resistors (with respect to ankle frame
               of respective foot):

              Left foot                                              
                (0.07025 , 0.0299)  *---------* (0.07025 , -0.0231)   
                                    |         |                       
                                    |    ^    /
                                    |    |x  /
                                    | <--+   |
                                    |  y     |
                (-0.03025 , 0.0299) *--------* (-0.02965 , -0.0191)
                
                Rectangle:      
                ( 0.07  , 0.029)  ( 0.07  , -0.019)
                (-0.029 , 0.029)  (-0.029 , -0.019)


              Right foot                                                    
                (0.07025 , 0.0231)  *---------* (0.07025 , -0.0299)
                                    |         |                             
                                    \    ^    |
                                     \   |x   |
                                     |<--+    |
                                     | y      |
                (-0.03025 , 0.0191)  *--------* (-0.02965 , -0.0299)

                Rectangle:
                ( 0.07  , 0.019)  ( 0.07  , -0.029)
                (-0.029 , 0.019)  (-0.029 , -0.029)
             */

            ss_left[0] = 0.07;
            ss_left[1] = 0.029;
            ss_left[2] = 0.029;
            ss_left[3] = 0.019;

            ss_right[0] = 0.07;
            ss_right[1] = 0.019;
            ss_right[2] = 0.029;
            ss_right[3] = 0.029;


            ds[0] = 0.07;
            ds[1] = 0.029 + support_distance_y/2;
            ds[2] = 0.029;
            ds[3] = 0.029 + support_distance_y/2;

            auto_ds[0] = 0.065;
            auto_ds[1] = 0.020;
            auto_ds[2] = 0.025;
            auto_ds[3] = 0.020;
        };
};



class WMG
{
    public:

        // --------------------

        WMG (   const unsigned int N_,
                const unsigned int T_, 
                const double step_height_ = 0.0135,
                double bezier_weight_1_ = 1.0,
                double bezier_weight_2_ = 2.0,
                double bezier_inclination_1_ = 0.01,
                double bezier_inclination_2_ = 0.008,
                bool use_fsr_constraints = false);

        ~WMG();


        void setFootstepParameters (
                const unsigned int def_periods,
                const unsigned int ds_periods, 
                const unsigned int ds_number,
                bool use_user_constraints_ = false);


        void setFootstepParametersMS (
                const unsigned int def_time_ms_,
                const unsigned int ds_time_ms_, 
                const unsigned int ds_number,
                bool use_user_constraints_ = false);


        void addFootstep(
                const double, 
                const double, 
                const double, 
                fs_type type = FS_TYPE_AUTO);


        WMGret formPreviewWindow (smpc_parameters &);


        // --------------------


        void getFeetPositions (
                const unsigned int shift_from_current_ms, 
                double * left_foot_pos, 
                double * right_foot_pos);


        bool isSupportSwitchNeeded ();


        void changeNextSSPosition (const double *posture, const bool zero_z_coordinate);


        void repositionFootsteps (const double diff_x, const double diff_y);


        // --------------------

        void FS2file(const std::string filename, const bool plot_ds = true);


        void getFootsteps(
                std::vector<double> &x_coord,
                std::vector<double> &y_coord,
                std::vector<double> &angle_rot);

        // --------------------


// variables
        std::vector<footstep> FS; 


        unsigned int N;

        unsigned int *T_ms;
        unsigned int sampling_period;

        
        int current_step_number;

        int first_preview_step;

        double step_height;


        defConstraints def_constraints;

        double user_constraints[4];
        double user_constraints_auto_ds[4];
        bool use_user_constraints;




        double bezier_weight_1;
        double bezier_weight_2;
        double bezier_inclination_1;
        double bezier_inclination_2;

    private:
        void getDSFeetPositions (const int, double *, double *);
        void getSSFeetPositions (const int, const double, double *, double *);
        void getSSFeetPositionsBezier (const int, const double, double *, double *);
        int getNextSS (const int, const fs_type type = FS_TYPE_AUTO);
        int getPrevSS (const int, const fs_type type = FS_TYPE_AUTO);

        unsigned int def_time_ms;
        unsigned int ds_time_ms;
        unsigned int ds_num;

        unsigned int last_time_decrement;
};

#endif /*WMG_H*/