diff --git a/include/robust-equilibrium-lib/logger.hh b/include/robust-equilibrium-lib/logger.hh
index bd5f8c209e039260ed61230b02f59ec1e39f6d81..43692ec18fe664605231e940f41f87a8a4642911 100644
--- a/include/robust-equilibrium-lib/logger.hh
+++ b/include/robust-equilibrium-lib/logger.hh
@@ -20,8 +20,11 @@
namespace robust_equilibrium
{
- //#define LOGGER_VERBOSITY_INFO_WARNING_ERROR
-#define LOGGER_VERBOSITY_INFO_WARNING_ERROR
+//#define LOGGER_VERBOSITY_ERROR
+//#define LOGGER_VERBOSITY_WARNING_ERROR
+//#define LOGGER_VERBOSITY_INFO_WARNING_ERROR
+//#define LOGGER_VERBOSITY_ALL
+#define LOGGER_VERBOSITY_ALL
#define SEND_MSG(msg,type) getLogger().sendMsg(msg,type,__FILE__,__LINE__)
diff --git a/include/robust-equilibrium-lib/util.hh b/include/robust-equilibrium-lib/util.hh
index 767f1bcdc42880d5facd0ce71c657b290ad49c37..9dbe3049ac22215c4254c1cbe00bcb94668a477e 100644
--- a/include/robust-equilibrium-lib/util.hh
+++ b/include/robust-equilibrium-lib/util.hh
@@ -57,24 +57,6 @@ 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 MatrixXX> & Cref_matrixXX;
-#else
-typedef Vector2 & Ref_vector2;
-typedef Vector3 & Ref_vector3;
-typedef VectorX & Ref_vectorX;
-typedef Rotation & Ref_rotation;
-typedef MatrixX3 & Ref_matrixX3;
-typedef Matrix43 & Ref_matrix43;
-typedef Matrix6X & Ref_matrix6X;
-typedef MatrixXX & Ref_matrixXX;
-
-typedef const Vector2 & Cref_vector2;
-typedef const Vector3 & Cref_vector3;
-typedef const VectorX & Cref_vectorX;
-typedef const Rotation & Cref_rotation;
-typedef const MatrixX3 & Cref_matrixX3;
-typedef const Matrix43 & Cref_matrix43;
-typedef const Matrix6X & Cref_matrix6X;
-typedef const MatrixXX & Cref_matrixXX;
#endif
/**
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index cc9ccc7bcb441587aeed7021385f9cc1f41d1ad2..27116eb8fbe1ceaa3945f5bdf006142fec33ff08 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -24,10 +24,10 @@ StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int gene
m_is_cdd_initialized = true;
}
- if(generatorsPerContact!=4)
+ if(generatorsPerContact<3)
{
- SEND_WARNING_MSG("Algorithm currently supports only 4 generators per contact!");
- generatorsPerContact = 4;
+ SEND_WARNING_MSG("Algorithm cannot work with less than 3 generators per contact!");
+ generatorsPerContact = 3;
}
m_name = name;
@@ -66,18 +66,16 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
// compute tangent directions
long int c = contactPoints.rows();
- int cg = m_generatorsPerContact;
- double muu;
+ unsigned int &cg = m_generatorsPerContact;
+ double theta, delta_theta=2*M_PI/cg;
// Tangent directions
- RVector3 T1, T2;
- // Matrix mapping contact forces to gravito-inertial wrench (6 X 3)
+ Vector3 T1, T2;
+ // Matrix mapping a 3d contact force to gravito-inertial wrench (6 X 3)
Matrix63 A;
A.topRows<3>() = -Matrix3::Identity();
// Lists of contact generators (3 X generatorsPerContact)
Matrix3X G(3, cg);
- long int m = c*cg; // number of generators
- m_G_centr.resize(6,m);
-
+ m_G_centr.resize(6,c*cg);
for(long int i=0; i<c; i++)
{
@@ -95,33 +93,25 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
T1.normalize();
T2.normalize();
-// cout<<"Contact point "<<i<<"\nT1="<<T1<<"\nT2="<<T2<<"\n";
-
// compute matrix mapping contact forces to gravito-inertial wrench
A.bottomRows<3>() = crossMatrix(-1.0*contactPoints.row(i).transpose());
-// cout<<"A:\n"<<A<<"\n";
// compute generators
- muu = frictionCoefficients(i)/sqrt(2.0);
- G.col(0) = muu*T1 + muu*T2 + contactNormals.row(i);
- G.col(1) = muu*T1 - muu*T2 + contactNormals.row(i);
- G.col(2) = -muu*T1 + muu*T2 + contactNormals.row(i);
- G.col(3) = -muu*T1 - muu*T2 + contactNormals.row(i);
-
- // normalize generators
- G.col(0).normalize();
- G.col(1).normalize();
- G.col(2).normalize();
- G.col(3).normalize();
-
-// cout<<"Generators contact "<<i<<"\n"<<G.transpose()<<"\n";
+ theta = 0.0;
+ for(int j=0; j<cg; j++)
+ {
+ G.col(j) = frictionCoefficients(i)*sin(theta)*T1
+ + frictionCoefficients(i)*cos(theta)*T2
+ + contactNormals.row(i).transpose();
+ G.col(j).normalize();
+// SEND_DEBUG_MSG("Contact "+toString(i)+" generator "+toString(j)+" = "+toString(G.col(j).transpose()));
+ theta += delta_theta;
+ }
// project generators in 6d centroidal space
m_G_centr.block(0,cg*i,6,cg) = A * G;
}
-// cout<<"G_centr:\n"<<m_G_centr.transpose()<<"\n";
-
if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_PP)
{
if(!computePolytopeProjection(m_G_centr))
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 4bf4602715e39299857640b81a85452d7563fddf..2834c568d16d003b1c3edd21adeaa78b32ef0689 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -75,19 +75,19 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium solv
if(equilibrium==true && rob<0.0)
{
- if(verb>0)
- SEND_ERROR_MSG(solver_1.getName()+" says com is in equilibrium while "+solver_2.getName()+" computed a negative robustness measure "+toString(rob));
+ if(verb>1)
+ SEND_ERROR_MSG(solver_2.getName()+" says com is in equilibrium while "+solver_1.getName()+" computed a negative robustness measure "+toString(rob));
error_counter++;
}
else if(equilibrium==false && rob>0.0)
{
- if(verb>0)
- SEND_ERROR_MSG(solver_1.getName()+" says com is not in equilibrium while "+solver_2.getName()+" computed a positive robustness measure "+toString(rob));
+ if(verb>1)
+ SEND_ERROR_MSG(solver_2.getName()+" says com is not in equilibrium while "+solver_1.getName()+" computed a positive robustness measure "+toString(rob));
error_counter++;
}
}
- if(verb>1)
+ if(verb>0)
cout<<"Test test_computeEquilibriumRobustness_vs_checkEquilibrium "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).\n";
return error_counter;
}
@@ -260,21 +260,24 @@ int main()
/************************************** USER PARAMETERS *******************************/
double mass = 70.0;
double mu = 0.3; // friction coefficient
- unsigned int generatorsPerContact = 4;
+ unsigned int generatorsPerContact = 8;
unsigned int N_CONTACTS = 2;
double MIN_FEET_DISTANCE = 0.3;
- double LX = 0.5*0.2172; // half foot size in x direction
- double LY = 0.5*0.138; // half foot size in y direction
+ double LX = 0.5*0.2172; // half contact surface size in x direction
+ double LY = 0.5*0.138; // half contact surface size in y direction
CONTACT_POINT_LOWER_BOUNDS << 0.0, 0.0, 0.0;
CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 0.5;
double gamma = atan(mu); // half friction cone angle
- RPY_LOWER_BOUNDS << -1*gamma, -1*gamma, -M_PI;
- RPY_UPPER_BOUNDS << +1*gamma, +1*gamma, +M_PI;
+ RPY_LOWER_BOUNDS << -0*gamma, -0*gamma, -M_PI;
+ RPY_UPPER_BOUNDS << +0*gamma, +0*gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
const int GRID_SIZE = 15;
/************************************ END USER PARAMETERS *****************************/
+ cout<<"Number of contacts: "<<N_CONTACTS<<endl;
+ cout<<"Number of generators per contact: "<<generatorsPerContact<<endl;
+
StaticEquilibrium solver_PP("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES);
StaticEquilibrium solver_LP_oases("LP oases", mass, generatorsPerContact, SOLVER_LP_QPOASES);
StaticEquilibrium solver_LP2_oases("LP2 oases", mass, generatorsPerContact, SOLVER_LP_QPOASES);
@@ -447,6 +450,7 @@ int main()
#endif
+ cout<<"\nRobustness grid computed with DLP oases\n";
drawRobustnessGrid(solver_DLP_oases, comPositions);
test_computeEquilibriumRobustness(solver_DLP_oases, solver_LP_oases, comPositions, PERF_DLP_OASES, PERF_LP_OASES, 1);