Skip to content
Snippets Groups Projects
Commit f69de260 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

implement checkRobustEquilibrium with non null acceleration for algorithm PP

parent 583fa92c
No related branches found
No related tags found
No related merge requests found
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment