From 0adfb886dd525978bf28e16d10935e94b1c48e4a Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Mon, 8 Aug 2016 07:16:45 +0200
Subject: [PATCH] exposed CWC in library api
---
.../static_equilibrium.hh | 13 ++++++++++
src/static_equilibrium.cpp | 25 +++++++++++++++++++
2 files changed, 38 insertions(+)
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index a5d935b..4ed0471 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -44,6 +44,8 @@ private:
/** Inequality matrix and vector defining the gravito-inertial wrench cone H w <= h */
MatrixXX m_H;
VectorX m_h;
+ /** False if a numerical instability appeared in the computation H and h*/
+ bool m_is_cdd_stable;
/** Inequality matrix and vector defining the CoM support polygon HD com + Hd <= h */
MatrixX3 m_HD;
@@ -209,6 +211,17 @@ public:
*/
LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max=0.0);
+ /**
+ * @brief Retrieve the inequalities that define the admissible wrenchs
+ * for the current contact set.
+ * @param H reference to the H matrix to initialize
+ * @param h reference to the h vector to initialize
+ * @return The status of the inequalities. If the inequalities are not defined
+ * due to numerical instabilities, will send appropriate error message,
+ * and return LP_STATUS_ERROR. If they are not defined because no
+ * contact has been defined, will return LP_STATUS_INFEASIBLE
+ */
+ LP_status getPolytopeInequalities(MatrixXX& H, VectorX& h) const;
};
} // end namespace robust_equilibrium
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index 04e51d5..cd4f0ca 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -19,6 +19,7 @@ bool StaticEquilibrium::m_is_cdd_initialized = false;
StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int generatorsPerContact,
SolverLP solver_type, bool useWarmStart)
+ : m_is_cdd_stable(true)
{
if(!m_is_cdd_initialized)
{
@@ -293,6 +294,28 @@ LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equi
return LP_STATUS_OPTIMAL;
}
+
+LP_status StaticEquilibrium::getPolytopeInequalities(MatrixXX& H, VectorX& h) const
+{
+ if(m_algorithm!=STATIC_EQUILIBRIUM_ALGORITHM_PP)
+ {
+ SEND_ERROR_MSG("getPolytopeInequalities is only implemented for the PP algorithm");
+ return LP_STATUS_ERROR;
+ }
+ if(!m_is_cdd_stable)
+ {
+ SEND_ERROR_MSG("numerical instability in cddlib");
+ return LP_STATUS_ERROR;
+ }
+ if(m_G_centr.cols()==0)
+ {
+ return LP_STATUS_INFEASIBLE;
+ }
+ H = m_H;
+ h = m_h;
+ return LP_STATUS_OPTIMAL;
+}
+
LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com)
{
const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
@@ -438,6 +461,7 @@ bool StaticEquilibrium::computePolytopeProjection(Cref_matrix6X v)
// getProfiler().stop("eigen_to_cdd");
dd_ErrorType error = dd_NoError;
+ m_is_cdd_stable = true;
// getProfiler().start("dd_DDMatrix2Poly");
dd_PolyhedraPtr H_= dd_DDMatrix2Poly(V, &error);
@@ -446,6 +470,7 @@ bool StaticEquilibrium::computePolytopeProjection(Cref_matrix6X v)
if(error != dd_NoError)
{
SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
+ m_is_cdd_stable = false;
return false;
}
--
GitLab