diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8ec9743f4ba076168d0148b775360dd14c21a004..ca7607462eeaaad48ac3cb7f7712a3f8bdd2a5b2 100644
--- a/CMakeLists.txt
+++ b/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})
diff --git a/include/example-adder/MPC.hpp b/include/example-adder/MPC.hpp
index b50b6ac18cd89da0df0dbb53c17e6ca2c800d36d..0e9a9d2ffaf1ba9dc598e3d035890a7f178fddaf 100644
--- a/include/example-adder/MPC.hpp
+++ b/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
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index 8bbeca1ed9ef265b76d0f7b34b2ccd0823f2c4df..7ae6e048d53830656660ef370037264a8b7fbe61 100644
--- a/python/CMakeLists.txt
+++ b/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)
diff --git a/python/example_adder/__init__.py b/python/example_adder/__init__.py
index ced46cd7ffc26bbe247b21f872f391916caa1b62..d76540ff51866219226c5f821274ee0ff3bf53aa 100644
--- a/python/example_adder/__init__.py
+++ b/python/example_adder/__init__.py
@@ -1 +1 @@
-from .libexample_adder import add, sub # noqa: F401
+from .libexample_adder import * # noqa: F401
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index a8ae6346bdc5b7734ef0c4e5a4e70d2774ee1b0a..4d2bbf41649adc7f81e12ebc3dc9370b578c5c5f 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -1,8 +1,48 @@
#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
diff --git a/src/MPC.cpp b/src/MPC.cpp
index 9b29c5d21b8595464efcf4c40f68afc3e5e24f09..5dc66b598af0d148495a97fc274b4c98329db431 100644
--- a/src/MPC.cpp
+++ b/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
diff --git a/src/main.cpp b/src/main.cpp
index 2e917216a2d5a364424a5b73b846bda0964109a3..3f1856277adb0f269af2d87ddaf5104cd194af22 100644
--- a/src/main.cpp
+++ b/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;