Commit affd699a authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

add ContactModel::generatorMatrix method

parent cbe03c98
......@@ -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.
......
......@@ -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);
......
Markdown is supported
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