From 22ec3a35a928c695aefba42f0010b0eaff2097f9 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Tue, 22 Nov 2016 16:12:48 +0100
Subject: [PATCH] add StaticEquilibrium::findMaximumAcceleration
---
.../static_equilibrium.hh | 21 +++++++++++++++
include/robust-equilibrium-lib/util.hh | 1 +
src/static_equilibrium.cpp | 26 +++++++++++++++++++
3 files changed, 48 insertions(+)
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index a30dea6..08c166f 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -234,6 +234,27 @@ public:
* contact has been defined, will return LP_STATUS_INFEASIBLE
*/
LP_status getPolytopeInequalities(MatrixXX& H, VectorX& h) const;
+
+ /**
+ * @brief findMaximumAcceleration Find the maximal acceleration along a given direction
+ find b, alpha0
+ minimize -alpha0
+ subject to -h <= [-G (Hv)] [b a0]^T <= -h
+ 0 <= [b a0]^T <= Inf
+
+
+ b are the coefficient of the contact force generators (f = V b)
+ v is the vector3 defining the direction of the motion
+ alpha0 is the maximal amplitude of the acceleration, for the given direction v
+ c is the CoM position
+ G is the matrix whose columns are the gravito-inertial wrench generators
+ A is [-G (Hv)]
+ * @param A
+ * @param h
+ * @param alpha0
+ * @return The status of the LP solver.
+ */
+ LP_status findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0);
};
} // end namespace robust_equilibrium
diff --git a/include/robust-equilibrium-lib/util.hh b/include/robust-equilibrium-lib/util.hh
index 522f8d8..f6c35c3 100644
--- a/include/robust-equilibrium-lib/util.hh
+++ b/include/robust-equilibrium-lib/util.hh
@@ -56,6 +56,7 @@ namespace robust_equilibrium
typedef const Eigen::Ref<const Vector2> & Cref_vector2;
typedef const Eigen::Ref<const Vector3> & Cref_vector3;
+ typedef const Eigen::Ref<const Vector6> & Cref_vector6;
typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
typedef const Eigen::Ref<const Rotation> & Cref_rotation;
typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index 8e4de3e..50d90fa 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -562,4 +562,30 @@ double StaticEquilibrium::convert_emax_to_b0(double emax)
return (emax/m_b0_to_emax_coefficient);
}
+
+LP_status StaticEquilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0){
+ int m = (int)A.cols() -1 ; // 4* number of contacts
+ VectorX b_a0(m+1);
+ VectorX c = VectorX::Zero(m+1);
+ c(m) = -1.0; // because we search max alpha0
+ VectorX lb = VectorX::Zero(m+1);
+ VectorX ub = VectorX::Ones(m+1)*1e10; // Inf
+ VectorX Alb = -h;
+ VectorX Aub = -h;
+
+
+ LP_status lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_a0);
+ if(lpStatus==LP_STATUS_OPTIMAL)
+ {
+ alpha0 = -1.0 * m_solver->getObjectiveValue();
+ return lpStatus;
+ }
+ alpha0 = 0.0;
+ SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
+ return lpStatus;
+
+}
+
+
+
} // end namespace robust_equilibrium
--
GitLab