Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/constraint.cpp	Lines:	153	153	100.0 %
Date:	2024-04-26 13:14:21	Branches:	471	929	50.7 %


//
// Copyright (c) 2015-2019 CNRS INRIA
//

#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/force-set.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include "utils/macros.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE (test_ForceSet)
{
  using namespace pinocchio;

  SE3 amb = SE3::Random();
  SE3 bmc = SE3::Random();
  SE3 amc = amb*bmc;

  ForceSet F(12);
  ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero());
  F.block(10,2) = F2;
  BOOST_CHECK_EQUAL(F.matrix().col(10).norm() , 0.0 );
  BOOST_CHECK(std::isnan(F.matrix()(0,9)));

  ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
  ForceSet F4 = amb.act(F3);
  SE3::Matrix6 aXb= amb;
  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix(), 1e-12));


  ForceSet bF = bmc.act(F3);
  ForceSet aF = amb.act(bF);
  ForceSet aF2 = amc.act(F3);
  BOOST_CHECK(aF.matrix().isApprox(aF2.matrix(), 1e-12));

  ForceSet F36 = amb.act(F3.block(3,6));
  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix(), 1e-12));


  ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6));
  BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36full.matrix().block(0,3,6,6),
                                                            1e-12));
}

BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
{
  using namespace pinocchio;

  Inertia Y = Inertia::Random();
  JointDataRX::Constraint_t S;

  ForceSet F1(1); F1.block(0,1) = Y*S;
  BOOST_CHECK(F1.matrix().isApprox(Y.matrix().col(3), 1e-12));

  ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
  Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3).matrix();
  BOOST_CHECK(StF2.isApprox(S.matrix().transpose()*F2.matrix().block(0,5,6,3)
                            , 1e-12));
}

template<typename JointModel>
void test_jmodel_nq_against_nq_ref(const JointModelBase<JointModel> & jmodel,
                                   const int & nq_ref)
{
  BOOST_CHECK(jmodel.nq() == nq_ref);
}

template<typename JointModel>
void test_jmodel_nq_against_nq_ref(const JointModelMimic<JointModel> & jmodel,
                                   const int & nq_ref)
{
  BOOST_CHECK(jmodel.jmodel().nq() == nq_ref);
}

template<typename JointModel, typename ConstraintDerived>
void test_nv_against_jmodel(const JointModelBase<JointModel> & jmodel,
                            const ConstraintBase<ConstraintDerived> & constraint)
{
  BOOST_CHECK(constraint.nv() == jmodel.nv());
}

template<typename JointModel, typename ConstraintDerived>
void test_nv_against_jmodel(const JointModelMimic<JointModel> & jmodel,
                            const ConstraintBase<ConstraintDerived> & constraint)
{
  BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv());
}

template<class JointModel>
struct buildModel
{
  static Model run(const JointModelBase<JointModel> & jmodel)
  {
    Model model;
    model.addJoint(0,jmodel,SE3::Identity(),"joint");

    return model;
  }
};

template<class JointModel>
struct buildModel< JointModelMimic<JointModel> >
{
  typedef JointModelMimic<JointModel> JointModel_;

  static Model run(const JointModel_ & jmodel)
  {
    Model model;
    model.addJoint(0,jmodel.jmodel(),SE3::Identity(),"joint");
    model.addJoint(0,jmodel,SE3::Identity(),"joint_mimic");

    return model;
  }
};

