Bindings working for a simple example

CMakeLists.txt

@@ -31,6 +31,7 @@ ADD_PROJECT_DEPENDENCY(Eigen3 REQUIRED)
 
 if(BUILD_PYTHON_INTERFACE)
   FINDPYTHON()
+  ADD_PROJECT_DEPENDENCY(eigenpy REQUIRED)
   string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
   set(${PY_NAME}_INSTALL_DIR ${PYTHON_SITELIB}/${PY_NAME})
   set(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
@@ -67,11 +68,14 @@ TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR
 # target_link_libraries(${PROJECT_NAME} PUBLIC /home/palex/Documents/Test_Cpp/example-adder/include/osqp_folder/lib/libosqp.so)
 # target_link_libraries(${PROJECT_NAME} PUBLIC /home/palex/libc/st_to_cc.o)
 
+#target_link_libraries(${PROJECT_NAME} PUBLIC )
+#target_include_directories(${PROJECT_NAME} PUBLIC /opt/openrobots/lib/libeigenpy.so)
+
 # Find OSQP library and headers
 find_package(osqp REQUIRED)
 
 # Link the OSQP shared library
-target_link_libraries(${PROJECT_NAME} PRIVATE osqp::osqp)
+target_link_libraries(${PROJECT_NAME} PRIVATE osqp::osqp eigenpy::eigenpy)
 
 if(SUFFIX_SO_VERSION)
   set_target_properties(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})

include/example-adder/MPC.hpp

@@ -13,6 +13,16 @@
 #include "osqp_folder/include/util.h"
 #include "osqp_folder/include/osqp_configure.h"
 #include "other/st_to_cc.hpp"
+
+#include "example-adder/gepadd.hpp"
+//#include <eigenpy/eigenpy.hpp>
+//#include <boost/python/suite/indexing/map_indexing_suite.hpp>
+//#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
+
+typedef Eigen::Matrix<double, 12, Eigen::Dynamic> typeXREF;
+typedef Eigen::Matrix<double, 20, 13> typeFSTEPS;
+typedef Eigen::MatrixXd matXd;
+
 class MPC
 {
 private:
@@ -84,6 +94,7 @@ private:
 
 public:
 
+    MPC();
     MPC(double dt_in, int n_steps_in, double T_gait_in);
 
     int create_matrices();
@@ -97,7 +108,7 @@ public:
     int retrieve_result();
     double * get_latest_result();
     double * get_x_next();
-    int run(int num_iter, Eigen::Matrix<double, 12, Eigen::Dynamic> xref_in, Eigen::Matrix<double, 20, 13> fsteps_in);
+    int ron(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();
@@ -107,6 +118,11 @@ public:
     double gethref() { return h_ref; }
     void my_print_csc_matrix(csc *M, const char *name);
 
+    // Bindings
+    // int run_python(int num_iter, float xref_py [], float fsteps_py []);
+    void run_python(MPC& self, const matXd& xref_py , const matXd& fsteps_py);
+
+
     //Eigen::Matrix<double, 12, 12> getA() { return A; }
     //Eigen::MatrixXf getML() { return ML; }
     /*void setDate(int year, int month, int day);
@@ -118,4 +134,34 @@ public:
 // Eigen::Matrix<double, 1, 2> get_projection_on_border(Eigen::Matrix<double,1,2>  robot, Eigen::Matrix<double, 1, 6> data_closest, double const& angle);
 
 
-#endif // MPC_H_INCLUDED
\ No newline at end of file
+/*namespace bp = boost::python;
+
+template <typename MPC>
+struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC> > {
+
+  // call macro for all ContactPhase methods that can be overloaded
+  // BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(isConsistent_overloads, ContactPhase::isConsistent, 0, 1)
+
+  template <class PyClass>
+  void visit(PyClass& cl) const {
+    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+        .def(bp::init<double, int, double>(bp::args("dt_in", "n_steps_in", "T_gait_in"), "Constructor."))
+        
+        // Run MPC from Python
+        .def("run_python", &MPC::run_python,
+             bp::args("xref_in", "fsteps_in"),
+             "Run MPC from Python.\n");
+  }
+
+  static void expose() {
+    bp::class_<MPC>("MPC", bp::no_init)
+        .def(MPCPythonVisitor<MPC>());
+
+    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+    //ENABLE_SPECIFIC_MATRIX_TYPE(typeXREF);
+    //ENABLE_SPECIFIC_MATRIX_TYPE(typeFSTEPS);
+  }
+
+};*/
+
+#endif // MPC_H_INCLUDED

python/CMakeLists.txt

@@ -3,7 +3,7 @@ set(${PY_NAME}_SOURCES
   )
 
 add_library(${PY_NAME} SHARED ${${PY_NAME}_SOURCES})
-target_link_libraries(${PY_NAME} PUBLIC ${PROJECT_NAME} ${PYTHON_LIBRARY})
+target_link_libraries(${PY_NAME} PUBLIC ${PROJECT_NAME} ${PYTHON_LIBRARY} eigenpy::eigenpy)
 target_include_directories(${PY_NAME} SYSTEM PRIVATE ${PYTHON_INCLUDE_DIRS})
 set_target_properties(${PY_NAME} PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${PY_NAME})
 TARGET_LINK_BOOST_PYTHON(${PY_NAME} PUBLIC)

python/example_adder/__init__.py

-from .libexample_adder import add, sub  # noqa: F401
+from .libexample_adder import *  # noqa: F401

python/gepadd.cpp

 #include "example-adder/gepadd.hpp"
+#include "example-adder/MPC.hpp"
 
+#include <eigenpy/eigenpy.hpp>
 #include <boost/python.hpp>
 
+namespace bp = boost::python;
+
+template <typename MPC>
+struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC> > {
+
+  // call macro for all ContactPhase methods that can be overloaded
+  // BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(isConsistent_overloads, ContactPhase::isConsistent, 0, 1)
+
+  template <class PyClass>
+  void visit(PyClass& cl) const {
+    cl.def(bp::init<>(bp::arg(""), "Default constructor."))
+        .def(bp::init<double, int, double>(bp::args("dt_in", "n_steps_in", "T_gait_in"), "Constructor."))
+        
+        // Run MPC from Python
+        .def("run_python", &MPC::run_python,
+             bp::args("xref_in", "fsteps_in"),
+             "Run MPC from Python.\n");
+  }
+
+  static void expose() {
+    bp::class_<MPC>("MPC", bp::no_init)
+        .def(MPCPythonVisitor<MPC>());
+
+    ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
+    //ENABLE_SPECIFIC_MATRIX_TYPE(typeXREF);
+    //ENABLE_SPECIFIC_MATRIX_TYPE(typeFSTEPS);
+  }
+
+};
+
+void exposeMPC() {
+    MPCPythonVisitor<MPC>::expose();
+}
+
 BOOST_PYTHON_MODULE(libexample_adder) {
   boost::python::def("add", gepetto::example::add);
   boost::python::def("sub", gepetto::example::sub);
-}
+
+  eigenpy::enableEigenPy();
+
+  exposeMPC();
+}
\ No newline at end of file

src/MPC.cpp

@@ -32,7 +32,12 @@ MPC::MPC(double dt_in, int n_steps_in, double T_gait_in)
 
     std::cout << settings->adaptive_rho_fraction << std::endl;
 }
