add method checkAdmissibleAcceleration

include/robust-equilibrium-lib/static_equilibrium.hh

@@ -238,7 +238,7 @@ public:
   /**
    * @brief findMaximumAcceleration Find the maximal acceleration along a given direction
           find          b, alpha0
-          minimize      -alpha0
+          maximize      alpha0
           subject to    -h <= [-G  (Hv)] [b a0]^T   <= -h
                         0       <= [b a0]^T <= Inf
 
@@ -255,6 +255,24 @@ public:
    * @return The status of the LP solver.
    */
   LP_status findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0);
+
+  /**
+   * @brief checkAdmissibleAcceleration return true if the given acceleration is admissible for the given contacts
+          find          b
+          subject to    G b = Ha + h
+                        0       <= b <= Inf
+          b         are the coefficient of the contact force generators (f = V b)
+          a         is the vector3 defining the acceleration
+          G         is the matrix whose columns are the gravito-inertial wrench generators
+          h and H come from polytope inequalities
+   * @param G
+   * @param H
+   * @param h
+   * @param a
+   * @return true if the acceleration is admissible, false otherwise
+   */
+  bool checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a );
+
 };
 
 } // end namespace robust_equilibrium

src/static_equilibrium.cpp

@@ -586,6 +586,26 @@ LP_status StaticEquilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vecto
 
 }
 
+bool StaticEquilibrium::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);
+  VectorX lb = VectorX::Zero(m);
+  VectorX ub = VectorX::Ones(m)*1e10; // Inf
+  VectorX Alb = H*a + h;
+  VectorX Aub = H*a + h;
+
+
+  LP_status lpStatus = m_solver->solve(c, lb, ub, G, Alb, Aub, b);
+  if(lpStatus==LP_STATUS_OPTIMAL)
+  {
+    return true;
+  }
+  else{
+    SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
+    return false;
+  }
+}
 
 
 } // end namespace robust_equilibrium