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

add method checkAdmissibleAcceleration

parent 22ec3a35
No related branches found
No related tags found
No related merge requests found
......@@ -238,7 +238,7 @@ public:
/**
* @brief findMaximumAcceleration Find the maximal acceleration along a given direction
find b, alpha0
minimize -alpha0
maximize alpha0
subject to -h <= [-G (Hv)] [b a0]^T <= -h
0 <= [b a0]^T <= Inf
......@@ -255,6 +255,24 @@ public:
* @return The status of the LP solver.
*/
LP_status findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0);
/**
* @brief checkAdmissibleAcceleration return true if the given acceleration is admissible for the given contacts
find b
subject to G b = Ha + h
0 <= b <= Inf
b are the coefficient of the contact force generators (f = V b)
a is the vector3 defining the acceleration
G is the matrix whose columns are the gravito-inertial wrench generators
h and H come from polytope inequalities
* @param G
* @param H
* @param h
* @param a
* @return true if the acceleration is admissible, false otherwise
*/
bool checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a );
};
} // end namespace robust_equilibrium
......
......@@ -586,6 +586,26 @@ LP_status StaticEquilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vecto
}
bool StaticEquilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a ){
int m = (int)G.cols(); // number of contact * 4
VectorX b(m);
VectorX c = VectorX::Zero(m);
VectorX lb = VectorX::Zero(m);
VectorX ub = VectorX::Ones(m)*1e10; // Inf
VectorX Alb = H*a + h;
VectorX Aub = H*a + h;
LP_status lpStatus = m_solver->solve(c, lb, ub, G, Alb, Aub, b);
if(lpStatus==LP_STATUS_OPTIMAL)
{
return true;
}
else{
SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
return false;
}
}
} // end namespace robust_equilibrium
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