From f69de2601337b47ef82a8713fb109d8aa0de2919 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Fri, 30 Mar 2018 10:09:36 +0200
Subject: [PATCH] implement checkRobustEquilibrium with non null acceleration
for algorithm PP
---
.../centroidal_dynamics.hh | 24 +++++++++++++++++++
src/centroidal_dynamics.cpp | 11 ++++++++-
2 files changed, 34 insertions(+), 1 deletion(-)
diff --git a/include/centroidal-dynamics-lib/centroidal_dynamics.hh b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
index 5148bbe..50bfb01 100644
--- a/include/centroidal-dynamics-lib/centroidal_dynamics.hh
+++ b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
@@ -225,6 +225,30 @@ public:
*/
LP_status checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max=0.0);
+
+ /**
+ * @brief Check whether the specified com position is in robust equilibrium.
+ * This amounts to solving the following feasibility LP:
+ * find b
+ * minimize 1
+ * subject to G b = D c + d
+ * b >= b0
+ * where:
+ * b are the coefficient of the contact force generators (f = G b)
+ * b0 is a parameter proportional to the specified robustness measure
+ * c is the specified CoM position
+ * G is the 6xm matrix whose columns are the gravito-inertial wrench generators
+ * D is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
+ * d is the 6d vector containing the gravity part of the gravito-inertial wrench
+ * @param com The 3d center of mass position to test.
+ * @param acc The 3d acceleration of the CoM.
+ * @param equilibrium True if com is in robust equilibrium, false otherwise.
+ * @param e_max Desired robustness level.
+ * @return The status of the LP solver.
+ */
+ LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max=0.0);
+
+
/**
* @brief Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
* This amounts to solving the following LP:
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index e63c550..4b2fd0e 100644
--- a/src/centroidal_dynamics.cpp
+++ b/src/centroidal_dynamics.cpp
@@ -366,9 +366,18 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
return LP_STATUS_ERROR;
}
+LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max){
+ checkRobustEquilibrium(com,zero_acc,equilibrium,e_max);
+}
-LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
+LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max)
{
+ // Take the acceleration in account in D and d :
+ m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
+ m_d.head<3>()= m_mass * (m_gravity - acc);
+ m_HD = m_H * m_D;
+ m_Hd = m_H * m_d;
+
if(m_G_centr.cols()==0)
{
equilibrium=false;
--
GitLab