diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5faf2e4a4e5cff3503dda00244dfa921a445da59..56a2770f7bf99000862aa25cb18231df2dfe1325 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,6 +30,7 @@ MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
include/robust-equilibrium-lib/config.hh
+ include/robust-equilibrium-lib/util.hh
include/robust-equilibrium-lib/solver_LP_abstract.hh
include/robust-equilibrium-lib/solver_LP_qpoases.hh
include/robust-equilibrium-lib/static_equilibrium.hh
diff --git a/include/robust-equilibrium-lib/config.hh b/include/robust-equilibrium-lib/config.hh
index ef304c476b87539ad7856a2f22d21d833eb56b38..850e82db57b452d0fb84c75330ce5fc110aa4916 100644
--- a/include/robust-equilibrium-lib/config.hh
+++ b/include/robust-equilibrium-lib/config.hh
@@ -1,9 +1,6 @@
#ifndef _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
#define _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
-#include <Eigen/Dense>
-#include <Eigen/src/Core/util/Macros.h>
-
// Package version (header).
# define ROBUST_EQUILIBRIUM_VERSION "UNKNOWN"
@@ -52,62 +49,4 @@
# define ROBUST_EQUILIBRIUM_LOCAL ROBUST_EQUILIBRIUM_DLLLOCAL
# endif // ROBUST_EQUILIBRIUM_STATIC
-namespace robust_equilibrium
-{
-
-//#define USE_FLOAT 1;
-#ifdef USE_FLOAT
-typedef float value_type;
-#else
-typedef double value_type;
-#endif
-
-typedef Eigen::Matrix <value_type, 2, 1> Vector2;
-typedef Eigen::Matrix <value_type, 1, 2> RVector2;
-typedef Eigen::Matrix <value_type, 3, 1> Vector3;
-typedef Eigen::Matrix <value_type, 1, 3> RVector3;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> VectorX;
-typedef Eigen::Matrix <value_type, 1, Eigen::Dynamic> RVectorX;
-typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Rotation;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3;
-typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
-
-//define Eigen ref if available
-#if EIGEN_VERSION_AT_LEAST(3,2,0)
-typedef Eigen::Ref<Vector2> Ref_vector2;
-typedef Eigen::Ref<Vector3> Ref_vector3;
-typedef Eigen::Ref<VectorX> Ref_vectorX;
-typedef Eigen::Ref<Rotation> Ref_rotation;
-typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
-typedef Eigen::Ref<Matrix43> Ref_matrix43;
-typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
-
-typedef const Eigen::Ref<const Vector2> & Cref_vector2;
-typedef const Eigen::Ref<const Vector3> & Cref_vector3;
-typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
-typedef const Eigen::Ref<const Rotation> & Cref_rotation;
-typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
-typedef const Eigen::Ref<const Matrix43> & Cref_matrix43;
-typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
-#else
-typedef vector2_t & Ref_vector2;
-typedef vector3_t & Ref_vector3;
-typedef vector_t & Ref_vector;
-typedef rotation_t & Ref_rotation;
-typedef T_rotation_t& Ref_matrixX3;
-typedef T_rotation_t& Ref_matrix43;
-typedef matrix_t & Ref_matrixXX;
-
-typedef const vector2_t & Cref_vector2;
-typedef const vector3_t & Cref_vector3;
-typedef const vector_t & Cref_vector;
-typedef const rotation_t & Cref_rotation;
-typedef const T_rotation_t& Cref_matrixX3;
-typedef const T_rotation_t& Cref_matrix43;
-typedef const matrix_t & Cref_matrixXX;
-#endif
-
-} //namespace robust_equilibrium
-
#endif //_ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index 3fcc887cfee41445cfe22603ecd0e795702ce2e1..78604c80a782dc4db14baad4caaaab1d4663f4ab 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -3,6 +3,7 @@
#include <Eigen/Dense>
#include <robust-equilibrium-lib/config.hh>
+#include <robust-equilibrium-lib/util.hh>
#include <robust-equilibrium-lib/solver_LP_abstract.hh>
namespace robust_equilibrium
@@ -22,15 +23,40 @@ private:
StaticEquilibriumAlgorithm m_algorithm;
SolverLPAbstract* m_solver;
+ unsigned int m_generatorsPerContact;
+ double m_mass;
+ Vector3 m_gravity;
+
+ /** Tangent directions for all contacts (numberOfContacts*generatorsPerContact X 3) */
+ MatrixX3 m_T1, m_T2;
+ /** Matrix mapping contact forces to gravito-inertial wrench (6 X 3*numberOfContacts) */
+ Matrix6X m_A;
+ /** Lists of contact generators (3 X numberOfContacts*generatorsPerContact) */
+ Matrix3X m_G;
+ /** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
+ Matrix6X m_G_centr;
+ /** Inequality matrix defining the gravito-inertial wrench cone H w <= h */
+ MatrixXX m_H;
+ /** Inequality vector defining the gravito-inertial wrench cone H w <= h */
+ VectorX m_h;
+ /** Inequality matrix defining the CoM support polygon HD com + Hd <= h */
+ MatrixX2 m_HD;
+ VectorX m_Hd;
+ /** Matrix and vector mapping 2d com position to GIW */
+ Matrix62 m_D;
+ Vector6 m_d;
+
+ bool computePolytopeProjection(Cref_matrix6X v);
+
public:
- StaticEquilibrium(unsigned int generatorsPerContact, SolverLP solver_type);
+ StaticEquilibrium(double mass, unsigned int generatorsPerContact, SolverLP solver_type);
bool setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
Cref_vectorX frictionCoefficients, StaticEquilibriumAlgorithm alg);
double computeEquilibriumRobustness(Cref_vector2 com);
- double checkRobustEquilibrium(Cref_vector2 com, double e_max=0.0);
+ bool checkRobustEquilibrium(Cref_vector2 com, double e_max=0.0);
double findExtremumOverLine(Cref_vector2 a, double b, double e_max=0.0);
diff --git a/include/robust-equilibrium-lib/util.hh b/include/robust-equilibrium-lib/util.hh
new file mode 100644
index 0000000000000000000000000000000000000000..8131b542bb8dd6668b608026472d0538b6a39301
--- /dev/null
+++ b/include/robust-equilibrium-lib/util.hh
@@ -0,0 +1,105 @@
+#ifndef _ROBUST_EQUILIBRIUM_LIB_UTIL_HH
+#define _ROBUST_EQUILIBRIUM_LIB_UTIL_HH
+
+#include <Eigen/Dense>
+#include <Eigen/src/Core/util/Macros.h>
+
+#include "cdd/cddmp.h"
+#include "cdd/setoper.h"
+#include "cdd/cddtypes.h"
+#include "cdd/cdd.h"
+
+namespace robust_equilibrium
+{
+
+//#define USE_FLOAT 1;
+#ifdef USE_FLOAT
+typedef float value_type;
+#else
+typedef double value_type;
+#endif
+
+typedef Eigen::Matrix <value_type, 2, 1> Vector2;
+typedef Eigen::Matrix <value_type, 1, 2> RVector2;
+typedef Eigen::Matrix <value_type, 3, 1> Vector3;
+typedef Eigen::Matrix <value_type, 1, 3> RVector3;
+typedef Eigen::Matrix <value_type, 6, 1> Vector6;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> VectorX;
+typedef Eigen::Matrix <value_type, 1, Eigen::Dynamic> RVectorX;
+typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Rotation;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, 2, Eigen::RowMajor> MatrixX2;
+typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Matrix3;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3;
+typedef Eigen::Matrix <value_type, 3, Eigen::Dynamic, Eigen::RowMajor> Matrix3X;
+typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
+typedef Eigen::Matrix <value_type, 6, Eigen::Dynamic, Eigen::RowMajor> Matrix6X;
+typedef Eigen::Matrix <value_type, 6, 2, Eigen::RowMajor> Matrix62;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
+
+//define Eigen ref if available
+#if EIGEN_VERSION_AT_LEAST(3,2,0)
+typedef Eigen::Ref<Vector2> Ref_vector2;
+typedef Eigen::Ref<Vector3> Ref_vector3;
+typedef Eigen::Ref<VectorX> Ref_vectorX;
+typedef Eigen::Ref<Rotation> Ref_rotation;
+typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
+typedef Eigen::Ref<Matrix43> Ref_matrix43;
+typedef Eigen::Ref<Matrix6X> Ref_matrix6X;
+typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
+
+typedef const Eigen::Ref<const Vector2> & Cref_vector2;
+typedef const Eigen::Ref<const Vector3> & Cref_vector3;
+typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
+typedef const Eigen::Ref<const Rotation> & Cref_rotation;
+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_vector;
+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
+
+/**
+ * Convert the specified list of rays from Eigen to cdd format.
+ * @param input The mXn input Eigen matrix contains m rays of dimension n.
+ * @return The mX(n+1) output cdd matrix, which contains an additional column,
+ * the first one, with all zeros.
+ */
+dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input);
+
+/**
+ * Compute the cross-product skew-symmetric matrix associated to the specified vector.
+ */
+Rotation crossMatrix(Cref_vector3 x);
+
+
+void init_library();
+
+void release_library();
+
+void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out);
+
+void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R);
+
+bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy,
+ Ref_matrix43 p, Ref_matrix43 N);
+
+} //namespace robust_equilibrium
+
+#endif //_ROBUST_EQUILIBRIUM_LIB_UTIL_HH
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 3ff2216517d770915e62ff8e50ee38c6f5df2f12..e9646898a74d7fae6b672f185d0b95c2a1181fc6 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -14,6 +14,7 @@ SET(${LIBRARY_NAME}_SOURCES
${INCLUDE_DIR}/robust-equilibrium-lib/static_equilibrium.hh
static_equilibrium.cpp
solver_LP_qpoases.cpp
+ util.cpp
)
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index a136d38158beb0bcd9f584c2104abeb79d19a27a..de7335000a5987ce111c14489020991d26f813c8 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -1,17 +1,93 @@
#include <robust-equilibrium-lib/static_equilibrium.hh>
+#include <iostream>
+#include <vector>
+
+using namespace std;
namespace robust_equilibrium
{
-StaticEquilibrium::StaticEquilibrium(unsigned int generatorsPerContact, SolverLP solver_type)
+StaticEquilibrium::StaticEquilibrium(double mass, unsigned int generatorsPerContact, SolverLP solver_type)
{
+ m_generatorsPerContact = generatorsPerContact;
+ m_mass = mass;
+ m_gravity.setZero();
+ m_gravity(2) = -9.81;
+ 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>();
}
bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
- Cref_vectorX frictionCoefficients, StaticEquilibriumAlgorithm alg)
+ Cref_vectorX frictionCoefficients, StaticEquilibriumAlgorithm alg)
{
+ assert(contactPoints.rows()==contactNormals.rows());
+ assert(contactPoints.rows()==frictionCoefficients.rows());
+
+ // compute tangent directions
+ long int c = contactPoints.rows();
+ int cg = m_generatorsPerContact;
+ long int m = c*cg; // number of generators
+ double muu;
+ m_T1.resize(c,3);
+ m_T2.resize(c,3);
+ m_A.resize(6,3*c);
+ m_G.resize(3,m);
+ m_G_centr.resize(6,m);
+
+ for(long int i=0; i<c; i++)
+ {
+ // check that contact normals have norm 1
+ if(fabs(contactNormals.row(i).norm()-1.0)>1e-6)
+ {
+ printf("ERROR Contact normals should have norm 1, this has norm %f", contactNormals.row(i).norm());
+ return false;
+ }
+ // compute tangent directions
+ m_T1.row(i) = contactNormals.row(i).cross(Vector3::UnitY());
+ if(m_T1.row(i).norm()<1e-5)
+ m_T1.row(i) = contactNormals.row(i).cross(Vector3::UnitX());
+ m_T2.row(i) = contactNormals.row(i).transpose().cross(m_T1.row(i));
+ m_T1.row(i).normalize();
+ m_T2.row(i).normalize();
+
+// cout<<"Contact point "<<i<<"\nT1="<<m_T1.row(i)<<"\nT2="<<m_T2.row(i)<<"\n";
+
+ // compute matrix mapping contact forces to gravito-inertial wrench
+ m_A.block<3,3>(0, 3*i) = -Matrix3::Identity();
+ m_A.block<3,3>(3, 3*i) = crossMatrix(-1.0*contactPoints.row(i).transpose());
+// cout<<"A:\n"<<m_A.block<6,3>(0,3*i)<<"\n";
+
+ // compute generators
+ muu = frictionCoefficients(i)/sqrt(2.0);
+ m_G.col(cg*i+0) = muu*m_T1.row(i) + muu*m_T2.row(i) + contactNormals.row(i);
+ m_G.col(cg*i+1) = muu*m_T1.row(i) - muu*m_T2.row(i) + contactNormals.row(i);
+ m_G.col(cg*i+2) = -muu*m_T1.row(i) + muu*m_T2.row(i) + contactNormals.row(i);
+ m_G.col(cg*i+3) = -muu*m_T1.row(i) - muu*m_T2.row(i) + contactNormals.row(i);
+
+ // normalize generators
+ m_G.col(cg*i+0).normalize();
+ m_G.col(cg*i+1).normalize();
+ m_G.col(cg*i+2).normalize();
+ m_G.col(cg*i+3).normalize();
+
+// cout<<"Generators contact "<<i<<"\n"<<m_G.middleCols<4>(cg*i).transpose()<<"\n";
+ }
+
+ // project generators in 6d centroidal space
+ for(unsigned int i=0; i<c; i++)
+ m_G_centr.block(0,cg*i,6,cg) = m_A.block<6,3>(0,3*i) * m_G.block(0,cg*i,3,cg);
+// cout<<"G_centr:\n"<<m_G_centr.transpose()<<"\n";
+
+ if(!computePolytopeProjection(m_G_centr))
+ return false;
+
+ m_HD = m_H * m_D;
+ m_Hd = m_H * m_d;
+
return true;
}
@@ -20,9 +96,19 @@ double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
return 0.0;
}
-double StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
+bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
{
- return 0.0;
+ if(e_max!=0.0)
+ {
+ std::cerr<<"ERROR checkRobustEquilibrium with e_max!=0 not implemented yet!";
+ return false;
+ }
+
+ VectorX res = m_HD * com + m_Hd;
+ for(long i=0; i<res.size(); i++)
+ if(res(i)>0.0)
+ return false;
+ return true;
}
double StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, double b, double e_max)
@@ -35,5 +121,44 @@ double StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, double
return 0.0;
}
+bool StaticEquilibrium::computePolytopeProjection(Cref_matrix6X v)
+{
+ dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose());
+ dd_ErrorType error = dd_NoError;
+ dd_PolyhedraPtr H_= dd_DDMatrix2Poly(V, &error);
+ if(error != dd_NoError)
+ {
+// if(dd_debug)
+ cout << ("numerical instability in cddlib. ill formed polytope") << endl;
+ return false;
+ }
+
+ dd_MatrixPtr b_A = dd_CopyInequalities(H_);
+ // get equalities and add them as complementary inequality constraints
+ std::vector<long> eq_rows;
+ for(long elem=1;elem<=(long)(b_A->linset[0]);++elem)
+ {
+ if (set_member(elem,b_A->linset))
+ eq_rows.push_back(elem);
+ }
+ int rowsize = (int)b_A->rowsize;
+// cout<<"Inequality matrix has "<<rowsize<<" rows and "<<b_A->colsize-1<<" columns\n";
+ m_H.resize(rowsize + eq_rows.size(), (int)b_A->colsize-1);
+ m_h.resize(rowsize + eq_rows.size());
+ for(int i=0; i < rowsize; ++i)
+ {
+ m_h(i) = (value_type)(*(b_A->matrix[i][0]));
+ for(int j=1; j < b_A->colsize; ++j)
+ m_H(i, j-1) = -(value_type)(*(b_A->matrix[i][j]));
+ }
+ int i = 0;
+ for(std::vector<long int>::const_iterator cit = eq_rows.begin(); cit != eq_rows.end(); ++cit, ++i)
+ {
+ m_h(rowsize + i) = -m_h((int)(*cit));
+ m_H(rowsize + i) = -m_H((int)(*cit));
+ }
+
+ return true;
+}
} // end namespace robust_equilibrium
diff --git a/src/util.cpp b/src/util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..27de0bf2df4efdf136dd9905abefcbe35dbaf3d9
--- /dev/null
+++ b/src/util.cpp
@@ -0,0 +1,192 @@
+#ifndef _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
+#define _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
+
+#include <Eigen/Dense>
+#include <Eigen/src/Core/util/Macros.h>
+
+#include "cdd/cddmp.h"
+#include "cdd/setoper.h"
+#include "cdd/cddtypes.h"
+#include "cdd/cdd.h"
+
+namespace robust_equilibrium
+{
+
+//#define USE_FLOAT 1;
+#ifdef USE_FLOAT
+typedef float value_type;
+#else
+typedef double value_type;
+#endif
+
+typedef Eigen::Matrix <value_type, 2, 1> Vector2;
+typedef Eigen::Matrix <value_type, 1, 2> RVector2;
+typedef Eigen::Matrix <value_type, 3, 1> Vector3;
+typedef Eigen::Matrix <value_type, 1, 3> RVector3;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> VectorX;
+typedef Eigen::Matrix <value_type, 1, Eigen::Dynamic> RVectorX;
+typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Rotation;
+typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Matrix3;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3;
+typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
+typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
+
+//define Eigen ref if available
+#if EIGEN_VERSION_AT_LEAST(3,2,0)
+typedef Eigen::Ref<Vector2> Ref_vector2;
+typedef Eigen::Ref<Vector3> Ref_vector3;
+typedef Eigen::Ref<VectorX> Ref_vectorX;
+typedef Eigen::Ref<Rotation> Ref_rotation;
+typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
+typedef Eigen::Ref<Matrix43> Ref_matrix43;
+typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
+
+typedef const Eigen::Ref<const Vector2> & Cref_vector2;
+typedef const Eigen::Ref<const Vector3> & Cref_vector3;
+typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
+typedef const Eigen::Ref<const Rotation> & Cref_rotation;
+typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
+typedef const Eigen::Ref<const Matrix43> & Cref_matrix43;
+typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
+#else
+typedef vector2_t & Ref_vector2;
+typedef vector3_t & Ref_vector3;
+typedef vector_t & Ref_vector;
+typedef rotation_t & Ref_rotation;
+typedef T_rotation_t& Ref_matrixX3;
+typedef T_rotation_t& Ref_matrix43;
+typedef matrix_t & Ref_matrixXX;
+
+typedef const vector2_t & Cref_vector2;
+typedef const vector3_t & Cref_vector3;
+typedef const vector_t & Cref_vector;
+typedef const rotation_t & Cref_rotation;
+typedef const T_rotation_t& Cref_matrixX3;
+typedef const T_rotation_t& Cref_matrix43;
+typedef const matrix_t & Cref_matrixXX;
+#endif
+
+typedef Eigen::AngleAxis<value_type> angle_axis_t;
+
+
+dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input)
+{
+ dd_debug = false;
+ dd_MatrixPtr M=NULL;
+ dd_rowrange i;
+ dd_colrange j;
+ dd_rowrange m_input = (dd_rowrange)(input.rows());
+ dd_colrange d_input = (dd_colrange)(input.cols()+1);
+ dd_RepresentationType rep=dd_Generator;
+ mytype value;
+ dd_NumberType NT = dd_Real;
+ dd_init(value);
+
+ M=dd_CreateMatrix(m_input, d_input);
+ M->representation=rep;
+ M->numbtype=NT;
+
+ for (i = 0; i < input.rows(); i++)
+ {
+ dd_set_d(value, 0);
+ dd_set(M->matrix[i][0],value);
+ for (j = 1; j < d_input; j++)
+ {
+ dd_set_d(value, input(i,j-1));
+ dd_set(M->matrix[i][j],value);
+ }
+ }
+ dd_clear(value);
+ return M;
+}
+
+
+void init_library()
+{
+ dd_set_global_constants();dd_debug = false;
+}
+
+void release_library()
+{
+ //dd_free_global_constants();
+}
+
+void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out)
+{
+
+ assert(lower_bounds.rows()==out.rows());
+ assert(upper_bounds.rows()==out.rows());
+ assert(lower_bounds.cols()==out.cols());
+ assert(upper_bounds.cols()==out.cols());
+ for(int i=0; i<out.rows(); i++)
+ for(int j=0; j<out.cols(); j++)
+ out(i,j) = (rand()/ value_type(RAND_MAX))*(upper_bounds(i,j)-lower_bounds(i,j)) + lower_bounds(i,j);
+}
+
+void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R)
+{
+ const int i = 0;
+ const int j = 1;
+ const int k = 2;
+ double si = sin(roll);
+ double sj = sin(pitch);
+ double sk = sin(yaw);
+ double ci = cos(roll);
+ double cj = cos(pitch);
+ double ck = cos(yaw);
+ double cc = ci*ck;
+ double cs = ci*sk;
+ double sc = si*ck;
+ double ss = si*sk;
+
+ R(i, i) = cj*ck;
+ R(i, j) = sj*sc-cs;
+ R(i, k) = sj*cc+ss;
+ R(j, i) = cj*sk;
+ R(j, j) = sj*ss+cc;
+ R(j, k) = sj*cs-sc;
+ R(k, i) = -sj;
+ R(k, j) = cj*si;
+ R(k, k) = cj*ci;
+// R = (angle_axis_t(roll, Vector3::UnitX())
+// * angle_axis_t(pitch, Vector3::UnitY())
+// * angle_axis_t(yaw, Vector3::UnitZ())).toRotationMatrix();
+}
+
+bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy,
+ Ref_matrix43 p, Ref_matrix43 N)
+{
+ // compute rotation matrix
+ Rotation R;
+ euler_matrix(rpy(0), rpy(1), rpy(2), R);
+ // contact points in local frame
+ p << lx, ly, 0,
+ lx, -ly, 0,
+ -lx, -ly, 0,
+ -lx, ly, 0;
+ // contact points in world frame
+ p.row(0) = pos + (R*p.row(0).transpose());
+ p.row(1) = pos + (R*p.row(1).transpose());
+ p.row(2) = pos + (R*p.row(2).transpose());
+ p.row(3) = pos + (R*p.row(3).transpose());
+ // normal direction in local frame
+ RVector3 n;
+ n << 0, 0, 1;
+ // normal directions in world frame
+ n = (R*n.transpose()).transpose();
+ N << n, n, n, n;
+ return true;
+}
+
+Rotation crossMatrix(Cref_vector3 x)
+{
+ Rotation res = Rotation::Zero();
+ res(0,1) = - x(2); res(0,2) = x(1);
+ res(1,0) = x(2); res(1,2) = - x(0);
+ res(2,0) = - x(1); res(2,1) = x(0);
+ return res;
+}
+
+} //namespace robust_equilibrium
+
+#endif //_ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 71642fcd7fafda8141afccfe407ac3face6b227c..1dfaa639c113f8929b4c9da7f16884c85ba87d79 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -1,139 +1,161 @@
#include <vector>
-#include <Eigen/Dense>
-#include <Eigen/src/Core/util/Macros.h>
-
-#include "cdd/cddmp.h"
-#include "cdd/setoper.h"
-#include "cdd/cddtypes.h"
-#include "cdd/cdd.h"
-
+#include <iostream>
#include <robust-equilibrium-lib/static_equilibrium.hh>
using namespace robust_equilibrium;
using namespace Eigen;
+using namespace std;
-typedef Eigen::AngleAxis<value_type> angle_axis_t;
-
-
-void init_library()
-{
- dd_set_global_constants();dd_debug = false;
-}
-
-void release_library()
-{
- //dd_free_global_constants();
-}
-
-bool uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out)
-{
- assert(lower_bounds.rows()==out.rows());
- assert(upper_bounds.rows()==out.rows());
- assert(lower_bounds.cols()==out.cols());
- assert(upper_bounds.cols()==out.cols());
- for(int i=0; i<out.rows(); i++)
- for(int j=0; j<out.cols(); j++)
- out(i,j) = (rand()/ value_type(RAND_MAX))*(upper_bounds(i,j)-lower_bounds(i,j)) + lower_bounds(i,j);
-}
-
-bool euler_matrix(double roll, double pitch, double yaw, Ref_rotation R)
-{
- R = (angle_axis_t(roll, Vector3::UnitX())
- * angle_axis_t(pitch, Vector3::UnitY())
- * angle_axis_t(yaw, Vector3::UnitZ())).toRotationMatrix();
-}
-
-bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy,
- Ref_matrix43 p, Ref_matrix43 N)
-{
- // compute rotation matrix
- Quaternion<value_type> q = angle_axis_t(rpy(0), Vector3::UnitX())
- * angle_axis_t(rpy(1), Vector3::UnitY())
- * angle_axis_t(rpy(2), Vector3::UnitZ());
- //euler_matrix(rpy[0], rpy[1], rpy[2], R);
- // contact points in local frame
- p << lx, ly, 0,
- lx, -ly, 0,
- -lx, -ly, 0,
- -lx, ly, 0;
- // contact points in world frame
- p.row(0) = pos + q*p.row(0);
- p.row(1) = pos + q*p.row(1);
- p.row(2) = pos + q*p.row(2);
- p.row(3) = pos + q*p.row(3);
- // normal direction in local frame
- RVector3 n;
- n << 0, 0, 1;
- // normal directions in world frame
- n = q*n;
- N << n, n, n, n;
- return true;
-}
int main()
{
+ init_library();
+ srand ((unsigned int)(time(NULL)));
+
int ret = 0;
+ double mass = 70.0;
double mu = 0.3; // friction coefficient
unsigned int generatorsPerContact = 4;
- unsigned int N_CONTACTS = 8;
+ unsigned int N_CONTACTS = 2;
unsigned int N_POINTS = 50; // number of com positions to test
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
RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
- CONTACT_POINT_LOWER_BOUNDS <<-0.5, -0.5, 0.0;
- CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 1.0;
+ CONTACT_POINT_LOWER_BOUNDS << 0.0, 0.0, 0.0;
+ CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 0.0;
double gamma = atan(mu); // half friction cone angle
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
- RPY_LOWER_BOUNDS << -2*gamma, -2*gamma, -M_PI;
- RPY_UPPER_BOUNDS << +2*gamma, +2*gamma, +M_PI;
+ RPY_LOWER_BOUNDS << -0.0*gamma, -0.0*gamma, -M_PI;
+ RPY_UPPER_BOUNDS << +0.0*gamma, +0.0*gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
+ const int GRID_SIZE = 25;
- StaticEquilibrium solver(generatorsPerContact, SOLVER_LP_QPOASES);
+ StaticEquilibrium solver(mass, generatorsPerContact, SOLVER_LP_QPOASES);
MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
MatrixXX p = MatrixXX::Zero(4*N_CONTACTS,3); // contact points
MatrixXX N = MatrixXX::Zero(4*N_CONTACTS,3); // contact normals
- VectorX frictionCoefficients(N_CONTACTS);
+ VectorX frictionCoefficients(4*N_CONTACTS);
frictionCoefficients.fill(mu);
+ contact_pos << 0.122, 0.361, 0.071,
+ 0.243, 0.029, 0.112;
+ contact_rpy << 0.205, -0.005, -1.335,
+ -0.02 , 0.206, 0.506;
+
// Generate contact positions and orientations
bool collision;
- for(int i=0; i<N_CONTACTS; i++)
+ for(unsigned int i=0; i<N_CONTACTS; i++)
{
- while(true) // generate contact position
- {
- uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
- collision = false;
- for(int j=0; j<i-1; j++)
- if((contact_pos.row(i)-contact_pos.row(j)).norm() < MIN_FEET_DISTANCE)
- collision = true;
- if(collision==false)
- break;
- }
+// while(true) // generate contact position
+// {
+// uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
+// if(i==0)
+// break;
+// collision = false;
+// for(unsigned int j=0; j<i-1; j++)
+// if((contact_pos.row(i)-contact_pos.row(j)).norm() < MIN_FEET_DISTANCE)
+// collision = true;
+// if(collision==false)
+// break;
+// }
+
+// // generate contact orientation
+// uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
- // generate contact orientation
- uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
generate_rectangle_contacts(LX, LY, contact_pos.row(i), contact_rpy.row(i),
p.middleRows<4>(i*4), N.middleRows<4>(i*4));
+
+ printf("Contact surface %d position (%.3f,%.3f,%.3f) ", i, contact_pos(i,0), contact_pos(i,1), contact_pos(i,2));
+ printf("Orientation (%.3f,%.3f,%.3f)\n", contact_rpy(i,0), contact_rpy(i,1), contact_rpy(i,2));
}
- solver.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_PP);
+ for(int i=0; i<p.rows(); i++)
+ {
+ printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1), p(i,2));
+ printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
+ }
+ if(!solver.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_PP))
+ {
+ printf("Error while setting new contacts");
+ return -1;
+ }
RVector2 com_LB, com_UB;
com_LB(0) = p.col(0).minCoeff()-X_MARG;
com_UB(0) = p.col(0).maxCoeff()+X_MARG;
com_LB(1) = p.col(1).minCoeff()-Y_MARG;
com_UB(1) = p.col(1).maxCoeff()+Y_MARG;
- MatrixXX com(N_POINTS,2);
- for(int i=0; i<N_POINTS; i++)
+
+ MatrixXi contactPointCoord(4*N_CONTACTS,2);
+ VectorX minDistContactPoint = 1e10*VectorX::Ones(4*N_CONTACTS);
+
+ 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 equilibrium(GRID_SIZE, GRID_SIZE);
+ Vector2 com;
+ 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++)
{
- uniform(com_LB, com_UB, com.row(i));
- double robustness = solver.checkRobustEquilibrium(com.row(i), 0.0);
- printf("CoM position (%.3f, %.3f) robustness %.3f\n", com(i,0), com(i,1), robustness);
+ com(1) = y_range(GRID_SIZE-1-i);
+ for(unsigned int j=0; j<GRID_SIZE; j++)
+ {
+// uniform(com_LB, com_UB, com);
+ com(0) = x_range(j);
+ if(solver.checkRobustEquilibrium(com, 0.0))
+ {
+ equilibrium(i,j) = 1.0;
+ printf("1 ");
+ }
+ else
+ {
+ equilibrium(i,j) = 0.0;
+ printf("- ");
+ }
+
+ // look for contact point positions on grid
+ for(long k=0; k<4*N_CONTACTS; k++)
+ {
+ double dist = (p.block<1,2>(k,0) - com.transpose()).norm();
+ if(dist < minDistContactPoint(k))
+ {
+ minDistContactPoint(k) = dist;
+ contactPointCoord(k,0) = i;
+ contactPointCoord(k,1) = j;
+ }
+ }
+ }
+ printf("\n");
+ }
+
+ cout<<"Max dist between contact points and grid points "<<minDistContactPoint.maxCoeff()<<"\n";
+
+ cout<<"\nContact point position on the same grid\n";
+ bool contactPointDrawn;
+ for(unsigned int i=0; i<GRID_SIZE; i++)
+ {
+ for(unsigned int j=0; j<GRID_SIZE; j++)
+ {
+ contactPointDrawn = false;
+ for(long k=0; k<4*N_CONTACTS; k++)
+ {
+ if(contactPointCoord(k,0)==i && contactPointCoord(k,1)==j)
+ {
+ cout<<"X ";
+ contactPointDrawn = true;
+ break;
+ }
+ }
+ if(contactPointDrawn==false)
+ cout<<"- ";
+ }
+ printf("\n");
}
return ret;