template<typename JointModel>
void test_constraint_operations(const JointModelBase<JointModel> & jmodel)
{
  typedef typename traits<JointModel>::JointDerived Joint;
  typedef typename traits<Joint>::Constraint_t ConstraintType;
  typedef typename traits<Joint>::JointDataDerived JointData;
  typedef Eigen::Matrix<typename JointModel::Scalar,6,Eigen::Dynamic> Matrix6x;

  JointData jdata = jmodel.createData();
  typedef typename JointModel::ConfigVector_t ConfigVector_t;
  ConfigVector_t q;

  // We need to use a model here in order to call the randomConfiguration to init q.
  Model model = buildModel<JointModel>::run(jmodel.derived());

  test_jmodel_nq_against_nq_ref(jmodel.derived(),model.nq);

  q = randomConfiguration(model,
                          ConfigVector_t::Constant(model.nq,-1.),
                          ConfigVector_t::Constant(model.nq, 1.));

  // By calling jmodel.calc, we then have jdata.S which is initialized with non NaN quantities
  jmodel.calc(jdata,q);

  ConstraintType constraint(jdata.S);

  test_nv_against_jmodel(jmodel.derived(),constraint);
  BOOST_CHECK(constraint.cols() == constraint.nv());
  BOOST_CHECK(constraint.rows() == 6);

  typedef typename JointModel::TangentVector_t TangentVector_t;
  TangentVector_t v = TangentVector_t::Random(constraint.nv());

  typename ConstraintType::DenseBase constraint_mat = constraint.matrix();
  Motion m = constraint * v;
  Motion m_ref = Motion(constraint_mat * v);

  BOOST_CHECK(m.isApprox(m_ref));

  // Test SE3 action
  {
    SE3 M = SE3::Random();
    typename ConstraintType::DenseBase S = M.act(constraint);
    typename ConstraintType::DenseBase S_ref(6,constraint.nv());

    for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
    {
      typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
      MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));

      m_out = M.act(m_in);
    }

    BOOST_CHECK(S.isApprox(S_ref));
  }

  // Test SE3 action inverse
  {
    SE3 M = SE3::Random();
    typename ConstraintType::DenseBase S = M.actInv(constraint);
    typename ConstraintType::DenseBase S_ref(6,constraint.nv());

    for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
    {
      typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
      MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));

      m_out = M.actInv(m_in);
    }

    BOOST_CHECK(S.isApprox(S_ref));
  }

  // Test SE3 action and SE3 action inverse
  {
    const SE3 M = SE3::Random();
    const SE3 Minv = M.inverse();

    typename ConstraintType::DenseBase S1_vice = M.actInv(constraint);
    typename ConstraintType::DenseBase S2_vice = Minv.act(constraint);

    BOOST_CHECK(S1_vice.isApprox(S2_vice));

    typename ConstraintType::DenseBase S1_versa = M.act(constraint);
    typename ConstraintType::DenseBase S2_versa = Minv.actInv(constraint);

    BOOST_CHECK(S1_versa.isApprox(S2_versa));

  }

  // Test Motion action
  {
    Motion v = Motion::Random();

    typename ConstraintType::DenseBase S = v.cross(constraint);
    typename ConstraintType::DenseBase S_ref(6,constraint.nv());

    for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
    {
      typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
      MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));

      m_out = v.cross(m_in);
    }
    BOOST_CHECK(S.isApprox(S_ref));
  }

  // Test transpose operations
  {
    const Eigen::DenseIndex dim = 20;
    const Matrix6x Fin = Matrix6x::Random(6,dim);
    Eigen::MatrixXd Fout = constraint.transpose() * Fin;
    Eigen::MatrixXd Fout_ref = constraint_mat.transpose() * Fin;
    BOOST_CHECK(Fout.isApprox(Fout_ref));

    Force force_in(Force::Random());
    Eigen::MatrixXd Stf = (constraint.transpose() * force_in);
    Eigen::MatrixXd Stf_ref = constraint_mat.transpose() * force_in.toVector();
    BOOST_CHECK(Stf_ref.isApprox(Stf));
  }

  // CRBA operations
  {
    const Inertia Y = Inertia::Random();
    Eigen::MatrixXd YS = Y * constraint;
    Eigen::MatrixXd YS_ref = Y.matrix() * constraint_mat;
    BOOST_CHECK(YS.isApprox(YS_ref));
  }

  // ABA operations
  {
    const Inertia Y = Inertia::Random();
    const Inertia::Matrix6 Y_mat = Y.matrix();
    Eigen::MatrixXd YS = Y_mat * constraint;
    Eigen::MatrixXd YS_ref = Y_mat * constraint_mat;
    BOOST_CHECK(YS.isApprox(YS_ref));
  }

}

template<typename JointModel_> struct init;

