diff --git a/CMakeLists.txt b/CMakeLists.txt
index ca7607462eeaaad48ac3cb7f7712a3f8bdd2a5b2..dc23b4a4ffe264f1aa44fc36fe05e84725a1a1de 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -59,23 +59,15 @@ set(${PROJECT_NAME}_SOURCES
add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS})
target_include_directories(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
+#Â Include Eigen3 directories
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR})
TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM INTERFACE ${EIGEN3_INCLUDE_DIR})
-# Find OSQP library and headers
-# find_package(osqp REQUIRED)
-# Link the OSQP shared library
-# 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 eigenpy::eigenpy)
+target_link_libraries(${PROJECT_NAME} PRIVATE osqp::osqp)
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 0e9a9d2ffaf1ba9dc598e3d035890a7f178fddaf..f842656f00b3dca4b69d73c1b9d315eabee766bd 100644
--- a/include/example-adder/MPC.hpp
+++ b/include/example-adder/MPC.hpp
@@ -14,154 +14,111 @@
#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:
- double dt, mass, mu, T_gait, h_ref;
- int n_steps, cpt_ML, cpt_P;
-
- Eigen::Matrix<double, 3, 3> gI;
- 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>::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;
- //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, 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] = {};
- int i_y_B [12*4] = {};
- int i_update_B [12*4] = {};
- // TODO FOR S ????
-
- // Matrix NK
- 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
- csc* P; // Compressed Sparse Column matrix
-
- // Matrix Q
- const static int size_nz_Q = 5000;
- double Q [size_nz_Q] = {}; // Q is full of zeros
-
- // OSQP solver variables
- OSQPWorkspace * workspce = new OSQPWorkspace();
- 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, 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;
- Eigen::Matrix<double, Eigen::Dynamic, 1> NK_low;
- Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> D;
- Eigen::Matrix<int, Eigen::Dynamic, 1> i_off;
-
-public:
-
- MPC();
- MPC(double dt_in, int n_steps_in, double T_gait_in);
-
- int create_matrices();
- int create_ML();
- int create_NK();
- int create_weight_matrices();
- int update_matrices(Eigen::Matrix<double, 20, 13> fsteps);
- int update_ML(Eigen::Matrix<double, 20, 13> fsteps);
- int update_NK();
- int call_solver(int);
- int retrieve_result();
- double * get_latest_result();
- double * get_x_next();
- 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();
- int construct_gait(Eigen::Matrix<double, 20, 13> fsteps_in);
-
- // Accessor
- 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);
- int getYear();
- int getMonth();
- int getDay();*/
-
+class MPC {
+ private:
+ double dt, mass, mu, T_gait, h_ref;
+ int n_steps, cpt_ML, cpt_P;
+
+ Eigen::Matrix<double, 3, 3> gI;
+ 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>::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;
+ // 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, 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] = {};
+ int i_y_B[12 * 4] = {};
+ int i_update_B[12 * 4] = {};
+ // TODO FOR S ????
+
+ // Matrix NK
+ 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
+ csc *P; // Compressed Sparse Column matrix
+
+ // Matrix Q
+ const static int size_nz_Q = 5000;
+ double Q[size_nz_Q] = {}; // Q is full of zeros
+
+ // OSQP solver variables
+ OSQPWorkspace *workspce = new OSQPWorkspace();
+ 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, 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;
+ Eigen::Matrix<double, Eigen::Dynamic, 1> NK_low;
+ Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> D;
+ Eigen::Matrix<int, Eigen::Dynamic, 1> i_off;
+
+ public:
+ MPC();
+ MPC(double dt_in, int n_steps_in, double T_gait_in);
+
+ int create_matrices();
+ int create_ML();
+ int create_NK();
+ int create_weight_matrices();
+ int update_matrices(Eigen::Matrix<double, 20, 13> fsteps);
+ int update_ML(Eigen::Matrix<double, 20, 13> fsteps);
+ int update_NK();
+ int call_solver(int);
+ 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);
+
+ Eigen::Matrix<double, 3, 3> getSkew(Eigen::Matrix<double, 3, 1> v);
+ int construct_S();
+ int construct_gait(Eigen::Matrix<double, 20, 13> fsteps_in);
+
+ // Accessor
+ double gethref() { return h_ref; }
+ void my_print_csc_matrix(csc *M, const char *name);
+
+ // Bindings
+ void run_python(int num_iter, 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);
+ int getYear();
+ int getMonth();
+ int getDay();*/
};
-// 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);
-
-
-/*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
+#endif // MPC_H_INCLUDED
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 4d2bbf41649adc7f81e12ebc3dc9370b578c5c5f..338b68bc419476f19c03c03d2c7fa14d6b8eb57e 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -9,17 +9,15 @@ 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."))
+ .def(bp::init<double, int, double>(bp::args("dt_in", "n_steps_in", "T_gait_in"),
+ "Constructor with parameters."))
// Run MPC from Python
.def("run_python", &MPC::run_python,
- bp::args("xref_in", "fsteps_in"),
+ bp::args("num_iter", "xref_in", "fsteps_in"),
"Run MPC from Python.\n");
}
@@ -28,8 +26,6 @@ struct MPCPythonVisitor : public bp::def_visitor<MPCPythonVisitor<MPC> > {
.def(MPCPythonVisitor<MPC>());
ENABLE_SPECIFIC_MATRIX_TYPE(matXd);
- //ENABLE_SPECIFIC_MATRIX_TYPE(typeXREF);
- //ENABLE_SPECIFIC_MATRIX_TYPE(typeFSTEPS);
}
};
diff --git a/src/MPC.cpp b/src/MPC.cpp
index 5dc66b598af0d148495a97fc274b4c98329db431..7d171db5f43ed7222a70ca3961cdcb2c0604f4d9 100644
--- a/src/MPC.cpp
+++ b/src/MPC.cpp
@@ -744,7 +744,7 @@ double * MPC::get_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)
+void MPC::run_python(int num_iter, const matXd& xref_py , const matXd& fsteps_py)
{
printf("Trigger bindings \n");
@@ -758,7 +758,7 @@ void MPC::run_python(MPC& self, const matXd& xref_py , const matXd& fsteps_py)
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::ron(int num_iter, Eigen::Matrix<double, 12, Eigen::Dynamic> xref_in, Eigen::Matrix<double, 20, 13> fsteps_in)
+int MPC::run(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;
@@ -903,7 +903,3 @@ void MPC::my_print_csc_matrix(csc *M, const char *name)
}
}
}
-
-/*void exposeMPC() {
- MPCPythonVisitor<MPC>::expose();
-}*/
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 3f1856277adb0f269af2d87ddaf5104cd194af22..fae1f865a348bb336d6e5c86c5b1469f1fca8d54 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -13,10 +13,10 @@ int main(int argc, char** argv) {
std::cout << gepetto::example::add(a, b) << std::endl;
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;
+ 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);
@@ -50,13 +50,10 @@ int main(int argc, char** argv) {
M1 << 1, 2, 3, 4, 5, 6,
7, 8, 9, 10, 11, 12,
13, 14, 15, 16, 17, 18;
-
+
Eigen::Map<Eigen::MatrixXf> M2(M1.data(), M1.size(), 1);
std::cout << "M2:" << std::endl << M2 << std::endl;*/
-
-
-
return EXIT_SUCCESS;
} else {
std::cerr << "This program needs 2 integers" << std::endl;