added equilibrium method

python/centroidal_dynamics_python.cpp

@@ -12,6 +12,13 @@ namespace centroidal_dynamics
 {
 using namespace boost::python;
 
+boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const Vector3& com, const Vector3& acc)
+{
+    double robustness;
+    LP_status status = self.computeEquilibriumRobustness(com, acc, robustness);
+    return boost::python::make_tuple(status, robustness);
+}
+
 
 BOOST_PYTHON_MODULE(centroidal_dynamics)
 {
@@ -19,6 +26,7 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
     eigenpy::enableEigenPy();
 
     eigenpy::enableEigenPySpecific<MatrixX3ColMajor,MatrixX3ColMajor>();
+    eigenpy::enableEigenPySpecific<Vector3,Vector3>();
     /*eigenpy::exposeAngleAxis();
     eigenpy::exposeQuaternion();*/
 
@@ -46,16 +54,27 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
             .value("EQUILIBRIUM_ALGORITHM_DIP", EQUILIBRIUM_ALGORITHM_DIP)
             .export_values();
 
+    enum_<LP_status>("LP_status")
+            .value("LP_STATUS_UNKNOWN", LP_STATUS_UNKNOWN)
+            .value("LP_STATUS_OPTIMAL", LP_STATUS_OPTIMAL)
+            .value("LP_STATUS_INFEASIBLE", LP_STATUS_INFEASIBLE)
+            .value("LP_STATUS_UNBOUNDED", LP_STATUS_UNBOUNDED)
+            .value("LP_STATUS_MAX_ITER_REACHED", LP_STATUS_MAX_ITER_REACHED)
+            .value("LP_STATUS_ERROR", LP_STATUS_ERROR)
+            .export_values();
+
+    /** END enum types **/
+
     bool (Equilibrium::*setNewContacts)
             (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm) = &Equilibrium::setNewContacts;
 
-    /** 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)
             .def("setNewContacts", setNewContacts)
+            .def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
     ;
 }
 

python/test/binding_tests.py

+#~ from centroidal_dynamics import Equilibrium, SolverLP, EquilibriumAlgorithm
+from centroidal_dynamics import *
+
+#testing constructors
+eq = Equilibrium("test", 54., 4) 
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES ) 
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES ) 
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False) 
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1) 
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True ) 
+
+#testing all methods
+previous = eq.useWarmStart()
+eq.setUseWarmStart(False)
+assert(previous != eq.useWarmStart())
+
+assert(eq.getName() == "test")
+
+
+# creating contact points
+from numpy import array, asmatrix, matrix
+
+z = array([0.,0.,1.])
+P = asmatrix(array([array([x,y,0]) for x in [-0.05,0.05] for y in [-0.1,0.1]]))
+N = asmatrix(array([z for _ in range(4)]))
+
+eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
+
+c= asmatrix(array([0.,0.,1.]))
+ddc= asmatrix(array([0.,0.,0.]))
+
+status, robustness = eq.computeEquilibriumRobustness(c,ddc)
+assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
+assert (robustness > 0), "first test should be in equilibrirum"
+	
+ddc= asmatrix(array([1000.,0.,0.]))
+status, robustness = eq.computeEquilibriumRobustness(c,ddc)
+assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
+assert (robustness < 0), "first test should NOT be in equilibrirum"