added static equilibrium robustness in python

python/centroidal_dynamics_python.cpp

@@ -12,6 +12,13 @@ namespace centroidal_dynamics
 {
 using namespace boost::python;
 
+boost::python::tuple wrapComputeQuasiEquilibriumRobustness(Equilibrium& self, const Vector3& com)
+{
+    double robustness;
+    LP_status status = self.computeEquilibriumRobustness(com, robustness);
+    return boost::python::make_tuple(status, robustness);
+}
+
 boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const Vector3& com, const Vector3& acc)
 {
     double robustness;
@@ -74,6 +81,7 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
             .def("getName", &Equilibrium::getName)
             .def("getAlgorithm", &Equilibrium::getAlgorithm)
             .def("setNewContacts", setNewContacts)
+            .def("computeEquilibriumRobustness", wrapComputeQuasiEquilibriumRobustness)
             .def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
     ;
 }

python/test/binding_tests.py

-#~ from centroidal_dynamics import Equilibrium, SolverLP, EquilibriumAlgorithm
 from centroidal_dynamics import *
 
 #testing constructors
@@ -9,11 +8,13 @@ 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
+#whether useWarmStart is enable (True by default)
 previous = eq.useWarmStart()
+#enable warm start in solver (only for QPOases)
 eq.setUseWarmStart(False)
 assert(previous != eq.useWarmStart())
 
+#access solver name
 assert(eq.getName() == "test")
 
 
@@ -24,15 +25,17 @@ 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)]))
 
+#setting contact positions and normals, as well as friction coefficients 
 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)
+#computing robustness of a given configuration, first with no argument (0 acceleration, static equilibrium)
+status, robustness = eq.computeEquilibriumRobustness(c)
 assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
 assert (robustness > 0), "first test should be in equilibrirum"
 	
+#computing robustness of a given configuration with non zero acceleration
 ddc= asmatrix(array([1000.,0.,0.]))
 status, robustness = eq.computeEquilibriumRobustness(c,ddc)
 assert (status == LP_STATUS_OPTIMAL), "LP should not fail"

src/centroidal_dynamics.cpp

@@ -182,9 +182,19 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
   return true;
 }
 
+static const Vector3 zero_acc = Vector3::Zero();
 
 LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &robustness)
 {
+    return computeEquilibriumRobustness(com, zero_acc, robustness);
+}
+
+LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness)
+{
+  // Take the acceleration in account in D and d :
+  m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
+  m_d.head<3>()= m_mass * (m_gravity - acc);
+
   const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
   if(m==0)
     return LP_STATUS_INFEASIBLE;
@@ -311,20 +321,6 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &ro
   return LP_STATUS_ERROR;
 }
 
-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;
-  m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
-  m_d.head<3>()= m_mass * (m_gravity - acc);
-  // compute equilibrium robustness with the new D and d
-  LP_status status = computeEquilibriumRobustness(com,robustness);
-  // Switch back to the original values of D and d
-  m_D = old_D;
-  m_d = old_d;
-  return status;
-}
-
 /**
   m_d.setZero();
   m_d.head<3>() = m_mass*m_gravity;