From aa592a5bdf40e05a0c947e3bce4acb937b2a51f9 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Wed, 15 Mar 2017 15:54:35 +0100
Subject: [PATCH] add mutator for m_G_centr
---
.../centroidal_dynamics.hh | 2 ++
src/centroidal_dynamics.cpp | 19 ++-----------------
2 files changed, 4 insertions(+), 17 deletions(-)
diff --git a/include/centroidal-dynamics-lib/centroidal_dynamics.hh b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
index e58532f..001c00c 100644
--- a/include/centroidal-dynamics-lib/centroidal_dynamics.hh
+++ b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
@@ -147,6 +147,8 @@ public:
bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg);
+ void setG(Cref_matrix6X G){m_G_centr = G;}
+
/**
* @brief Compute a measure of the robustness of the equilibrium of the specified com position.
* This amounts to solving the following LP:
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index 3799f9a..4346d93 100644
--- a/src/centroidal_dynamics.cpp
+++ b/src/centroidal_dynamics.cpp
@@ -325,21 +325,6 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
}
-LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness){
- // Take the acceleration in account in D and d :
- Matrix63 old_D = m_D;
- Vector6 old_d = m_d;
- m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
- m_d.head<3>()= m_mass * (m_gravity - acc);
- // compute equilibrium robustness with the new D and d
- LP_status status = computeEquilibriumRobustness(com,robustness);
- // Switch back to the original values of D and d
- m_D = old_D;
- m_d = old_d;
- return status;
-}
-
-
LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
{
if(m_G_centr.cols()==0)
@@ -601,7 +586,7 @@ double Equilibrium::convert_emax_to_b0(double emax)
-LP_status StaticEquilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,Cref_vector3 v, double& alpha0){
+LP_status Equilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,Cref_vector3 v, double& alpha0){
int m = (int)m_G_centr.cols() -1 ; // 4* number of contacts
VectorX b_a0(m+1);
VectorX c = VectorX::Zero(m+1);
@@ -633,7 +618,7 @@ LP_status StaticEquilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vecto
}
-bool StaticEquilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a ){
+bool Equilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a ){
int m = (int)m_G_centr.cols(); // number of contact * generator per contacts
VectorX b(m);
VectorX c = VectorX::Zero(m);
--
GitLab