Commit 00ba708e authored by Steve Tonneau's avatar Steve Tonneau
Browse files

binded non eigen methods

parent 704dc865
......@@ -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()
......
......@@ -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.
......
......@@ -10,7 +10,7 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <robust-equilibrium-lib/config.hh>
#include <centroidal-dynamics-lib/config.hh>
#include <sstream>
#include <Eigen/Dense>
#include <map>
......
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)
#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
......@@ -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);
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment