diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh index 08c166f540b82c50c94336766d8e418de2bd4454..a0e47c72240820685e7ddf99d47479826e0b580d 100644 --- a/include/robust-equilibrium-lib/static_equilibrium.hh +++ b/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 diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp index 50d90fa8c84cf32c5ce77dc803064ec05e7cb3f3..09c97ac50a051ccbb011fafb8906917223e3a543 100644 --- a/src/static_equilibrium.cpp +++ b/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