diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index 1eed5f438b9ac38c01186ccdb22b57cb1125e5e2..61298c472909e7e8ca2036e0b7e4d084f05f4dcc 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/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);
};
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index f7e4705f41defa1d0bb98d1f3cc69f4a0c88c799..6e194f137dc3fd375780622caa366d7e6a76c31b 100644
--- a/src/static_equilibrium.cpp
+++ b/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;
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 101846d1880a7636aec8dc29dbe8435605ce7aad..5421ab8a6f126c6663ba495afe9c7ef12e1391d5 100644
--- a/test/test_static_equilibrium.cpp
+++ b/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)