smpc_solver.h

Go to the documentation of this file.

#ifndef SMPC_SOLVER_H
#define SMPC_SOLVER_H

#include <vector>


class qp_as;
class qp_ip;



#define SMPC_NUM_STATE_VAR 6
#define SMPC_NUM_CONTROL_VAR 2
#define SMPC_NUM_VAR 8

namespace smpc
{
    // -------------------------------

    
    void enable_fexceptions();


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


    class state
    {
        public:
            double state_vector[SMPC_NUM_STATE_VAR];


            state();


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


            double &x  () {return state_vector[0];};
            double &y  () {return state_vector[3];};
            const double &x  () const {return state_vector[0];};
            const double &y  () const {return state_vector[3];};

            double &vx () {return state_vector[1];};
            double &vy () {return state_vector[4];};
            const double &vx () const {return state_vector[1];};
            const double &vy () const {return state_vector[4];};

            double &ax () {return state_vector[2];};
            double &ay () {return state_vector[5];};
            const double &ax () const {return state_vector[2];};
            const double &ay () const {return state_vector[5];};



            // -------------------------------
            
           
            void set (double x_, double y_);


            void set (double x_, double vx_, double ax_,
                     double y_, double vy_, double ay_);
    };


    class state_com : public state
    {
        public:
            state_com () : state () {};
    };

    
    class state_zmp : public state
    {
        public:
            state_zmp () : state () {};
    };


    class control
    {
        public:
            double control_vector[SMPC_NUM_CONTROL_VAR];


            control();

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


            double &jx () {return control_vector[0];};
            double &jy () {return control_vector[1];};
    };



    class solver
    {
        public:
            virtual ~solver() = 0;

            virtual void set_parameters (
                    const double* T,
                    const double* h,
                    const double h_initial,
                    const double* angle,
                    const double* zref_x,
                    const double* zref_y,
                    const double* lb,
                    const double* ub) = 0;



            virtual void form_init_fp (
                    const double *x_coord,
                    const double *y_coord,
                    const state_com &init_state,
                    double* X) = 0;

            virtual void form_init_fp (
                    const double *x_coord,
                    const double *y_coord,
                    const state_zmp &init_state,
                    double* X) = 0;


            virtual void solve () = 0;


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



            virtual void get_next_state (state_com &s) const = 0;
            virtual void get_next_state (state_zmp &s) const = 0;
            

            virtual void get_state (state_com &s, const int ind) const = 0;
            virtual void get_state (state_zmp &s, const int ind) const = 0;


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


            virtual void get_first_controls (control &c) const = 0;


            virtual void get_controls (control &c, const int ind) const = 0;
    };




    class solver_as : public solver
    {
        public:

            solver_as (
                    const int N, 
                    const double gain_position = 2000.0, 
                    const double gain_velocity = 150.0, 
                    const double gain_acceleration = 0.02,
                    const double gain_jerk = 1.0,
                    const double tol = 1e-7,
                    const unsigned int max_added_constraints_num = 0,
                    const bool constraint_removal_on = true,
                    const bool obj_computation_on = false);


            ~solver_as();


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


            void set_parameters (
                    const double*, const double*, const double, const double*, 
                    const double*, const double*, const double*, const double*);
            void form_init_fp (const double *, const double *, const state_com &, double*);
            void form_init_fp (const double *, const double *, const state_zmp &, double*);
            void solve ();
            void get_next_state (state_com &) const;
            void get_next_state (state_zmp &) const;
            void get_state (state_com &, const int) const;
            void get_state (state_zmp &, const int) const;
            void get_first_controls (control &) const;
            void get_controls (control &, const int) const;


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

       
            unsigned int added_constraints_num;

            unsigned int removed_constraints_num;

            unsigned int active_set_size;


            std::vector<double> objective_log;


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


            qp_as *qp_sol;
    };


    enum backtrackingSearchType
    {
        SMPC_IP_BS_NONE = 0,
        SMPC_IP_BS_LOGBAR = 1,
        SMPC_IP_BS_ORIGINAL = 2
    };


    class solver_ip : public solver
    {
        public:

            solver_ip (
                    const int N, 
                    const double gain_position = 2000.0, 
                    const double gain_velocity = 150.0, 
                    const double gain_acceleration = 0.01,
                    const double gain_jerk = 1.0,
                    const double tol = 1e-3,
                    const double tol_out = 1e-2,
                    const double t = 100,
                    const double mu = 15,
                    const double bs_alpha = 0.01,
                    const double bs_beta = 0.5,
                    const int unsigned max_iter = 0,
                    const backtrackingSearchType bs_type = SMPC_IP_BS_LOGBAR,
                    const bool obj_computation_on = false);

            ~solver_ip();


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


            void set_parameters (
                    const double*, const double*, const double, const double*, 
                    const double*, const double*, const double*, const double*);
            void form_init_fp (const double *, const double *, const state_com &, double*);
            void form_init_fp (const double *, const double *, const state_zmp &, double*);
            void solve ();
            void get_next_state (state_com &) const;
            void get_next_state (state_zmp &) const;
            void get_state (state_com &, const int) const;
            void get_state (state_zmp &, const int) const;
            void get_first_controls (control &) const;
            void get_controls (control &, const int) const;


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


            unsigned int ext_loop_iterations;

            unsigned int int_loop_iterations;

            unsigned int bt_search_iterations;


            std::vector<double> objective_log;


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


            qp_ip *qp_sol;
    };
}

#endif /*SMPC_SOLVER_H*/