Debug MPC functions

include/example-adder/MPC.hpp

@@ -9,7 +9,10 @@
 #include <Eigen/Dense>
 #include "osqp_folder/include/osqp.h"
 #include "osqp_folder/include/cs.h"
-
+#include "osqp_folder/include/auxil.h"
+#include "osqp_folder/include/util.h"
+#include "osqp_folder/include/osqp_configure.h"
+#include "other/st_to_cc.hpp"
 class MPC
 {
 private:
@@ -20,23 +23,26 @@ private:
     Eigen::Matrix<double, 6, 1> q;
     Eigen::Matrix<double, 6, 1> v = Eigen::Matrix<double, 6, 1>::Zero();
     Eigen::Matrix<double, 3, 4> footholds = Eigen::Matrix<double, 3, 4>::Zero();
+    Eigen::Matrix<double, 1, 12> footholds_tmp = Eigen::Matrix<double, 12, 1>::Zero();
     Eigen::Matrix<double, 3, 4> lever_arms = Eigen::Matrix<double, 3, 4>::Zero();
     Eigen::Matrix<int, 20, 5> gait = Eigen::Matrix<int, 20, 5>::Zero();
     Eigen::Matrix<double, 12, 1> g = Eigen::Matrix<double, 12, 1>::Zero();
 
-    Eigen::Matrix<double, 12, 12> A = Eigen::Matrix<double, 12, 12>::Zero();
+    Eigen::Matrix<double, 12, 12> A = Eigen::Matrix<double, 12, 12>::Identity();
     Eigen::Matrix<double, 12, 12> B = Eigen::Matrix<double, 12, 12>::Zero();
     Eigen::Matrix<double, 12, 1> x0 = Eigen::Matrix<double, 12, 1>::Zero();
+    double x_next [12] = {};
     double f_applied [12] = {};
 
     // Matrix ML
     const static int size_nz_ML = 5000;
-    c_int r_ML [size_nz_ML] = {}; // row indexes of non-zero values in matrix ML
-    c_int c_ML [size_nz_ML] = {}; // col indexes of non-zero values in matrix ML
-    c_float v_ML [size_nz_ML] = {};  // non-zero values in matrix ML
+    //int r_ML [size_nz_ML] = {}; // row indexes of non-zero values in matrix ML
+    //int c_ML [size_nz_ML] = {}; // col indexes of non-zero values in matrix ML
+    //double v_ML [size_nz_ML] = {};  // non-zero values in matrix ML
+    //csc* ML_triplet; // Compressed Sparse Column matrix (triplet format)
     csc* ML; // Compressed Sparse Column matrix
-    inline void add_to_ML(int i, int j, double v); // function to fill the triplet r/c/v
-    inline void add_to_P(int i, int j, double v); // function to fill the triplet r/c/v
+    inline void add_to_ML(int i, int j, double v, int *r_ML, int *c_ML, double *v_ML); // function to fill the triplet r/c/v
+    inline void add_to_P(int i, int j, double v, int *r_P, int *c_P, double *v_P); // function to fill the triplet r/c/v
 
     // Indices that are used to udpate ML
     int i_x_B [12*4] = {};
@@ -48,12 +54,13 @@ private:
     const static int size_nz_NK = 5000;
     double v_NK_up [size_nz_NK] = {};  // maxtrix NK (upper bound)
     double v_NK_low [size_nz_NK] = {};  // maxtrix NK (lower bound)
+    double v_warmxf [size_nz_NK] = {};  // maxtrix NK (lower bound)
 
     // Matrix P
     const static int size_nz_P = 5000;
-    c_int r_P [size_nz_P] = {}; // row indexes of non-zero values in matrix ML
-    c_int c_P [size_nz_P] = {}; // col indexes of non-zero values in matrix ML
-    c_float v_P [size_nz_P] = {};  // non-zero values in matrix ML
+    //c_int r_P [size_nz_P] = {}; // row indexes of non-zero values in matrix ML
+    //c_int c_P [size_nz_P] = {}; // col indexes of non-zero values in matrix ML
+    //c_float v_P [size_nz_P] = {};  // non-zero values in matrix ML
     csc* P; // Compressed Sparse Column matrix
 
     // Matrix Q
@@ -62,12 +69,12 @@ private:
 
     // OSQP solver variables
     OSQPWorkspace * workspce = new OSQPWorkspace();
-    OSQPData * data = new OSQPData();
-    OSQPSettings * settings = new OSQPSettings();
+    OSQPData * data;
+    OSQPSettings * settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings));
 
     // Matrices whose size depends on the arguments sent to the constructor function
     Eigen::Matrix<double, 12, Eigen::Dynamic> xref;
