diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5029d5d5595a3a061cbe345b897132fb1f2a873e..0e2347132bc9b539b0f16cd318e8335d89bd4641 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -31,7 +31,7 @@ SETUP_PROJECT()
string (REPLACE "-Werror" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
-OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" OFF)
+OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
# search for python
FINDPYTHON(2.7 EXACT REQUIRED)
diff --git a/include/centroidal-dynamics-lib/centroidal_dynamics.hh b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
index 8d437e9f8f2957fc7336154c1c69d6eab5c4c30a..1586f246f37168bf9a641d630683ca9efa4eee98 100644
--- a/include/centroidal-dynamics-lib/centroidal_dynamics.hh
+++ b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
@@ -29,6 +29,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium
public:
const double m_mass; /// mass of the system
const Vector3 m_gravity; /// gravity vector
+ /** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
+ Matrix6X m_G_centr;
private:
static bool m_is_cdd_initialized; /// true if cdd lib has been initialized, false otherwise
@@ -40,8 +42,6 @@ private:
unsigned int m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point
- /** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
- Matrix6X m_G_centr;
/** Inequality matrix and vector defining the gravito-inertial wrench cone H w <= h */
MatrixXX m_H;
@@ -222,6 +222,30 @@ public:
*/
LP_status checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max=0.0);
+
+ /**
+ * @brief Check whether the specified com position is in robust equilibrium.
+ * This amounts to solving the following feasibility LP:
+ * find b
+ * minimize 1
+ * subject to G b = D c + d
+ * b >= b0
+ * where:
+ * b are the coefficient of the contact force generators (f = G b)
+ * 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 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 3d center of mass position to test.
+ * @param acc The 3d acceleration of the CoM.
+ * @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_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max=0.0);
+
+
/**
* @brief Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
* This amounts to solving the following LP:
diff --git a/include/centroidal-dynamics-lib/util.hh b/include/centroidal-dynamics-lib/util.hh
index cf97755f8281e2fc73f282810225eceb5463f0d3..284c7c35a4a6ba63c251a03856dfd15980680501 100644
--- a/include/centroidal-dynamics-lib/util.hh
+++ b/include/centroidal-dynamics-lib/util.hh
@@ -7,6 +7,8 @@
#include <iostream>
#include <fstream>
+#include <cmath>
+#include <cassert>
#include <Eigen/Dense>
#include <Eigen/src/Core/util/Macros.h>
@@ -136,6 +138,61 @@ namespace centroidal_dynamics
std::string getDateAndTimeAsString();
+ /**
+ * Computes a binomal coefficient
+ * @return n!/((n–k)! k!).
+ */
+ value_type nchoosek(const int n, const int k);
+
+ template < typename DerivedV, typename DerivedU>
+ void doCombs(Eigen::Matrix<typename DerivedU::Scalar,1,Eigen::Dynamic>& running,
+ int& running_i, int& running_j, Eigen::PlainObjectBase<DerivedU> & U, const Eigen::MatrixBase<DerivedV> & V,
+ int offset, int k)
+ {
+ int N = (int)(V.size());
+ if(k==0)
+ {
+ U.row(running_i) = running;
+ running_i++;
+ return;
+ }
+ for (int i = offset; i <= N - k; ++i)
+ {
+ running(running_j) = V(i);
+ running_j++;
+ doCombs(running, running_i, running_j, U, V, i+1,k-1);
+ running_j--;
+ }
+ }
+
+ /**
+ * Computes a matrix C containing all possible combinations of the elements of vector v taken k at a time.
+ * Matrix C has k columns and n!/((n–k)! k!) rows, where n is length(v).
+ * @param V n-long vector of elements
+ * @param k size of sub-set to consider
+ * @param U result matrix
+ * @return nchoosek by k long matrix where each row is a unique k-size
+ * the first one, with all zeros.
+ */
+ template < typename DerivedV, typename DerivedU>
+ void nchoosek(
+ const Eigen::MatrixBase<DerivedV> & V,
+ const int k,
+ Eigen::PlainObjectBase<DerivedU> & U)
+ {
+ using namespace Eigen;
+ if(V.size() == 0)
+ {
+ U.resize(0,k);
+ return;
+ }
+ assert((V.cols() == 1 || V.rows() == 1) && "V must be a vector");
+ U.resize(nchoosek((int)(V.size()),k),k);
+ int running_i = 0;
+ int running_j = 0;
+ Matrix<typename DerivedU::Scalar,1,Dynamic> running(1,k);
+ doCombs(running, running_i, running_j, U, V,0,k);
+ }
} //namespace centroidal_dynamics
#endif //_CENTROIDAL_DYNAMICS_LIB_UTIL_HH
diff --git a/python/centroidal_dynamics_python.cpp b/python/centroidal_dynamics_python.cpp
index 82f1b37628d29b7b35ed3441a77fece7d1bf6a1d..82c17b50c1a5755d7e1d783d871396b1b294b8c7 100644
--- a/python/centroidal_dynamics_python.cpp
+++ b/python/centroidal_dynamics_python.cpp
@@ -26,6 +26,25 @@ boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const V
return boost::python::make_tuple(status, robustness);
}
+boost::python::tuple wrapCheckRobustEquilibrium(Equilibrium& self, const Vector3& com)
+{
+ bool robustness;
+ LP_status status = self.checkRobustEquilibrium(com, robustness);
+ return boost::python::make_tuple(status, robustness);
+}
+
+bool wrapSetNewContacts(Equilibrium& self, const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
+ const double frictionCoefficient, const EquilibriumAlgorithm alg)
+{
+ return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
+}
+
+bool wrapSetNewContactsFull(Equilibrium& self, const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
+ const double frictionCoefficient, const EquilibriumAlgorithm alg)
+{
+ return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
+}
+
boost::python::tuple wrapGetPolytopeInequalities(Equilibrium& self)
{
MatrixXX H;
@@ -86,17 +105,19 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
/** END enum types **/
- bool (Equilibrium::*setNewContacts)
- (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm) = &Equilibrium::setNewContacts;
+ //bool (Equilibrium::*setNewContacts)
+ // (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm, const int graspIndex, const double maxGraspForce) = &Equilibrium::setNewContacts;
class_<Equilibrium>("Equilibrium", init<std::string, double, unsigned int, optional <SolverLP, bool, const unsigned int, const bool> >())
.def("useWarmStart", &Equilibrium::useWarmStart)
.def("setUseWarmStart", &Equilibrium::setUseWarmStart)
.def("getName", &Equilibrium::getName)
.def("getAlgorithm", &Equilibrium::getAlgorithm)
- .def("setNewContacts", setNewContacts)
+ .def("setNewContacts", wrapSetNewContacts)
+ .def("setNewContacts", wrapSetNewContactsFull)
.def("computeEquilibriumRobustness", wrapComputeQuasiEquilibriumRobustness)
.def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
+ .def("checkRobustEquilibrium", wrapCheckRobustEquilibrium)
.def("getPolytopeInequalities", wrapGetPolytopeInequalities)
;
}
diff --git a/python/test/binding_tests.py b/python/test/binding_tests.py
index 1110c0f8c51ae86ff7a3429c414247ad18e12e53..683bad2b29b0383435de8c18063344b9c22aaa47 100644
--- a/python/test/binding_tests.py
+++ b/python/test/binding_tests.py
@@ -45,3 +45,23 @@ assert (robustness < 0), "first test should NOT be in equilibrirum"
eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
H,h = eq.getPolytopeInequalities()
+#~ c= asmatrix(array([0.,0.,1.])).T
+status, stable = eq.checkRobustEquilibrium(c)
+assert (status == LP_STATUS_OPTIMAL), "checkRobustEquilibrium should not fail"
+assert (stable), "lat test should be in equilibrirum"
+
+
+from numpy import array, vstack, zeros, sqrt, cross, matrix, asmatrix
+from numpy.linalg import norm
+import numpy as np
+
+def compute_w(c, ddc, dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81])):
+ w1 = m * (ddc - g_vec)
+ return array(w1.tolist() + (cross(c, w1) + dL).tolist())
+
+
+def is_stable(H,c=array([0.,0.,1.]), ddc=array([0.,0.,0.]), dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81]), robustness = 0.):
+ w = compute_w(c, ddc, dL, m, g_vec)
+ return (H.dot(-w)<=-robustness).all()
+
+assert(is_stable(H))
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index ace1b74a5faeca3f2dedfa0a13a19faf926c5299..b8f0cd798f9b8ea9aabf85afa0d9671fba7422c0 100644
--- a/src/centroidal_dynamics.cpp
+++ b/src/centroidal_dynamics.cpp
@@ -142,6 +142,7 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
// project generators in 6d centroidal space
m_G_centr.block(0,cg*i,6,cg) = A * G;
}
+
// Compute the coefficient to convert b0 to e_max
Vector3 f0 = Vector3::Zero();
for(int j=0; j<cg; j++)
@@ -173,7 +174,6 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
const double frictionCoefficient, const EquilibriumAlgorithm alg)
{
assert(contactPoints.rows()==contactNormals.rows());
-
if(alg==EQUILIBRIUM_ALGORITHM_IP)
{
SEND_ERROR_MSG("Algorithm IP not implemented yet");
@@ -190,7 +190,7 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
// Lists of contact generators (3 X generatorsPerContact)
m_G_centr.resize(6,contactPoints.rows()*m_generatorsPerContact);
- if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient,false))
+ if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient, false))
{
return false;
}
@@ -351,13 +351,22 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
return lpStatus_dual;
}
- SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
+ SEND_ERROR_MSG("computeEquilibriumRobustness is not implemented for the specified algorithm");
return LP_STATUS_ERROR;
}
+LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max){
+ checkRobustEquilibrium(com,zero_acc,equilibrium,e_max);
+}
-LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
+LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max)
{
+ // Take the acceleration in account in D and d :
+ m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
+ m_d.head<3>()= m_mass * (m_gravity - acc);
+ m_HD = m_H * m_D;
+ m_Hd = m_H * m_d;
+
if(m_G_centr.cols()==0)
{
equilibrium=false;
@@ -545,14 +554,19 @@ LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vecto
SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
return LP_STATUS_ERROR;
}
-
bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
{
- int n = (int)(v.rows());
- int m = (int)(v.cols());
+ // todo: for the moment using ad hoc upper bound = 500 N
+ int n = (int)v.rows();
+ int m = (int)v.cols();
+ if (n>m)
+ {
+ SEND_ERROR_MSG("V has more lines that columns, this case is not handled!");
+ return false;
+ }
// getProfiler().start("eigen_to_cdd");
- dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
+ dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
// getProfiler().stop("eigen_to_cdd");
dd_ErrorType error = dd_NoError;
@@ -611,7 +625,6 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
return false;
}
-
return true;
}
diff --git a/src/util.cpp b/src/util.cpp
index 85d069d31a8d1acecf4a0b48ebcb7866966e70fd..07989162e696d999d42af1b639d8a47c01734379 100644
--- a/src/util.cpp
+++ b/src/util.cpp
@@ -24,11 +24,9 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize
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);
@@ -50,7 +48,6 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize
set_free(impl_linset);
free(newpos);
}
-
return M;
}
@@ -165,6 +162,51 @@ std::string getDateAndTimeAsString()
strftime(buffer,80,"%Y%m%d_%I%M%S",timeinfo);
return std::string(buffer);
}
+/*
+int fact(const int n)
+{
+ assert(n>=0);
+ int res = 1;
+ for (int i=2 ; i <= n ; ++i)
+ res *= i;
+ return res;
+}
+
+int choosenk(const int n, const int k)
+{
+ return fact(n) / (fact(k) * fact(n - k));
+}*/
+
+/* is this faster ?
+value_type choosenk(const int n, const int k)
+{
+ if(k>n/2)
+ return nchoosek(n,n-k);
+ else if(k==1)
+ return n;
+ else
+ {
+ double c = 1;
+ for(int i = 1;i<=k;i++)
+ c *= (((double)n-k+i)/((double)i));
+ return std::round(c);
+ }
+}*/
+
+value_type nchoosek(const int n, const int k)
+{
+ if(k>n/2)
+ return nchoosek(n,n-k);
+ else if(k==1)
+ return n;
+ else
+ {
+ value_type c = 1;
+ for(int i = 1;i<=k;i++)
+ c *= (((value_type)n-k+i)/((value_type)i));
+ return round(c);
+ }
+}
} //namespace centroidal_dynamics
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 18bb1a7e5eab44002fa26e64f8fac3999f3dfd8b..b5c3176f2e48c058c3ec02589310c4a71e498fd4 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -42,4 +42,5 @@ endif ( MSVC )
ADD_TESTCASE(test_static_equilibrium FALSE)
ADD_TESTCASE(test_LP_solvers FALSE)
+PKG_CONFIG_USE_DEPENDENCY(test_LP_solvers qpOASES)
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index 6a90d8eacc3c5bf698db48f6872c4299af15f08c..9a575de886ac95cb3ffccf2e603ade5d05b6abdc 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -343,7 +343,7 @@ void test_small_LP()
model.addRow(3, row2Index, row2Value,
1.0, 1.0);
- int n = model.getNumCols();
+ int n = model.getdimVarXs();
int m = model.getNumRows();
cout<<"Problem has "<<n<<" variables and "<<m<<" constraints.\n";