From 4ed41dbf51de321e93e7039493e76b4f939a0adf Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Thu, 23 Feb 2017 11:49:31 +0100
Subject: [PATCH] in checkAdmissible acceleration : retry if lpStatus==ERROR
---
src/static_equilibrium.cpp | 12 ++++++++----
1 file changed, 8 insertions(+), 4 deletions(-)
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index 13c6589..594e337 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -75,7 +75,7 @@ bool StaticEquilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matr
for(long int i=0; i<c; i++)
{
// check that contact normals have norm 1
- if(fabs(contactNormals.row(i).norm()-1.0)>1e-6)
+ if(fabs(contactNormals.row(i).norm()-1.0)>1e-4)
{
SEND_ERROR_MSG("Contact normals should have norm 1, this has norm %f"+toString(contactNormals.row(i).norm()));
return false;
@@ -620,15 +620,19 @@ bool StaticEquilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrix
VectorX ub = VectorX::Ones(m)*1e10; // Inf
VectorX Alb = H*a + h;
VectorX Aub = H*a + h;
+ int iter = 0;
+ LP_status lpStatus;
+ do{
+ lpStatus = m_solver->solve(c, lb, ub, G, Alb, Aub, b);
+ iter ++;
+ }while(lpStatus == LP_STATUS_ERROR && iter < 5);
-
- LP_status lpStatus = m_solver->solve(c, lb, ub, G, Alb, Aub, b);
if(lpStatus==LP_STATUS_OPTIMAL || lpStatus==LP_STATUS_UNBOUNDED)
{
return true;
}
else{
- SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
+ //SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
return false;
}
}
--
GitLab