-    Eigen::Matrix<double, 12, Eigen::Dynamic> x;
+    Eigen::Matrix<double, Eigen::Dynamic, 1> x;
     Eigen::Matrix<int, Eigen::Dynamic, 1> S_gait;
     Eigen::Matrix<double, Eigen::Dynamic, 1> warmxf;
     Eigen::Matrix<double, Eigen::Dynamic, 1> NK_up;
@@ -89,7 +96,8 @@ public:
     int call_solver(int);
     int retrieve_result();
     double * get_latest_result();
-    int run(Eigen::Matrix<double, 12, Eigen::Dynamic> xref_in, Eigen::Matrix<double, 20, 13> fsteps_in);
+    double * get_x_next();
+    int run(int num_iter, Eigen::Matrix<double, 12, Eigen::Dynamic> xref_in, Eigen::Matrix<double, 20, 13> fsteps_in);
     
     Eigen::Matrix<double, 3, 3> getSkew(Eigen::Matrix<double, 3, 1> v);
     int construct_S();
@@ -97,6 +105,7 @@ public:
     
     // Accessor
     double gethref() { return h_ref; }
+    void my_print_csc_matrix(csc *M, const char *name);
 
     //Eigen::Matrix<double, 12, 12> getA() { return A; }
     //Eigen::MatrixXf getML() { return ML; }

src/main.cpp

@@ -4,6 +4,7 @@
 #include <Eigen/Core>
 #include "example-adder/gepadd.hpp"
 #include "example-adder/MPC.hpp"
+#include "other/st_to_cc.hpp"
 
 int main(int argc, char** argv) {
   if (argc == 3) {
@@ -11,8 +12,29 @@ int main(int argc, char** argv) {
     std::cout << "The sum of " << a << " and " << b << " is: ";
     std::cout << gepetto::example::add(a, b) << std::endl;
 
-    MPC test(0.001f, 2, 0.32f);
-    test.create_matrices();
+    Eigen::Matrix<double, 20, 13> test_fsteps = Eigen::Matrix<double, 20, 13>::Zero();
+    test_fsteps.row(0) << 15, 0.19, 0.15, 0.0,  0.0,   0.0, 0.0,   0.0,  0.0, 0.0, -0.19, -0.15, 0.0;
+    test_fsteps.row(1) << 1,  0.19, 0.15, 0.0, 0.19, -0.15, 0.0, -0.19, 0.15, 0.0, -0.19, -0.15, 0.0;
+    test_fsteps.row(2) << 15,  0.0,  0.0, 0.0, 0.19, -0.15, 0.0, -0.19, 0.15, 0.0,   0.0,   0.0, 0.0;
+    test_fsteps.row(3) << 1,  0.19, 0.15, 0.0, 0.19, -0.15, 0.0, -0.19, 0.15, 0.0, -0.19, -0.15, 0.0;
+
+    Eigen::Matrix<double, 12, Eigen::Dynamic> test_xref = Eigen::Matrix<double, 12, Eigen::Dynamic>::Zero(12, 33);
+    test_xref.row(2) = 0.17 * Eigen::Matrix<double, 1, Eigen::Dynamic>::Ones(1, 33);
+
+    MPC test(0.02f, 32, 0.64f);
+    test.run(0, test_xref, test_fsteps);
+    double * result;
+    result = test.get_latest_result();
+    /*for (int i = 0; i < 12; i++)
+    {
+      std::cout << *(result+i) << ", ";
+    }*/
+    test_fsteps(0, 0) = 14;
+    test_fsteps.row(4) = test_fsteps.row(0);
+    test_fsteps(4, 0) = 1;
+
+    test.run(1, test_xref, test_fsteps);
+
     std::cout << test.gethref() << std::endl;
     /*std::cout << test.getA() << std::endl;
     std::cout << test.getML() << std::endl;
@@ -20,7 +42,7 @@ int main(int argc, char** argv) {
     std::cout << test.getMonth() << std::endl;
     std::cout << test.getYear() << std::endl;*/
 
-    Eigen::MatrixXd m(2,2);
+    /*Eigen::MatrixXd m(2,2);
     m(0,0) = 3;
     m(1,0) = 2.5;
     m(0,1) = -1;
@@ -33,7 +55,9 @@ int main(int argc, char** argv) {
           13, 14, 15, 16, 17, 18;
     
     Eigen::Map<Eigen::MatrixXf> M2(M1.data(), M1.size(), 1);
-    std::cout << "M2:" << std::endl << M2 << std::endl;
+    std::cout << "M2:" << std::endl << M2 << std::endl;*/
+
+
 
 
     return EXIT_SUCCESS;