From 00ba708e45923d42f732be94b68cc53c975e9f00 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Wed, 8 Mar 2017 16:39:23 +0100
Subject: [PATCH] binded non eigen methods
---
CMakeLists.txt | 15 ++++---
.../centroidal_dynamics.hh | 10 ++---
include/centroidal-dynamics-lib/logger.hh | 2 +-
python/CMakeLists.txt | 12 ++++++
python/centroidal_dynamics_python.cpp | 42 +++++++++++++++++++
src/centroidal_dynamics.cpp | 30 ++++++-------
test/test_static_equilibrium.cpp | 30 ++++++-------
7 files changed, 99 insertions(+), 42 deletions(-)
create mode 100644 python/CMakeLists.txt
create mode 100644 python/centroidal_dynamics_python.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1486c3b..4a8c183 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -31,15 +31,15 @@ SETUP_PROJECT()
string (REPLACE "-Werror" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
+OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
- OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" OFF)
-ENDIF(BUILD_PYTHON_INTERFACE)
+ find_package( PythonLibs 2.7 REQUIRED )
+ include_directories( ${PYTHON_INCLUDE_DIRS} )
-find_package( PythonLibs 2.7 REQUIRED )
-include_directories( ${PYTHON_INCLUDE_DIRS} )
+ find_package( Boost COMPONENTS python REQUIRED )
+ include_directories( ${Boost_INCLUDE_DIR} )
+ENDIF(BUILD_PYTHON_INTERFACE)
-find_package( Boost COMPONENTS python REQUIRED )
-include_directories( ${Boost_INCLUDE_DIR} )
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
@@ -76,6 +76,9 @@ ADD_REQUIRED_DEPENDENCY("qpOASES")
add_subdirectory (src)
add_subdirectory (test)
+IF(BUILD_PYTHON_INTERFACE)
+ add_subdirectory (python)
+ENDIF(BUILD_PYTHON_INTERFACE)
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
diff --git a/include/centroidal-dynamics-lib/centroidal_dynamics.hh b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
index 5ecf4aa..1de25df 100644
--- a/include/centroidal-dynamics-lib/centroidal_dynamics.hh
+++ b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
@@ -24,7 +24,7 @@ enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm
EQUILIBRIUM_ALGORITHM_DIP /// incremental projection algorithm based on dual LP formulation
};
-class CENTROIDAL_DYNAMICS_DLLAPI StaticEquilibrium
+class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium
{
private:
static bool m_is_cdd_initialized; /// true if cdd lib has been initialized, false otherwise
@@ -83,7 +83,7 @@ private:
public:
/**
- * @brief StaticEquilibrium constructor.
+ * @brief Equilibrium constructor.
* @param name Name of the object.
* @param mass Mass of the system for which to test equilibrium.
* @param generatorsPerContact Number of generators used to approximate the friction cone per contact point.
@@ -93,8 +93,8 @@ public:
* @param canonicalize_cdd_matrix whether to remove redundant inequalities when computing double description matrices
* a small pertubation of the system
*/
- StaticEquilibrium(std::string name, double mass, unsigned int generatorsPerContact,
- SolverLP solver_type, bool useWarmStart=true, const unsigned int max_num_cdd_trials=0,
+ Equilibrium(std::string name, double mass, unsigned int generatorsPerContact,
+ SolverLP solver_type = SOLVER_LP_QPOASES, bool useWarmStart=true, const unsigned int max_num_cdd_trials=0,
const bool canonicalize_cdd_matrix=false);
/**
@@ -107,7 +107,7 @@ public:
* @brief Specifies whether the LP solver is allowed to use warm start.
* @param uws True if the LP solver is allowed to use warm start, false otherwise.
*/
- void useWarmStart(bool uws){ m_solver->setUseWarmStart(uws); }
+ void setUseWarmStart(bool uws){ m_solver->setUseWarmStart(uws); }
/**
* @brief Get the name of this object.
diff --git a/include/centroidal-dynamics-lib/logger.hh b/include/centroidal-dynamics-lib/logger.hh
index 7fe616d..314be7d 100644
--- a/include/centroidal-dynamics-lib/logger.hh
+++ b/include/centroidal-dynamics-lib/logger.hh
@@ -10,7 +10,7 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include <robust-equilibrium-lib/config.hh>
+#include <centroidal-dynamics-lib/config.hh>
#include <sstream>
#include <Eigen/Dense>
#include <map>
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
new file mode 100644
index 0000000..34d4390
--- /dev/null
+++ b/python/CMakeLists.txt
@@ -0,0 +1,12 @@
+cmake_minimum_required( VERSION 2.8 )
+
+include_directories("${INCLUDE_DIR}")
+include_directories("${EIGEN3_INCLUDE_DIR}")
+
+# Define the wrapper library that wraps our library
+add_library( centroidal_dynamics SHARED centroidal_dynamics_python.cpp )
+target_link_libraries( centroidal_dynamics ${Boost_LIBRARIES} ${PROJECT_NAME} )
+# don't prepend wrapper library name with lib
+set_target_properties( centroidal_dynamics PROPERTIES PREFIX "" )
+
+#INSTALL(TARGETS centroidal_dynamics DESTINATION lib)
diff --git a/python/centroidal_dynamics_python.cpp b/python/centroidal_dynamics_python.cpp
new file mode 100644
index 0000000..8c4e3a2
--- /dev/null
+++ b/python/centroidal_dynamics_python.cpp
@@ -0,0 +1,42 @@
+#include "centroidal-dynamics-lib/centroidal_dynamics.hh"
+
+#include <boost/python.hpp>
+
+namespace centroidal_dynamics
+{
+using namespace boost::python;
+
+BOOST_PYTHON_MODULE(centroidal_dynamics)
+{
+ /** BEGIN enum types **/
+ #ifdef CLP_FOUND
+ enum_<SolverLP>("SolverLP")
+ .value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES)
+ .value("SOLVER_LP_CLP", SOLVER_LP_CLP)
+ .export_values();
+ #else
+ enum_<SolverLP>("SolverLP")
+ .value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES)
+ .export_values();
+ #endif
+
+
+ enum_<EquilibriumAlgorithm>("EquilibriumAlgorithm")
+ .value("EQUILIBRIUM_ALGORITHM_LP", EQUILIBRIUM_ALGORITHM_LP)
+ .value("EQUILIBRIUM_ALGORITHM_LP2", EQUILIBRIUM_ALGORITHM_LP2)
+ .value("EQUILIBRIUM_ALGORITHM_DLP", EQUILIBRIUM_ALGORITHM_DLP)
+ .value("EQUILIBRIUM_ALGORITHM_PP", EQUILIBRIUM_ALGORITHM_PP)
+ .value("EQUILIBRIUM_ALGORITHM_IP", EQUILIBRIUM_ALGORITHM_IP)
+ .value("EQUILIBRIUM_ALGORITHM_DIP", EQUILIBRIUM_ALGORITHM_DIP)
+ .export_values();
+
+ /** END enum types **/
+ class_<Equilibrium>("Equilibrium", init<std::string, double, unsigned int, optional <SolverLP, bool, const unsigned int, const bool> >())
+ .def("useWarmStart", &Equilibrium::useWarmStart)
+ .def("setUseWarmStart", &Equilibrium::setUseWarmStart)
+ .def("getName", &Equilibrium::getName)
+ .def("getAlgorithm", &Equilibrium::getAlgorithm)
+ ;
+}
+
+} // namespace centroidal_dynamics
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index 329b194..38fc825 100644
--- a/src/centroidal_dynamics.cpp
+++ b/src/centroidal_dynamics.cpp
@@ -15,9 +15,9 @@ using namespace std;
namespace centroidal_dynamics
{
-bool StaticEquilibrium::m_is_cdd_initialized = false;
+bool Equilibrium::m_is_cdd_initialized = false;
-StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int generatorsPerContact,
+Equilibrium::Equilibrium(string name, double mass, unsigned int generatorsPerContact,
SolverLP solver_type, bool useWarmStart,
const unsigned int max_num_cdd_trials, const bool canonicalize_cdd_matrix)
: m_is_cdd_stable(true)
@@ -53,7 +53,7 @@ StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int gene
m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
}
-bool StaticEquilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
+bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
double frictionCoefficient, const bool perturbate)
{
long int c = contactPoints.rows();
@@ -129,7 +129,7 @@ bool StaticEquilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matr
return true;
}
-bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
+bool Equilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
double frictionCoefficient, EquilibriumAlgorithm alg)
{
assert(contactPoints.rows()==contactNormals.rows());
@@ -175,7 +175,7 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
}
-LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &robustness)
+LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &robustness)
{
const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
if(m==0)
@@ -303,7 +303,7 @@ LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, doub
return LP_STATUS_ERROR;
}
-LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness){
+LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness){
// Take the acceleration in account in D and d :
Matrix63 old_D = m_D;
Vector6 old_d = m_d;
@@ -324,7 +324,7 @@ LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref
m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
*/
-LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
+LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
{
if(m_G_centr.cols()==0)
{
@@ -355,7 +355,7 @@ LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equi
}
-LP_status StaticEquilibrium::getPolytopeInequalities(MatrixXX& H, VectorX& h) const
+LP_status Equilibrium::getPolytopeInequalities(MatrixXX& H, VectorX& h) const
{
if(m_algorithm!=EQUILIBRIUM_ALGORITHM_PP)
{
@@ -376,7 +376,7 @@ LP_status StaticEquilibrium::getPolytopeInequalities(MatrixXX& H, VectorX& h) co
return LP_STATUS_OPTIMAL;
}
-LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com)
+LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com)
{
const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
if(m_G_centr.cols()==0)
@@ -506,7 +506,7 @@ LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a
return LP_STATUS_ERROR;
}
-LP_status StaticEquilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max)
+LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max)
{
if(m_G_centr.cols()==0)
return LP_STATUS_INFEASIBLE;
@@ -514,7 +514,7 @@ LP_status StaticEquilibrium::findExtremumInDirection(Cref_vector3 direction, Ref
return LP_STATUS_ERROR;
}
-bool StaticEquilibrium::computePolytopeProjection(Cref_matrix6X v)
+bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
{
// getProfiler().start("eigen_to_cdd");
dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
@@ -573,18 +573,18 @@ bool StaticEquilibrium::computePolytopeProjection(Cref_matrix6X v)
return true;
}
-double StaticEquilibrium::convert_b0_to_emax(double b0)
+double Equilibrium::convert_b0_to_emax(double b0)
{
return (b0*m_b0_to_emax_coefficient);
}
-double StaticEquilibrium::convert_emax_to_b0(double emax)
+double Equilibrium::convert_emax_to_b0(double emax)
{
return (emax/m_b0_to_emax_coefficient);
}
-LP_status StaticEquilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0){
+LP_status Equilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0){
int m = (int)A.cols() -1 ; // 4* number of contacts
VectorX b_a0(m+1);
VectorX c = VectorX::Zero(m+1);
@@ -612,7 +612,7 @@ LP_status StaticEquilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vecto
}
-bool StaticEquilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a ){
+bool Equilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a ){
int m = (int)G.cols(); // number of contact * 4
VectorX b(m);
VectorX c = VectorX::Zero(m);
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index f05dd7b..a12b032 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -24,8 +24,8 @@ using namespace std;
#define EPS 1e-3 // required precision
-/** Check the coherence between the method StaticEquilibrium::computeEquilibriumRobustness
- * and the method StaticEquilibrium::checkRobustEquilibrium.
+/** Check the coherence between the method Equilibrium::computeEquilibriumRobustness
+ * and the method Equilibrium::checkRobustEquilibrium.
* @param solver_1 Solver used to test computeEquilibriumRobustness.
* @param solver_2 Solver used to test checkRobustEquilibrium.
* @param comPositions List of 2d com positions on which to perform the tests.
@@ -33,8 +33,8 @@ using namespace std;
* @param PERF_STRING_2 String to use for logging the computation times of solver_2
* @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
*/
-int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium *solver_1,
- StaticEquilibrium *solver_2,
+int test_computeEquilibriumRobustness_vs_checkEquilibrium(Equilibrium *solver_1,
+ Equilibrium *solver_2,
Cref_matrixXX comPositions,
const string& PERF_STRING_1="",
const string& PERF_STRING_2="",
@@ -93,7 +93,7 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium *sol
return error_counter;
}
-/** Test two different solvers on the method StaticEquilibrium::computeEquilibriumRobustness.
+/** Test two different solvers on the method Equilibrium::computeEquilibriumRobustness.
* @param solver_1 First solver to test.
* @param solver_2 Second solver to test.
* @param comPositions List of 2d com positions on which to perform the tests.
@@ -101,7 +101,7 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium *sol
* @param PERF_STRING_2 String to use for logging the computation times of solver_2
* @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
*/
-int test_computeEquilibriumRobustness(StaticEquilibrium *solver_1, StaticEquilibrium *solver_2, Cref_matrixXX comPositions,
+int test_computeEquilibriumRobustness(Equilibrium *solver_1, Equilibrium *solver_2, Cref_matrixXX comPositions,
const string& PERF_STRING_1, const string& PERF_STRING_2, int verb=0)
{
int error_counter = 0;
@@ -146,7 +146,7 @@ int test_computeEquilibriumRobustness(StaticEquilibrium *solver_1, StaticEquilib
return error_counter;
}
-/** Test method StaticEquilibrium::findExtremumOverLine. The test works in this way: first it
+/** Test method Equilibrium::findExtremumOverLine. The test works in this way: first it
* calls the method findExtremumOverLine of the solver to test to find the extremum over a random
* line with a specified robustness. Then it checks that the point found really has the specified
* robustness by using the ground-truth solver.
@@ -159,7 +159,7 @@ int test_computeEquilibriumRobustness(StaticEquilibrium *solver_1, StaticEquilib
* @param PERF_STRING_GROUND_TRUTH String to use for logging the computation times of solver_ground_truth
* @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
*/
-int test_findExtremumOverLine(StaticEquilibrium *solver_to_test, StaticEquilibrium *solver_ground_truth,
+int test_findExtremumOverLine(Equilibrium *solver_to_test, Equilibrium *solver_ground_truth,
Cref_vector3 a0, int N_TESTS, double e_max,
const string& PERF_STRING_TEST, const string& PERF_STRING_GROUND_TRUTH, int verb=0)
{
@@ -226,11 +226,11 @@ int test_findExtremumOverLine(StaticEquilibrium *solver_to_test, StaticEquilibri
}
/** Draw a grid on the screen using the robustness computed with the method
- * StaticEquilibrium::computeEquilibriumRobustness.
+ * Equilibrium::computeEquilibriumRobustness.
* @param solver The solver to use for computing the equilibrium robustness.
* @param comPositions Grid of CoM positions in the form of an Nx2 matrix.
*/
-void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, StaticEquilibrium *solver,
+void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium *solver,
Cref_matrixXX comPositions, Cref_matrixXX p)
{
MatrixXi contactPointCoord(4*N_CONTACTS,2);
@@ -381,11 +381,11 @@ void testWithLoadedData()
return;
}
- StaticEquilibrium* solvers[N_SOLVERS];
+ Equilibrium* solvers[N_SOLVERS];
double robustness[N_SOLVERS];
for(int s=0; s<N_SOLVERS; s++)
{
- solvers[s] = new StaticEquilibrium(solverNames[s], mass, generatorsPerContact, SOLVER_LP_QPOASES);
+ solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact, SOLVER_LP_QPOASES);
if(!solvers[s]->setNewContacts(contactPoints, contactNormals, mu, algorithms[s]))
{
SEND_ERROR_MSG("Error while setting new contacts for solver "+solvers[s]->getName());
@@ -461,10 +461,10 @@ int main()
cout<<"Number of generators per contact: "<<generatorsPerContact<<endl;
cout<<"Gonna test equilibrium on a 2d grid of "<<GRID_SIZE<<"X"<<GRID_SIZE<<" points "<<endl;
- StaticEquilibrium* solver_PP = new StaticEquilibrium("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES);
- StaticEquilibrium* solvers[N_SOLVERS];
+ Equilibrium* solver_PP = new Equilibrium("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES);
+ Equilibrium* solvers[N_SOLVERS];
for(int s=0; s<N_SOLVERS; s++)
- solvers[s] = new StaticEquilibrium(solverNames[s], mass, generatorsPerContact, lp_solver_types[s]);
+ solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact, lp_solver_types[s]);
MatrixXX p, N;
RVector2 com_LB, com_UB;
--
GitLab