From 38c091092ad2ddfe98db226f0370a40717d62ead Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@laas.fr>
Date: Thu, 9 Mar 2017 15:27:05 +0100
Subject: [PATCH] Eigen param binding operational. Added signature for ColMajor
matrices in setNewContacts
---
.../centroidal_dynamics.hh | 21 ++++++++++++++++---
include/centroidal-dynamics-lib/util.hh | 6 ++++++
python/centroidal_dynamics_python.cpp | 19 +++++++----------
src/centroidal_dynamics.cpp | 12 +++++++++--
test/test_static_equilibrium.cpp | 10 ++++++---
5 files changed, 48 insertions(+), 20 deletions(-)
diff --git a/include/centroidal-dynamics-lib/centroidal_dynamics.hh b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
index 1be7e6b..c17cd50 100644
--- a/include/centroidal-dynamics-lib/centroidal_dynamics.hh
+++ b/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.
diff --git a/include/centroidal-dynamics-lib/util.hh b/include/centroidal-dynamics-lib/util.hh
index 22a6dcb..40baee9 100644
--- a/include/centroidal-dynamics-lib/util.hh
+++ b/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.
*/
diff --git a/python/centroidal_dynamics_python.cpp b/python/centroidal_dynamics_python.cpp
index 9b8cb27..180b225 100644
--- a/python/centroidal_dynamics_python.cpp
+++ b/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)
;
}
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index 0fd3093..ff62aeb 100644
--- a/src/centroidal_dynamics.cpp
+++ b/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());
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index a12b032..3935089 100644
--- a/test/test_static_equilibrium.cpp
+++ b/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);
--
GitLab