Eigen param binding operational. Added signature for ColMajor matrices in setNewContacts

include/centroidal-dynamics-lib/centroidal_dynamics.hh

@@ -120,15 +120,30 @@ public:
   /**
    * @brief Specify a new set of contacts.
    * All 3d vectors are expressed in a reference frame having the z axis aligned with gravity.
-   * In other words the gravity vecotr is (0, 0, -9.81).
+   * In other words the gravity vecotr is (0, 0, -9.81). This method considers row-major
+   * matrices as input.
    * @param contactPoints List of N 3d contact points as an Nx3 matrix.
    * @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
    * @param frictionCoefficient The contact friction coefficient.
    * @param alg Algorithm to use for testing equilibrium.
    * @return True if the operation succeeded, false otherwise.
    */
-  bool setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
-                      double frictionCoefficient, EquilibriumAlgorithm alg);
+  bool setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
+                      const double frictionCoefficient, const EquilibriumAlgorithm alg);
+
+  /**
+   * @brief Specify a new set of contacts.
+   * All 3d vectors are expressed in a reference frame having the z axis aligned with gravity.
+   * In other words the gravity vecotr is (0, 0, -9.81). This method considers column major
+   * matrices as input, and converts them into rowmajor matrices for internal use with the solvers.
+   * @param contactPoints List of N 3d contact points as an Nx3 matrix.
+   * @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
+   * @param frictionCoefficient The contact friction coefficient.
+   * @param alg Algorithm to use for testing equilibrium.
+   * @return True if the operation succeeded, false otherwise.
+   */
+  bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor&  contactNormals,
+                      const double frictionCoefficient, const EquilibriumAlgorithm alg);
 
   /**
    * @brief Compute a measure of the robustness of the equilibrium of the specified com position.

include/centroidal-dynamics-lib/util.hh

@@ -64,6 +64,12 @@ namespace centroidal_dynamics
   typedef const Eigen::Ref<const Matrix6X>    & Cref_matrix6X;
   typedef const Eigen::Ref<const MatrixXX>    & Cref_matrixXX;
 
+  /**Column major definitions for compatibility with classical eigen use**/
+  typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::ColMajor>              MatrixX3ColMajor;
+  typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixXXColMajor;
+  typedef const Eigen::Ref<const MatrixX3ColMajor>  & Cref_matrixX3ColMajor;
+  typedef Eigen::Ref<MatrixXXColMajor>              &  ref_matrixXXColMajor;
+
   /**
    * Write the specified matrix to a binary file with the specified name.
    */

python/centroidal_dynamics_python.cpp

@@ -3,7 +3,6 @@
 
 #include <eigenpy/memory.hpp>
 #include <eigenpy/eigenpy.hpp>
-#include <eigenpy/geometry.hpp>
 
 #include <boost/python.hpp>
 
@@ -13,22 +12,15 @@ namespace centroidal_dynamics
 {
 using namespace boost::python;
 
-bool wrapSetNewContacts(Equilibrium& self, const MatrixXX& contactPoints, const MatrixXX& contactNormals) //,
-                    //double frictionCoefficient, EquilibriumAlgorithm alg)
-{
-    return self.setNewContacts(contactPoints, contactNormals, 0.3, EQUILIBRIUM_ALGORITHM_LP);
-}
-
 
 BOOST_PYTHON_MODULE(centroidal_dynamics)
 {
     /** BEGIN eigenpy init**/
     eigenpy::enableEigenPy();
 
-    eigenpy::enableEigenPySpecific<MatrixX3,MatrixX3>();
-    eigenpy::enableEigenPySpecific<MatrixXX,MatrixXX>();
-    eigenpy::exposeAngleAxis();
-    eigenpy::exposeQuaternion();
+    eigenpy::enableEigenPySpecific<MatrixX3ColMajor,MatrixX3ColMajor>();
+    /*eigenpy::exposeAngleAxis();
+    eigenpy::exposeQuaternion();*/
 
     /** END eigenpy init**/
 
@@ -54,13 +46,16 @@ BOOST_PYTHON_MODULE(centroidal_dynamics)
             .value("EQUILIBRIUM_ALGORITHM_DIP", EQUILIBRIUM_ALGORITHM_DIP)
             .export_values();
 
+    bool (Equilibrium::*setNewContacts)
+            (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm) = &Equilibrium::setNewContacts;
+
     /** END enum types **/
     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", &wrapSetNewContacts)
+            .def("setNewContacts", setNewContacts)
     ;
 }
 

src/centroidal_dynamics.cpp

@@ -129,8 +129,16 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
     return true;
 }
 
-bool Equilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
-                                       double frictionCoefficient, EquilibriumAlgorithm alg)
+bool Equilibrium::setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
+                                       const double frictionCoefficient, const EquilibriumAlgorithm alg)
+{
+    MatrixX3 _contactPoints  = contactPoints;
+    MatrixX3 _contactNormals = contactNormals;
+    return setNewContacts(_contactPoints,_contactNormals, frictionCoefficient, alg);
+}
+
+bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
+                                 const double frictionCoefficient, const EquilibriumAlgorithm alg)
 {
   assert(contactPoints.rows()==contactNormals.rows());
 

test/test_static_equilibrium.cpp

@@ -308,7 +308,7 @@ void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE, doub
                       RVector3 &CONTACT_POINT_UPPER_BOUNDS,
                       RVector3 &RPY_LOWER_BOUNDS,
                       RVector3 &RPY_UPPER_BOUNDS,
-                      MatrixXX& p, MatrixXX& N)
+                      MatrixX3& p, MatrixX3& N)
 {
   MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
   MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
@@ -381,12 +381,16 @@ void testWithLoadedData()
     return;
   }
 
+  // this is a test to ensure that a matrixXX can be cast into a MatrixX3
+  const MatrixX3& cp = contactPoints;
+  const MatrixX3& cn = contactNormals;
   Equilibrium* solvers[N_SOLVERS];
   double robustness[N_SOLVERS];
   for(int s=0; s<N_SOLVERS; s++)
   {
     solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact, SOLVER_LP_QPOASES);
-    if(!solvers[s]->setNewContacts(contactPoints, contactNormals, mu, algorithms[s]))
+
+    if(!solvers[s]->setNewContacts(cp, cn, mu, algorithms[s]))
     {
       SEND_ERROR_MSG("Error while setting new contacts for solver "+solvers[s]->getName());
       continue;
@@ -466,7 +470,7 @@ int main()
   for(int s=0; s<N_SOLVERS; s++)
     solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact, lp_solver_types[s]);
 
-  MatrixXX p, N;
+  MatrixX3 p, N;
   RVector2 com_LB, com_UB;
   VectorX x_range(GRID_SIZE), y_range(GRID_SIZE);
   MatrixXX comPositions(GRID_SIZE*GRID_SIZE, 3);