Commit 38c09109 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

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

parent 06bdb35a
......@@ -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.
......
......@@ -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.
*/
......
......@@ -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)
;
}
......
......@@ -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());
......
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment