[IMPORTANT CHANGE] Take com as a 3d point rather than a 2d point (this should...

[IMPORTANT CHANGE] Take com as a 3d point rather than a 2d point (this should allow to consider cases in which gravityis not aligned with the z axis in the future).

include/robust-equilibrium-lib/static_equilibrium.hh

@@ -46,11 +46,11 @@ private:
   VectorX m_h;
 
   /** Inequality matrix and vector defining the CoM support polygon HD com + Hd <= h */
-  MatrixX2 m_HD;
+  MatrixX3 m_HD;
   VectorX  m_Hd;
 
   /** Matrix and vector mapping 2d com position to GIW */
-  Matrix62 m_D;
+  Matrix63 m_D;
   Vector6 m_d;
 
   /** Coefficient used for converting the robustness measure in Newtons */
@@ -112,13 +112,13 @@ public:
    *     b0        is a parameter proportional to the robustness measure
    *     c         is the specified CoM position
    *     G         is the 6xm matrix whose columns are the gravito-inertial wrench generators
-   *     D         is the 6x2 matrix mapping the CoM position in gravito-inertial wrench
+   *     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 2d center of mass position to test.
+   * @param com The 3d center of mass position to test.
    * @param robustness The computed measure of robustness.
    * @return The status of the LP solver.
    */
-  LP_status computeEquilibriumRobustness(Cref_vector2 com, double &robustness);
+  LP_status computeEquilibriumRobustness(Cref_vector3 com, double &robustness);
 
   /**
    * @brief Check whether the specified com position is in robust equilibrium.
@@ -132,14 +132,14 @@ public:
    *     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 6x2 matrix mapping the CoM position in gravito-inertial wrench
+   *     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 2d center of mass position to test.
+   * @param com The 3d center of mass position to test.
    * @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_vector2 com, bool &equilibrium, double e_max=0.0);
+  LP_status checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max=0.0);
 
   /**
    * @brief Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
@@ -153,14 +153,14 @@ public:
    *     b0        is an m-dimensional vector of identical values that are proportional to e_max
    *     c         is the 1d line parameter
    *     G         is the 6xm matrix whose columns are the gravito-inertial wrench generators
-   *     D         is the 6x2 matrix mapping the CoM position in gravito-inertial wrench
+   *     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 a 2d vector representing the line direction
    * @param a0 2d vector representing an arbitrary point over the line
    * @param e_max Desired robustness in terms of the maximum force error tolerated by the system
    * @return The status of the LP solver.
   */
-  LP_status findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com);
+  LP_status findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com);
 
   /**
    * @brief Find the extremum com position that is in robust equilibrium in the specified direction.
@@ -173,16 +173,16 @@ public:
    *     a         is the specified 2d direction
    *     b         are the m coefficients of the contact force generators (f = G b)
    *     b0        is an m-dimensional vector of identical values that are proportional to e_max
-   *     c         is the 2d com position
+   *     c         is the 3d com position
    *     G         is the 6xm matrix whose columns are the gravito-inertial wrench generators
-   *     D         is the 6x2 matrix mapping the CoM position in gravito-inertial wrench
+   *     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 direction Desired 2d direction.
-   * @param com Output 2d com position.
+   * @param direction Desired 3d direction.
+   * @param com Output 3d com position.
    * @param e_max Desired robustness level.
    * @return The status of the LP solver.
    */
-  LP_status findExtremumInDirection(Cref_vector2 direction, Ref_vector2 com, double e_max=0.0);
+  LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max=0.0);
 
 };
 

src/static_equilibrium.cpp

@@ -43,7 +43,7 @@ StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int gene
   m_d.setZero();
   m_d.head<3>() = m_mass*m_gravity;
   m_D.setZero();
-  m_D.block<3,2>(3,0) = crossMatrix(-m_mass*m_gravity).leftCols<2>();
+  m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
 }
 
 bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
@@ -133,7 +133,7 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
 }
 
 
-LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com, double &robustness)
+LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &robustness)
 {
   const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
 
@@ -253,7 +253,7 @@ LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com, doub
   return LP_STATUS_ERROR;
 }
 
-LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, bool &equilibrium, double e_max)
+LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
 {
   if(e_max!=0.0)
   {
@@ -278,7 +278,7 @@ LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, bool &equi
   return LP_STATUS_OPTIMAL;
 }
 
-LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com)
+LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com)
 {
   const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
   double b0 = convert_emax_to_b0(e_max);
@@ -397,7 +397,7 @@ LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a
   return LP_STATUS_ERROR;
 }
 
-LP_status StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, Ref_vector2 com, double e_max)
+LP_status StaticEquilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max)
 {
   SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
   return LP_STATUS_ERROR;

test/test_static_equilibrium.cpp

@@ -160,16 +160,16 @@ int test_computeEquilibriumRobustness(StaticEquilibrium solver_1, StaticEquilibr
  * @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
  */
 int test_findExtremumOverLine(StaticEquilibrium &solver_to_test, StaticEquilibrium &solver_ground_truth,
-                              Cref_vector2 a0, int N_TESTS, double e_max,
+                              Cref_vector3 a0, int N_TESTS, double e_max,
                               const char* PERF_STRING_TEST, const char* PERF_STRING_GROUND_TRUTH, int verb=0)
 {
   int error_counter = 0;
-  Vector2 a, com;
+  Vector3 a, com;
   LP_status status;
   double desired_robustness, robustness;
   for(unsigned int i=0; i<N_TESTS; i++)
   {
-    uniform(-1.0*Vector2::Ones(), Vector2::Ones(), a);
+    uniform(-1.0*Vector3::Ones(), Vector3::Ones(), a);
     if(e_max>=0.0)
       desired_robustness = (rand()/ value_type(RAND_MAX))*e_max;
     else
@@ -349,7 +349,7 @@ int main()
   VectorX x_range(GRID_SIZE), y_range(GRID_SIZE);
   x_range.setLinSpaced(GRID_SIZE,com_LB(0),com_UB(0));
   y_range.setLinSpaced(GRID_SIZE,com_LB(1),com_UB(1));
-  MatrixXX comPositions(GRID_SIZE*GRID_SIZE, 2);
+  MatrixXX comPositions(GRID_SIZE*GRID_SIZE, 3);
   cout<<"Gonna test equilibrium on a 2d grid of "<<GRID_SIZE<<"X"<<GRID_SIZE<<" points ";
   cout<<"ranging from "<<com_LB<<" to "<<com_UB<<endl;
   for(unsigned int i=0; i<GRID_SIZE; i++)
@@ -358,11 +358,12 @@ int main()
     {
       comPositions(i*GRID_SIZE+j, 1) = y_range(GRID_SIZE-1-i);
       comPositions(i*GRID_SIZE+j, 0) = x_range(j);
+      comPositions(i*GRID_SIZE+j, 2) = 0.0;
 
       // look for contact point positions on grid
       for(long k=0; k<4*N_CONTACTS; k++)
       {
-        double dist = (p.block<1,2>(k,0) - comPositions.row(i*GRID_SIZE+j)).norm();
+        double dist = (p.block<1,2>(k,0) - comPositions.block<1,2>(i*GRID_SIZE+j,0)).norm();
         if(dist < minDistContactPoint(k))
         {
           minDistContactPoint(k) = dist;
@@ -477,7 +478,8 @@ int main()
 
 
   const int N_TESTS = 100;
-  Vector2 a0 = 0.5*(com_LB+com_UB);
+  Vector3 a0 = Vector3::Zero();
+  a0.head<2>() = 0.5*(com_LB+com_UB);
   double e_max;
   LP_status status = solver_LP_oases.computeEquilibriumRobustness(a0, e_max);
   if(status!=LP_STATUS_OPTIMAL)