From dbbb6731540561bf718ca4489a412b2c679002d7 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Tue, 29 Nov 2016 15:15:24 +0100
Subject: [PATCH] add method checkAdmissibleAcceleration
---
.../static_equilibrium.hh | 20 ++++++++++++++++++-
src/static_equilibrium.cpp | 20 +++++++++++++++++++
2 files changed, 39 insertions(+), 1 deletion(-)
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index 08c166f..a0e47c7 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 50d90fa..09c97ac 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
--
GitLab