- 
+
+MPC::MPC()
+{
+    MPC(0.02, 32, 0.64);
+}
+
 /*
 Create the constraint matrices of the MPC (M.X = N and L.X <= K)
 Create the weight matrices P and Q of the MPC solver (cost 1/2 x^T * P * X + X^T * Q)
@@ -735,11 +740,25 @@ double * MPC::get_x_next()
     return x_next;
 }
 
+
+/*
+Run function with arrays as input for compatibility between Python and C++
+*/
+void MPC::run_python(MPC& self, const matXd& xref_py , const matXd& fsteps_py)
+{
+
+    printf("Trigger bindings \n");
+    printf("xref: %f %f %f \n", xref_py(0, 0), xref_py(1, 0), xref_py(2, 0));
+    printf("fsteps: %f %f %f \n", fsteps_py(0, 0), fsteps_py(1, 0), fsteps_py(2, 0));
+
+    return;
+}
+
 /*
 Run one iteration of the whole MPC by calling all the necessary functions (data retrieval,
 update of constraint matrices, update of the solver, running the solver, retrieving result)
 */
-int MPC::run(int num_iter, Eigen::Matrix<double, 12, Eigen::Dynamic> xref_in, Eigen::Matrix<double, 20, 13> fsteps_in)
+int MPC::ron(int num_iter, Eigen::Matrix<double, 12, Eigen::Dynamic> xref_in, Eigen::Matrix<double, 20, 13> fsteps_in)
 {
     // Recontruct the gait based on the computed footsteps
     std::cout << "Recontruct gait" << std::endl;
@@ -877,10 +896,14 @@ void MPC::my_print_csc_matrix(csc *M, const char *name)
             //std::cout << M->i[i] << " " << j << " " << M->x[k++] << std::endl;
             int a = (int)M->i[i];
             int b = (int)j;
-            float c = M->x[k++];
+            double c = M->x[k++];
             if ((a >= 12*n_steps) && (a<12*n_steps+24) && (b >= 12*n_steps) && (b < 12*n_steps*2)){
             c_print("\t%3u [%3u,%3u] = %.3g\n", k-1, a, b-12*n_steps, c);}
       }
     }
   }
-}
\ No newline at end of file
+}
+
+/*void exposeMPC() {
+    MPCPythonVisitor<MPC>::expose();
+}*/
\ No newline at end of file

src/main.cpp

@@ -21,21 +21,18 @@ int main(int argc, char** argv) {
     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);
+    /*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.gethref() << std::endl;*/
     /*std::cout << test.getA() << std::endl;
     std::cout << test.getML() << std::endl;
     std::cout << test.getDay() << std::endl;