update of CPPMPC to a newer version of MATMPC generated model

.gitignore

@@ -0,0 +1,4 @@
+lib/
+**/*.o
+data.txt
+examples/*_d*

examples/InvertedPendulum.cpp

@@ -1,53 +1,55 @@
 #include <iostream>
 #include <fstream>
-#include <math.h>
 #include "mpc_common.hpp"
 #include "rti_step.hpp"
 #include "casadi_wrapper.hpp"
 
 using namespace std;
-int main()
-{  
 
+int main()
+{
     // define problem size
     model_size size;
     size.nx = 4;
     size.nu = 1;
     size.ny = 5;
-    size.nyN= 4;
+    size.nyN = 4;
     size.np = 0;
     size.nbu = 1;
     size.nbx = 1;
     size.nbg = 0;
     size.nbgN = 0;
     size.N = 40;
-    size.nbx_idx = new int[size.nbx];            
-    size.nbx_idx[0]=0;
-
+    size.nbx_idx = new int[size.nbx];
+    size.nbx_idx[0] = 0;
+    size.nbu_idx = new int[size.nbu];
+    size.nbu_idx[0] = 0;
+
     // create a RTI controller
     rti_step_workspace rti_work(size);
 
     // initial condition and parameters
     rti_work.x0(1) = M_PI;
 
-    int i;
-    for(i=0;i<size.N+1;i++){
+    for(int i=0;i<size.N+1;i++)
         rti_work.QP.in.x(1,i) = M_PI;
-    }
-    rti_work.QP.in.W(0,0) = 10; 
-    rti_work.QP.in.W(1,1) = 10; 
-    rti_work.QP.in.W(2,2) = 0.1; 
-    rti_work.QP.in.W(3,3) = 0.1; 
-    rti_work.QP.in.W(4,4) = 0.01;
-
-    rti_work.QP.in.WN(0,0) = 10; 
-    rti_work.QP.in.WN(1,1) = 10; 
-    rti_work.QP.in.WN(2,2) = 0.1; 
-    rti_work.QP.in.WN(3,3) = 0.1;
-
-    rti_work.QP.in.lbu(0) = -20; 
+
+    rti_work.QP.in.W(0,0) = 10;
+    rti_work.QP.in.W(1,0) = 10;
+    rti_work.QP.in.W(2,0) = 0.1;
+    rti_work.QP.in.W(3,0) = 0.1;
+    rti_work.QP.in.W(4,0) = 0.01;
+    for(int i=1;i<size.N;i++)
+        rti_work.QP.in.W.col(i) = rti_work.QP.in.W.col(0);
+
+    rti_work.QP.in.WN(0) = 10;
+    rti_work.QP.in.WN(1) = 10;
+    rti_work.QP.in.WN(2) = 0.1;
+    rti_work.QP.in.WN(3) = 0.1;
+
+    rti_work.QP.in.lbu(0) = -20;
     rti_work.QP.in.ubu(0) = 20;
-    rti_work.QP.in.lbx(0) = -2; 
+    rti_work.QP.in.lbx(0) = -2;
     rti_work.QP.in.ubx(0) = 2;
 
     rti_work.QP.in.reg = 1E-8;
@@ -63,11 +65,14 @@ int main()
     ofstream myfile;
     myfile.open ("data.txt");
 
+    rti_work.info();
+    myfile <<"Sample 0: " << rti_work.x0.transpose() << " | - |OBJ=: "<<rti_work.OBJ <<" |CPT=: " << "" <<endl;
     // start the simulation
-    while(t<Tf){
-        
+    while (t < Tf)
+    {
         // call RTI solving routine
         rti_work.step();
+        rti_work.info();
 
         // feedback
         simu_in[0] = rti_work.QP.in.x.col(0).data();
@@ -80,24 +85,25 @@ int main()
         t += Ts;
 
         // store the closed-loop results
-        myfile <<"Sample " << rti_work.sample <<": " << rti_work.x0.transpose() << " | " << rti_work.QP.in.u.col(0) <<" |CPT=: " << rti_work.CPT << "ms" <<endl;
+        myfile <<"Sample " << rti_work.sample <<": " << rti_work.x0.transpose() << " | " << rti_work.QP.in.u.col(0) << " |OBJ=: "<<rti_work.OBJ <<" |CPT=: " << rti_work.CPT << "ms" <<endl;
 
         // shifting(optional)
-        for(i=0;i<size.N-1;i++){
+        for (int i=0; i<size.N-1; i++)
+        {
             rti_work.QP.in.x.col(i) = rti_work.QP.in.x.col(i+1);
             rti_work.QP.in.u.col(i) = rti_work.QP.in.u.col(i+1);
         }
-        rti_work.QP.in.x.col(size.N-1) = rti_work.QP.in.x.col(size.N);   
-
+        rti_work.QP.in.x.col(size.N-1) = rti_work.QP.in.x.col(size.N);
     }
 
     // free memory
     myfile.close();
-      
+
     delete [] size.nbx_idx;
     size.nbx_idx = NULL;
-
+    size.nbu_idx = NULL;
+
     rti_work.free();
 
     return 0;
-}
+}

include/casadi_wrapper.hpp

@@ -9,11 +9,11 @@ void impl_jac_u_Fun(double **in, double **out);
 void impl_jac_xdot_Fun(double **in, double **out);
 void F_Fun(double **in, double **out);
 void D_Fun(double **in, double **out);
-void Ji_Fun(double **in, double **out);
+void Hi_Fun(double **in, double **out);
 void gi_Fun(double **in, double **out);
 void path_con_Fun(double **in, double **out);
 void Ci_Fun(double **in, double **out);
-void JN_Fun(double **in, double **out);
+void HN_Fun(double **in, double **out);
 void gN_Fun(double **in, double **out);
 void path_con_N_Fun(double **in, double **out);
 void CN_Fun(double **in, double **out);

include/qp_problem.hpp

@@ -1,15 +1,15 @@
 #ifndef QP_PROBLEM_H_
 #define QP_PROBLEM_H_
 
-#include <Eigen/Dense>
+#include <eigen3/Eigen/Dense>
 #include "mpc_common.hpp"
 
 using namespace Eigen;
 
 class qp_in{
     public:
         MatrixXd x;
-        MatrixXd u; 
+        MatrixXd u;
         MatrixXd y;
         VectorXd yN;
         MatrixXd W;
@@ -42,27 +42,18 @@ class qp_data{
         MatrixXd Cgx;
         MatrixXd CgN;
         MatrixXd Cgu;
-        VectorXd lb_u;
-        VectorXd ub_u;
         VectorXd lb_x;
         VectorXd ub_x;
+        VectorXd lb_u;
+        VectorXd ub_u;
         VectorXd lb_g;
         VectorXd ub_g;
-
-        qp_data& init(model_size& size);
-};
-
-class qp_workspace{
-    public:
-        MatrixXd Jx;
-        MatrixXd Ju;
-        MatrixXd JxN;
 
-        qp_workspace& init(model_size& size);
+        qp_data& init(model_size& size);
 };
 
 class qp_out{
-    public: 
+    public:
         MatrixXd dx;
         MatrixXd du;
         MatrixXd lam;
@@ -77,12 +68,12 @@ class qp_problem{
     public:
         qp_in in;
         qp_data data;
-        qp_workspace work;
         qp_out out;
 
         qp_problem& init(model_size& size);
         qp_problem& generateQP(model_size& size);
         qp_problem& expandSol(model_size& size, VectorXd& x0);
+        qp_problem& info(model_size& size, double& OBJ);
 };
 
-#endif
+#endif

include/rti_step.hpp

@@ -17,16 +17,16 @@ class rti_step_workspace{
         VectorXd x0;     // current state
         int sample;      // current sample
         double CPT;      // computation time
-
+        double OBJ;
         rti_step_workspace& init(model_size& size);
         rti_step_workspace& step();
         rti_step_workspace& free();
+        rti_step_workspace& info();
 
     private:
-        Timer timer;       
+        Timer timer;
         full_condensing_workspace cond_work;
-        qpoases_workspace qpoases_work; 
-
+        qpoases_workspace qpoases_work;
 };
 
-#endif
+#endif