implement checkRobustEquilibrium with non null acceleration for algorithm PP

include/centroidal-dynamics-lib/centroidal_dynamics.hh

@@ -225,6 +225,30 @@ public:
    */
   LP_status checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max=0.0);
 
+
+  /**
+   * @brief Check whether the specified com position is in robust equilibrium.
+   * This amounts to solving the following feasibility LP:
+   *       find          b
+   *       minimize      1
+   *       subject to    G b = D c + d
+   *                     b >= b0
+   *  where:
+   *     b         are the coefficient of the contact force generators (f = G b)
+   *     b0        is a parameter proportional to the specified robustness measure
+   *     c         is the specified CoM position
+   *     G         is the 6xm matrix whose columns are the gravito-inertial wrench generators
+   *     D         is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
+   *     d         is the 6d vector containing the gravity part of the gravito-inertial wrench
+   * @param com The 3d center of mass position to test.
+   * @param acc The 3d acceleration of the CoM.
+   * @param equilibrium True if com is in robust equilibrium, false otherwise.
+   * @param e_max Desired robustness level.
+   * @return The status of the LP solver.
+   */
+  LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max=0.0);
+
+
   /**
    * @brief Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
    * This amounts to solving the following LP:

src/centroidal_dynamics.cpp

@@ -366,9 +366,18 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
   return LP_STATUS_ERROR;
 }
 
+LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max){
+    checkRobustEquilibrium(com,zero_acc,equilibrium,e_max);
+}
 
-LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
+LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max)
 {
+    // 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);
+    m_HD = m_H * m_D;
+    m_Hd = m_H * m_d;
+
   if(m_G_centr.cols()==0)
   {
     equilibrium=false;