Merge pull request #4 from stonneau/rbprm_v_1

Rbprm v 1

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)

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:

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

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)
     ;
 }

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))

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;
 }
 

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
 

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)
 

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";