template<typename JointModel_>
struct init
{
  static JointModel_ run()
  {
    JointModel_ jmodel;
    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;

  static JointModel run()
  {
    typedef typename JointModel::Vector3 Vector3;
    JointModel jmodel(Vector3::Random().normalized());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    JointModel jmodel((JointModelRX()));

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,2> JointModelRZ;

    JointModel jmodel(JointModelRX(),SE3::Random());
    jmodel.addJoint(JointModelRY(),SE3::Random());
    jmodel.addJoint(JointModelRZ(),SE3::Random());

    jmodel.setIndexes(0,0,0);

    return jmodel;
  }
};

template<typename JointModel_>
struct init<pinocchio::JointModelMimic<JointModel_> >
{
  typedef pinocchio::JointModelMimic<JointModel_> JointModel;

  static JointModel run()
  {
    JointModel_ jmodel_ref = init<JointModel_>::run();

    JointModel jmodel(jmodel_ref,1.,0.);
    jmodel.setIndexes(0,0,0);

    return jmodel;
  }
};

struct TestJointConstraint
{

  template <typename JointModel>
  void operator()(const JointModelBase<JointModel> &) const
  {
    JointModel jmodel = init<JointModel>::run();
    jmodel.setIndexes(0,0,0);

    test_constraint_operations(jmodel);
  }

};

BOOST_AUTO_TEST_CASE(test_joint_constraint_operations)
{
  typedef JointCollectionDefault::JointModelVariant Variant;
  boost::mpl::for_each<Variant::types>(TestJointConstraint());
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2019 CNRS INRIA
3			//
4
5			#include "pinocchio/spatial/se3.hpp"
6			#include "pinocchio/spatial/inertia.hpp"
7			#include "pinocchio/multibody/force-set.hpp"
8			#include "pinocchio/multibody/model.hpp"
9			#include "pinocchio/algorithm/joint-configuration.hpp"
10
11			#include "utils/macros.hpp"
12
13			#include <iostream>
14
15			#include <boost/test/unit_test.hpp>
16			#include <boost/utility/binary.hpp>
17
18			using namespace pinocchio;
19
20			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
21
22	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE (test_ForceSet)
23			{
24			using namespace pinocchio;
25
26	✓✗	2	SE3 amb = SE3::Random();
27	✓✗	2	SE3 bmc = SE3::Random();
28	✓✗	2	SE3 amc = amb*bmc;
29
30	✓✗	4	ForceSet F(12);
31	✓✗✓✗ ✓✗✓✗ ✓✗	6	ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero());
32	✓✗✓✗	2	F.block(10,2) = F2;
33	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK_EQUAL(F.matrix().col(10).norm() , 0.0 );
34	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(std::isnan(F.matrix()(0,9)));
35
36	✓✗✓✗ ✓✗✓✗ ✓✗	6	ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
37	✓✗	4	ForceSet F4 = amb.act(F3);
38	✓✗	2	SE3::Matrix6 aXb= amb;
39	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix(), 1e-12));
40
41
42	✓✗	4	ForceSet bF = bmc.act(F3);
43	✓✗	4	ForceSet aF = amb.act(bF);
44	✓✗	4	ForceSet aF2 = amc.act(F3);
45	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(aF.matrix().isApprox(aF2.matrix(), 1e-12));
46
47	✓✗✓✗	4	ForceSet F36 = amb.act(F3.block(3,6));
48	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix(), 1e-12));
49
50
51	✓✗✓✗ ✓✗✓✗ ✓✗	4	ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6));
52	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36full.matrix().block(0,3,6,6),
53			1e-12));
54		2	}
55
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_ConstraintRX )
57			{
58			using namespace pinocchio;
59
60	✓✗	2	Inertia Y = Inertia::Random();
61		2	JointDataRX::Constraint_t S;
62
63	✓✗✓✗ ✓✗✓✗	4	ForceSet F1(1); F1.block(0,1) = Y*S;
64	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(F1.matrix().isApprox(Y.matrix().col(3), 1e-12));
65
66	✓✗✓✗ ✓✗✓✗ ✓✗	6	ForceSet F2( Eigen::Matrix<double,3,9>::Random(),Eigen::Matrix<double,3,9>::Random() );
67	✓✗✓✗ ✓✗✓✗ ✓✗	4	Eigen::MatrixXd StF2 = S.transpose()*F2.block(5,3).matrix();
68	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(StF2.isApprox(S.matrix().transpose()*F2.matrix().block(0,5,6,3)
69			, 1e-12));
70		2	}
71
72			template<typename JointModel>
73		36	void test_jmodel_nq_against_nq_ref(const JointModelBase<JointModel> & jmodel,
74			const int & nq_ref)
75			{
76	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	36	BOOST_CHECK(jmodel.nq() == nq_ref);
77		36	}
78
79			template<typename JointModel>
80		6	void test_jmodel_nq_against_nq_ref(const JointModelMimic<JointModel> & jmodel,
81			const int & nq_ref)
82			{
83	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	6	BOOST_CHECK(jmodel.jmodel().nq() == nq_ref);
84		6	}
85
86			template<typename JointModel, typename ConstraintDerived>
87		36	void test_nv_against_jmodel(const JointModelBase<JointModel> & jmodel,
88			const ConstraintBase<ConstraintDerived> & constraint)
89			{
90	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	36	BOOST_CHECK(constraint.nv() == jmodel.nv());
91		36	}
92
93			template<typename JointModel, typename ConstraintDerived>
94		6	void test_nv_against_jmodel(const JointModelMimic<JointModel> & jmodel,
95			const ConstraintBase<ConstraintDerived> & constraint)
96			{
97	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	6	BOOST_CHECK(constraint.nv() == jmodel.jmodel().nv());
98		6	}
99
100			template<class JointModel>
101			struct buildModel
102			{
103		36	static Model run(const JointModelBase<JointModel> & jmodel)
104			{
105		36	Model model;
106	✓✗✓✗ ✓✗✓✗	36	model.addJoint(0,jmodel,SE3::Identity(),"joint");
107
108		36	return model;
109			}
110			};
111
112			template<class JointModel>
113			struct buildModel< JointModelMimic<JointModel> >
114			{
115			typedef JointModelMimic<JointModel> JointModel_;
116
117		6	static Model run(const JointModel_ & jmodel)
118			{
119		6	Model model;
120	✓✗✓✗ ✓✗✓✗	6	model.addJoint(0,jmodel.jmodel(),SE3::Identity(),"joint");
121	✓✗✓✗ ✓✗✓✗	6	model.addJoint(0,jmodel,SE3::Identity(),"joint_mimic");
122
123		6	return model;
124			}
125			};
126
127			template<typename JointModel>
128		42	void test_constraint_operations(const JointModelBase<JointModel> & jmodel)
129			{
130			typedef typename traits<JointModel>::JointDerived Joint;
131			typedef typename traits<Joint>::Constraint_t ConstraintType;
132			typedef typename traits<Joint>::JointDataDerived JointData;
133			typedef Eigen::Matrix<typename JointModel::Scalar,6,Eigen::Dynamic> Matrix6x;
134
135	✓✗	44	JointData jdata = jmodel.createData();
136			typedef typename JointModel::ConfigVector_t ConfigVector_t;
137	✓✗	44	ConfigVector_t q;
138
139			// We need to use a model here in order to call the randomConfiguration to init q.
140	✓✗	84	Model model = buildModel<JointModel>::run(jmodel.derived());
141
142	✓✗	42	test_jmodel_nq_against_nq_ref(jmodel.derived(),model.nq);
143
144	✓✗✓✗	42	q = randomConfiguration(model,
145	✓✗	42	ConfigVector_t::Constant(model.nq,-1.),
146	✓✗	42	ConfigVector_t::Constant(model.nq, 1.));
147
148			// By calling jmodel.calc, we then have jdata.S which is initialized with non NaN quantities
149	✓✗	42	jmodel.calc(jdata,q);
150
151	✓✗	18	ConstraintType constraint(jdata.S);
152
153	✓✗	42	test_nv_against_jmodel(jmodel.derived(),constraint);
154	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	42	BOOST_CHECK(constraint.cols() == constraint.nv());
155	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	42	BOOST_CHECK(constraint.rows() == 6);
156
157			typedef typename JointModel::TangentVector_t TangentVector_t;
158	✓✗✓✗ ✓✗	44	TangentVector_t v = TangentVector_t::Random(constraint.nv());
159
160	✓✗✓✗	44	typename ConstraintType::DenseBase constraint_mat = constraint.matrix();
161	✓✗✓✗	42	Motion m = constraint * v;
162	✓✗✓✗	42	Motion m_ref = Motion(constraint_mat * v);
163
164	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(m.isApprox(m_ref));
165
166			// Test SE3 action
167			{
168	✓✗	42	SE3 M = SE3::Random();
169	✓✗	44	typename ConstraintType::DenseBase S = M.act(constraint);
170	✓✗✓✗	44	typename ConstraintType::DenseBase S_ref(6,constraint.nv());
171
172	✓✗✓✓	114	for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
173			{
174			typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
175	✓✗✓✗ ✓✗✓✗	72	MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
176
177	✓✗✓✗	72	m_out = M.act(m_in);
178			}
179
180	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(S.isApprox(S_ref));
181			}
182
183			// Test SE3 action inverse
184			{
185	✓✗	42	SE3 M = SE3::Random();
186	✓✗	44	typename ConstraintType::DenseBase S = M.actInv(constraint);
187	✓✗✓✗	44	typename ConstraintType::DenseBase S_ref(6,constraint.nv());
188
189	✓✗✓✓	114	for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
190			{
191			typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
192	✓✗✓✗ ✓✗✓✗	72	MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
193
194	✓✗✓✗	72	m_out = M.actInv(m_in);
195			}
196
197	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(S.isApprox(S_ref));
198			}
199
200			// Test SE3 action and SE3 action inverse
201			{
202	✓✗	42	const SE3 M = SE3::Random();
203	✓✗	42	const SE3 Minv = M.inverse();
204
205	✓✗	44	typename ConstraintType::DenseBase S1_vice = M.actInv(constraint);
206	✓✗	44	typename ConstraintType::DenseBase S2_vice = Minv.act(constraint);
207
208	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(S1_vice.isApprox(S2_vice));
209
210	✓✗	44	typename ConstraintType::DenseBase S1_versa = M.act(constraint);
211	✓✗	44	typename ConstraintType::DenseBase S2_versa = Minv.actInv(constraint);
212
213	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(S1_versa.isApprox(S2_versa));
214
215			}
216
217			// Test Motion action
218			{
219	✓✗	42	Motion v = Motion::Random();
220
221	✓✗	44	typename ConstraintType::DenseBase S = v.cross(constraint);
222	✓✗✓✗	44	typename ConstraintType::DenseBase S_ref(6,constraint.nv());
223
224	✓✗✓✓	114	for(Eigen::DenseIndex k = 0; k < constraint.nv(); ++k)
225			{
226			typedef typename ConstraintType::DenseBase::ColXpr Vector6Like;
227	✓✗✓✗ ✓✗✓✗	72	MotionRef<Vector6Like> m_in(constraint_mat.col(k)), m_out(S_ref.col(k));
228
229	✓✗✓✗	72	m_out = v.cross(m_in);
230			}
231	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(S.isApprox(S_ref));
232			}
233
234			// Test transpose operations
235			{
236		42	const Eigen::DenseIndex dim = 20;
237	✓✗✓✗	84	const Matrix6x Fin = Matrix6x::Random(6,dim);
238	✓✓✓✓ ✓✗✓✗	84	Eigen::MatrixXd Fout = constraint.transpose() * Fin;
239	✓✗✓✗ ✓✗	84	Eigen::MatrixXd Fout_ref = constraint_mat.transpose() * Fin;
240	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(Fout.isApprox(Fout_ref));
241
242	✓✗	42	Force force_in(Force::Random());
243	✓✓✗✓ ✓✗✓✗	84	Eigen::MatrixXd Stf = (constraint.transpose() * force_in);
244	✓✗✓✗ ✓✗✓✗	84	Eigen::MatrixXd Stf_ref = constraint_mat.transpose() * force_in.toVector();
245	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(Stf_ref.isApprox(Stf));
246			}
247
248			// CRBA operations
249			{
250	✓✗	42	const Inertia Y = Inertia::Random();
251	✓✗✓✗	84	Eigen::MatrixXd YS = Y * constraint;
252	✓✗✓✗ ✓✗	84	Eigen::MatrixXd YS_ref = Y.matrix() * constraint_mat;
253	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(YS.isApprox(YS_ref));
254			}
255
256			// ABA operations
257			{
258	✓✗	42	const Inertia Y = Inertia::Random();
259	✓✗	42	const Inertia::Matrix6 Y_mat = Y.matrix();
260	✓✓✗✓ ✗	84	Eigen::MatrixXd YS = Y_mat * constraint;
261	✓✗✓✗	84	Eigen::MatrixXd YS_ref = Y_mat * constraint_mat;
262	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	42	BOOST_CHECK(YS.isApprox(YS_ref));
263			}
264
265		42	}
266
267			template<typename JointModel_> struct init;
268
269			template<typename JointModel_>
270			struct init
271			{
272		34	static JointModel_ run()
273			{
274		34	JointModel_ jmodel;
275		34	jmodel.setIndexes(0,0,0);
276		34	return jmodel;
277			}
278			};
279
280			template<typename Scalar, int Options>
281			struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
282			{
283			typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;
284
285		1	static JointModel run()
286			{
287			typedef typename JointModel::Vector3 Vector3;
288	✓✗✓✗	1	JointModel jmodel(Vector3::Random().normalized());
289
290		1	jmodel.setIndexes(0,0,0);
291		1	return jmodel;
292			}
293			};
294
295			template<typename Scalar, int Options>
296			struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
297			{
298			typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;
299
300		1	static JointModel run()
301			{
302			typedef typename JointModel::Vector3 Vector3;
303	✓✗✓✗	1	JointModel jmodel(Vector3::Random().normalized());
304
305		1	jmodel.setIndexes(0,0,0);
306		1	return jmodel;
307			}
308			};
309
310			template<typename Scalar, int Options>
311			struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
312			{
313			typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;
314
315		1	static JointModel run()
316			{
317			typedef typename JointModel::Vector3 Vector3;
318	✓✗✓✗	1	JointModel jmodel(Vector3::Random().normalized());
319
320		1	jmodel.setIndexes(0,0,0);
321		1	return jmodel;
322			}
323			};
324
325			template<typename Scalar, int Options, template<typename,int> class JointCollection>
326			struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
327			{
328			typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;
329
330			static JointModel run()
331			{
332			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
333			JointModel jmodel((JointModelRX()));
334
335			jmodel.setIndexes(0,0,0);
336			return jmodel;
337			}
338			};
339
340			template<typename Scalar, int Options, template<typename,int> class JointCollection>
341			struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
342			{
343			typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;
344
345		1	static JointModel run()
346			{
347			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
348			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
349			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,2> JointModelRZ;
350
351	✓✗✓✗	1	JointModel jmodel(JointModelRX(),SE3::Random());
352	✓✗✓✗ ✓✗	1	jmodel.addJoint(JointModelRY(),SE3::Random());
353	✓✗✓✗ ✓✗	1	jmodel.addJoint(JointModelRZ(),SE3::Random());
354
355	✓✗	1	jmodel.setIndexes(0,0,0);
356
357		1	return jmodel;
358			}
359			};
360
361			template<typename JointModel_>
362			struct init<pinocchio::JointModelMimic<JointModel_> >
363			{
364			typedef pinocchio::JointModelMimic<JointModel_> JointModel;
365
366		6	static JointModel run()
367			{
368	✓✗	6	JointModel_ jmodel_ref = init<JointModel_>::run();
369
370	✓✗	6	JointModel jmodel(jmodel_ref,1.,0.);
371	✓✗	6	jmodel.setIndexes(0,0,0);
372
373		12	return jmodel;
374			}
375			};
376
377			struct TestJointConstraint
378			{
379
380			template <typename JointModel>
381		42	void operator()(const JointModelBase<JointModel> &) const
382			{
383	✓✗	44	JointModel jmodel = init<JointModel>::run();
384	✓✗	42	jmodel.setIndexes(0,0,0);
385
386	✓✗	42	test_constraint_operations(jmodel);
387		42	}
388
389			};
390
391	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_joint_constraint_operations)
392			{
393			typedef JointCollectionDefault::JointModelVariant Variant;
394	✓✗	2	boost::mpl::for_each<Variant::types>(TestJointConstraint());
395		2	}
396
397			BOOST_AUTO_TEST_SUITE_END()