add ContactModel::generatorMatrix method

include/multicontact-api/scenario/contact-model.hpp

@@ -9,6 +9,7 @@
 #include "multicontact-api/serialization/eigen-matrix.hpp"
 #include <iostream>
 #include <Eigen/Dense>
+#include <pinocchio/spatial/skew.hpp>
 
 namespace multicontact_api {
 namespace scenario {
@@ -18,7 +19,9 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   typedef _Scalar Scalar;
+  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
   typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic> Matrix3X;
+  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic> Matrix6X;
 
   /// \brief Default constructor.
   ContactModelTpl()
@@ -87,6 +90,21 @@ struct ContactModelTpl : public serialization::Serializable<ContactModelTpl<_Sca
     m_num_contact_points = positions.cols();
   }
 
+  /**
+   * @brief generatorMatrix Return a 6x(num_contact_points*3) matrix
+   * containing the generator used to compute contact forces.
+   * @return
+   */
+  Matrix6X generatorMatrix() const {
+    Matrix6X gen = Matrix6X::Zero(6, m_num_contact_points * 3);
+    for(size_t i=0; i<m_num_contact_points; i++)
+    {
+      gen.block(0, i*3, 3, 3) = Matrix3::Identity();
+      gen.block(3, i*3, 3, 3) = pinocchio::skew(m_contact_points_positions.col(i));
+    }
+    return gen;
+  }
+
   /// \brief Friction coefficient.
   Scalar m_mu;
   /// \brief ZMP radius.

unittest/scenario.cpp

@@ -34,6 +34,7 @@ using curves::t_pointX_t;
 using namespace multicontact_api::scenario;
 typedef ContactSequence::ContactPhaseVector ContactPhaseVector;
 typedef ContactModel::Matrix3X Matrix3X;
+typedef ContactModel::Matrix6X Matrix6X;
 
 typedef pinocchio::SE3Tpl<double> SE3;
 
@@ -384,6 +385,9 @@ BOOST_AUTO_TEST_CASE(contact_model_points_positions) {
   BOOST_CHECK_EQUAL(mp.num_contact_points(), 6);
   BOOST_CHECK_EQUAL(mp.contact_points_positions().cols(), 6);
   BOOST_CHECK(mp.contact_points_positions().isApprox(positions));
+  Matrix6X generators = mp.generatorMatrix();
+  BOOST_CHECK_EQUAL(generators.rows(), 6);
+  BOOST_CHECK_EQUAL(generators.cols(), 6*3);
 
   mp.num_contact_points(2);
   BOOST_CHECK_EQUAL(mp.num_contact_points(), 2);