include/smpc_solver.h

Go to the documentation of this file.

#ifndef SMPC_SOLVER_H
#define SMPC_SOLVER_H

class qp_solver;



#define SMPC_NUM_STATE_VAR 6

#define SMPC_NUM_CONTROL_VAR 2

#define SMPC_NUM_VAR 8

namespace smpc
{
    class solver;



    class state
    {
        public:
            double state_vector[SMPC_NUM_STATE_VAR];


            state();

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


            double &x  () {return state_vector[0];};
            double &vx () {return state_vector[1];};
            double &ax () {return state_vector[2];};

            double &y  () {return state_vector[3];};
            double &vy () {return state_vector[4];};
            double &ay () {return state_vector[5];};


            // -------------------------------
            
           
            virtual void get_next_state (const solver &smpc_solver) = 0;


            virtual void get_state (const solver &smpc_solver, const int ind) = 0;


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


            void set (double x_, double y_);


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


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

            void get_next_state (const solver &smpc_solver);
            void get_state (const solver &smpc_solver, const int ind);
    };

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

            void get_next_state (const solver &smpc_solver);
            void get_state (const solver &smpc_solver, const int ind);
    };


    class control
    {
        public:
            double control_vector[SMPC_NUM_CONTROL_VAR];


            control();

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


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


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


            void get_first_controls (const solver &smpc_solver);


            void get_controls (const solver &smpc_solver, const int ind);
    };




    class solver
    {
        public:


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


            solver (
                    const int N, 
                    const double Alpha = 150.0, 
                    const double Beta = 2000.0, 
                    const double Gamma = 1.0,
                    const double regularization = 0.01,
                    const double tol = 1e-7);


            solver (
                    const int N, 
                    const int max_iter, // no default to avoid ambiguity
                    const double Alpha = 150.0, 
                    const double Beta = 2000.0, 
                    const double Gamma = 1.0,
                    const double regularization = 0.01,
                    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);


            ~solver();

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

            
            void enable_fexceptions();


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


            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);


            void form_init_fp (
                    const double *x_coord,
                    const double *y_coord,
                    const state_orig &init_state,
                    double* X);


            int solve ();
       

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

            qp_solver *qp_sol;
    };
}

#endif /*SMPC_SOLVER_H*/