Commit f69de260 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

implement checkRobustEquilibrium with non null acceleration for algorithm PP

parent 583fa92c
......@@ -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:
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment