add a method computeEquilibriumRobustness which take an acceleration as argument

include/robust-equilibrium-lib/static_equilibrium.hh

@@ -153,6 +153,30 @@ public:
    */
   LP_status computeEquilibriumRobustness(Cref_vector3 com, double &robustness);
 
+  /**
+   * @brief Compute a measure of the robustness of the equilibrium of the specified com position.
+   * This amounts to solving the following LP:
+   *       find          b, b0
+   *       maximize      b0
+   *       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 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 robustness The computed measure of robustness.
+   * @return The status of the LP solver.
+   * @note If the system is in force closure the status will be LP_STATUS_UNBOUNDED, meaning that the
+   * system can reach infinite robustness. This is due to the fact that we are not considering
+   * any upper limit for the friction cones.
+   */
+  LP_status computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness);
+
   /**
    * @brief Check whether the specified com position is in robust equilibrium.
    * This amounts to solving the following feasibility LP:

src/static_equilibrium.cpp

@@ -303,6 +303,27 @@ LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, doub
   return LP_STATUS_ERROR;
 }
 
+LP_status StaticEquilibrium::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;
+  m_D.setZero();
+  m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
+*/
+
 LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
 {
   if(m_G_centr.cols()==0)