diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp index 7a4c5c5658a675403d361d4eb7b444af49c8133f..d17db93e5eb74d004ebcdaf0aad7ef4c294b12a4 100644 --- a/src/centroidal_dynamics.cpp +++ b/src/centroidal_dynamics.cpp @@ -517,6 +517,9 @@ LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vecto bool Equilibrium::computePolytopeProjection(Cref_matrix6X v) { + int n = (int)(v.rows()); + int m = (int)(v.cols()); + // getProfiler().start("eigen_to_cdd"); dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix); // getProfiler().stop("eigen_to_cdd"); @@ -571,6 +574,15 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v) } // getProfiler().stop("cdd to eigen"); + std::cout<<" inequalities : m = "<<m<<std::endl; + if(m_h.rows() < n ) + { + SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope"); + m_is_cdd_stable = false; + return false; + } + + return true; }