Commit 86b1c65d authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

new methods use member matrices instead of parameter

parent b0c9d98d
......@@ -280,7 +280,7 @@ public:
* @brief findMaximumAcceleration Find the maximal acceleration along a given direction
find b, alpha0
maximize alpha0
subject to -h <= [-G (Hv)] [b a0]^T <= -h
subject to [-G (Hv)] [b a0]^T = -h
0 <= [b a0]^T <= Inf
......@@ -289,13 +289,15 @@ public:
alpha0 is the maximal amplitude of the acceleration, for the given direction v
c is the CoM position
G is the matrix whose columns are the gravito-inertial wrench generators
A is [-G (Hv)]
* @param A
* @param h
* @param alpha0
H is m*[I3 ĉ]^T
h is m*[-g (c x -g)]^T
* @param H input
* @param h input
* @param v input
* @param alpha0 output
* @return The status of the LP solver.
*/
LP_status findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0);
LP_status findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 v, double& alpha0);
/**
* @brief checkAdmissibleAcceleration return true if the given acceleration is admissible for the given contacts
......@@ -305,14 +307,12 @@ public:
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
H is m*[I3 ĉ]^T
h is m*[-g (c x -g)]^T
* @param a the acceleration
* @return true if the acceleration is admissible, false otherwise
*/
bool checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a );
bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a );
};
......
......@@ -62,6 +62,7 @@ namespace centroidal_dynamics
typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
typedef const Eigen::Ref<const Matrix43> & Cref_matrix43;
typedef const Eigen::Ref<const Matrix6X> & Cref_matrix6X;
typedef const Eigen::Ref<const Matrix63> & Cref_matrix63;
typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
/**Column major definitions for compatibility with classical eigen use**/
......
......@@ -324,12 +324,21 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
return LP_STATUS_ERROR;
}
/**
m_d.setZero();
m_d.head<3>() = m_mass*m_gravity;
m_D.setZero();
m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
*/
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)
{
......@@ -591,10 +600,14 @@ double Equilibrium::convert_emax_to_b0(double emax)
}
LP_status Equilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0){
int m = (int)A.cols() -1 ; // 4* number of contacts
LP_status StaticEquilibrium::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);
MatrixXX A = MatrixXX::Zero(6, m+1);
A.topLeftCorner(6,m) = - m_G_centr;
A.topRightCorner(6,1) = H * v;
c(m) = -1.0; // because we search max alpha0
VectorX lb = VectorX::Zero(m+1);
VectorX ub = VectorX::Ones(m+1)*1e10; // Inf
......@@ -619,8 +632,9 @@ LP_status Equilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h,
}
bool Equilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a ){
int m = (int)G.cols(); // number of contact * 4
bool StaticEquilibrium::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);
VectorX lb = VectorX::Zero(m);
......@@ -630,9 +644,9 @@ bool Equilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H,
int iter = 0;
LP_status lpStatus;
do{
lpStatus = m_solver->solve(c, lb, ub, G, Alb, Aub, b);
lpStatus = m_solver->solve(c, lb, ub, m_G_centr, Alb, Aub, b);
iter ++;
}while(lpStatus == LP_STATUS_ERROR && iter < 5);
}while(lpStatus == LP_STATUS_ERROR && iter < 3);
if(lpStatus==LP_STATUS_OPTIMAL || lpStatus==LP_STATUS_UNBOUNDED)
{
......
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