Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/rnea.cpp	Lines:	156	156	100.0 %
Date:	2024-01-23 21:41:47	Branches:	528	1050	50.3 %


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

/*
 * Unittest of the RNE algorithm. The code simply test that the algorithm does
 * not cause any serious errors. The numerical values are not cross validated
 * in any way.
 *
 */

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"

#include <iostream>

//#define __SSE3__
#include <fenv.h>

#ifdef __SSE3__
#include <pmmintrin.h>
#endif

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

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_rnea )
{
  #ifdef __SSE3__
    _MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
  #endif
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  pinocchio::Data data(model);
  data.v[0] = Motion::Zero();
  data.a[0] = -model.gravity;

  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd a = VectorXd::Random(model.nv);

  #ifdef NDEBUG
    const size_t NBT = 10000;
  #else
    const size_t NBT = 1;
    std::cout << "(the time score in debug mode is not relevant)  " ;
  #endif

  PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
  SMOOTH(NBT)
    {
      rnea(model,data,q,v,a);
    }
  timer.toc(std::cout,NBT);

}

BOOST_AUTO_TEST_CASE ( test_nle_vs_rnea )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model; buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  pinocchio::Data data_nle(model);
  pinocchio::Data data_rnea(model);

  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);

  VectorXd tau_nle (VectorXd::Zero (model.nv));
  VectorXd tau_rnea (VectorXd::Zero (model.nv));

  // -------
  q.tail(model.nq-7).setZero();
  v.setZero();

  tau_nle = nonLinearEffects(model,data_nle,q,v);
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));

  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));

  // -------
  q.tail(model.nq-7).setZero();
  v.setOnes();

  tau_nle = nonLinearEffects(model,data_nle,q,v);
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));

  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));

  // -------
  q.tail(model.nq-7).setOnes();
  v.setOnes();

  tau_nle = nonLinearEffects(model,data_nle,q,v);
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));

  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));

  // -------
  q = randomConfiguration(model);
  v.setRandom();

  tau_nle = nonLinearEffects(model,data_nle,q,v);
  tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));

  BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
}

BOOST_AUTO_TEST_CASE (test_rnea_with_fext)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  Data data_rnea_fext(model);
  Data data_rnea(model);

  VectorXd q = randomConfiguration(model);

  VectorXd v (VectorXd::Random(model.nv));
  VectorXd a (VectorXd::Random(model.nv));

  PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Zero());

  JointIndex rf = model.getJointId("rleg6_joint"); Force Frf = Force::Random();
  fext[rf] = Frf;
  JointIndex lf = model.getJointId("lleg6_joint"); Force Flf = Force::Random();
  fext[lf] = Flf;

  rnea(model,data_rnea,q,v,a);
  VectorXd tau_ref(data_rnea.tau);
  Data::Matrix6x Jrf(Data::Matrix6x::Zero(6,model.nv));
  computeJointJacobian(model,data_rnea,q,rf,Jrf);
  tau_ref -= Jrf.transpose() * Frf.toVector();

  Data::Matrix6x Jlf(Data::Matrix6x::Zero(6,model.nv));
  computeJointJacobian(model,data_rnea,q,lf,Jlf);
  tau_ref -= Jlf.transpose() * Flf.toVector();

  rnea(model,data_rnea_fext,q,v,a,fext);

  BOOST_CHECK(tau_ref.isApprox(data_rnea_fext.tau));
}

BOOST_AUTO_TEST_CASE(test_compute_gravity)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  Data data_rnea(model);
  Data data(model);

  VectorXd q = randomConfiguration(model);

  rnea(model,data_rnea,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
  computeGeneralizedGravity(model,data,q);

  BOOST_CHECK(data_rnea.tau.isApprox(data.g));

  // Compare with Jcom
  crba(model,data_rnea,q);
  Data::Matrix3x Jcom = getJacobianComFromCrba(model,data_rnea);

  VectorXd g_ref(-data_rnea.mass[0]*Jcom.transpose()*Model::gravity981);

  BOOST_CHECK(g_ref.isApprox(data.g));
}

BOOST_AUTO_TEST_CASE(test_compute_static_torque)
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  Data data_rnea(model);
  Data data(model);

  VectorXd q = randomConfiguration(model);

  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
  ForceVector fext((size_t)model.njoints);
  for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
    (*it).setRandom();

  rnea(model,data_rnea,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv),fext);
  computeStaticTorque(model,data,q,fext);

  BOOST_CHECK(data_rnea.tau.isApprox(data.tau));

  // Compare with Jcom + Jacobian of joint
  crba(model,data_rnea,q);
  Data::Matrix3x Jcom = getJacobianComFromCrba(model,data_rnea);

  VectorXd static_torque_ref = -data_rnea.mass[0]*Jcom.transpose()*Model::gravity981;
  computeJointJacobians(model,data_rnea,q);

  Data::Matrix6x J_local(6,model.nv);
  for(JointIndex joint_id = 1; joint_id < (JointIndex)(model.njoints); ++joint_id)
  {
    J_local.setZero();
    getJointJacobian(model, data_rnea, joint_id, LOCAL, J_local);
    static_torque_ref -= J_local.transpose() * fext[joint_id].toVector();
  }

  BOOST_CHECK(static_torque_ref.isApprox(data.tau));
}

BOOST_AUTO_TEST_CASE(test_compute_coriolis)
{
  using namespace Eigen;
  using namespace pinocchio;

  const double prec = Eigen::NumTraits<double>::dummy_precision();

  Model model;
  buildModels::humanoidRandom(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);

  Data data_ref(model);
  Data data(model);

  VectorXd q = randomConfiguration(model);

  VectorXd v (VectorXd::Random(model.nv));
  computeCoriolisMatrix(model,data,q,Eigen::VectorXd::Zero(model.nv));
  BOOST_CHECK(data.C.isZero(prec));


  model.gravity.setZero();
  rnea(model,data_ref,q,v,VectorXd::Zero(model.nv));
  computeJointJacobiansTimeVariation(model,data_ref,q,v);
  computeCoriolisMatrix(model,data,q,v);

  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  BOOST_CHECK(data.J.isApprox(data_ref.J));

  VectorXd tau = data.C * v;
  BOOST_CHECK(tau.isApprox(data_ref.tau,prec));

  dccrba(model,data_ref,q,v);
  crba(model,data_ref,q);

  const Data::Vector3 & com = data_ref.com[0];
  Motion vcom(data_ref.vcom[0],Data::Vector3::Zero());
  SE3 cM1(data.oMi[1]); cM1.translation() -= com;

  BOOST_CHECK((cM1.toDualActionMatrix()*data_ref.M.topRows<6>()).isApprox(data_ref.Ag,prec));

  Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
  Force dh(data_ref.dAg * v);
  BOOST_CHECK(dh.isApprox(dh_ref,prec));

  {
    Data data_ref(model), data_ref_plus(model);
    Eigen::MatrixXd dM(data.C + data.C.transpose());

    const double alpha = 1e-8;
    Eigen::VectorXd q_plus(model.nq);
    q_plus = integrate(model,q,alpha*v);

    crba(model,data_ref,q);
    data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
    crba(model,data_ref_plus,q_plus);
    data_ref_plus.M.triangularView<Eigen::StrictlyLower>() = data_ref_plus.M.transpose().triangularView<Eigen::StrictlyLower>();

    Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M)/alpha;
    BOOST_CHECK(dM.isApprox(dM_ref,sqrt(alpha)));
  }

}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2020 CNRS INRIA
3			//
4
5			/*
6			* Unittest of the RNE algorithm. The code simply test that the algorithm does
7			* not cause any serious errors. The numerical values are not cross validated
8			* in any way.
9			*
10			*/
11
12			#include "pinocchio/spatial/fwd.hpp"
13			#include "pinocchio/multibody/model.hpp"
14			#include "pinocchio/multibody/data.hpp"
15			#include "pinocchio/algorithm/rnea.hpp"
16			#include "pinocchio/algorithm/jacobian.hpp"
17			#include "pinocchio/algorithm/center-of-mass.hpp"
18			#include "pinocchio/algorithm/joint-configuration.hpp"
19			#include "pinocchio/algorithm/crba.hpp"
20			#include "pinocchio/algorithm/centroidal.hpp"
21			#include "pinocchio/parsers/sample-models.hpp"
22			#include "pinocchio/utils/timer.hpp"
23
24			#include <iostream>
25
26			//#define __SSE3__
27			#include <fenv.h>
28
29			#ifdef __SSE3__
30			#include <pmmintrin.h>
31			#endif
32
33			#include <boost/test/unit_test.hpp>
34			#include <boost/utility/binary.hpp>
35
36			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
37
38	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_rnea )
39			{
40			#ifdef __SSE3__
41			_MM_SET_DENORMALS_ZERO_MODE(_MM_DENORMALS_ZERO_ON);
42			#endif
43			using namespace Eigen;
44			using namespace pinocchio;
45
46	✓✗✓✗	4	pinocchio::Model model; buildModels::humanoidRandom(model);
47
48	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
49	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill( 1.);
50
51	✓✗	4	pinocchio::Data data(model);
52	✓✗	2	data.v[0] = Motion::Zero();
53	✓✗	2	data.a[0] = -model.gravity;
54
55	✓✗	4	VectorXd q = randomConfiguration(model);
56	✓✗✓✗	4	VectorXd v = VectorXd::Random(model.nv);
57	✓✗✓✗	4	VectorXd a = VectorXd::Random(model.nv);
58
59			#ifdef NDEBUG
60			const size_t NBT = 10000;
61			#else
62		2	const size_t NBT = 1;
63	✓✗	2	std::cout << "(the time score in debug mode is not relevant) " ;
64			#endif
65
66	✓✗✓✗	4	PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
67	✓✓	4	SMOOTH(NBT)
68			{
69	✓✗	2	rnea(model,data,q,v,a);
70			}
71	✓✗	2	timer.toc(std::cout,NBT);
72
73		2	}
74
75	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_nle_vs_rnea )
76			{
77			using namespace Eigen;
78			using namespace pinocchio;
79
80	✓✗✓✗	4	pinocchio::Model model; buildModels::humanoidRandom(model);
81
82	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
83	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill( 1.);
84
85	✓✗	4	pinocchio::Data data_nle(model);
86	✓✗	4	pinocchio::Data data_rnea(model);
87
88	✓✗	4	VectorXd q = randomConfiguration(model);
89	✓✗✓✗	4	VectorXd v = VectorXd::Random(model.nv);
90
91	✓✗✓✗	4	VectorXd tau_nle (VectorXd::Zero (model.nv));
92	✓✗✓✗	4	VectorXd tau_rnea (VectorXd::Zero (model.nv));
93
94			// -------
95	✓✗✓✗	2	q.tail(model.nq-7).setZero();
96	✓✗	2	v.setZero();
97
98	✓✗✓✗	2	tau_nle = nonLinearEffects(model,data_nle,q,v);
99	✓✗✓✗ ✓✗	2	tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
100
101	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
102
103			// -------
104	✓✗✓✗	2	q.tail(model.nq-7).setZero();
105	✓✗	2	v.setOnes();
106
107	✓✗✓✗	2	tau_nle = nonLinearEffects(model,data_nle,q,v);
108	✓✗✓✗ ✓✗	2	tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
109
110	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
111
112			// -------
113	✓✗✓✗	2	q.tail(model.nq-7).setOnes();
114	✓✗	2	v.setOnes();
115
116	✓✗✓✗	2	tau_nle = nonLinearEffects(model,data_nle,q,v);
117	✓✗✓✗ ✓✗	2	tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
118
119	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
120
121			// -------
122	✓✗	2	q = randomConfiguration(model);
123	✓✗	2	v.setRandom();
124
125	✓✗✓✗	2	tau_nle = nonLinearEffects(model,data_nle,q,v);
126	✓✗✓✗ ✓✗	2	tau_rnea = rnea(model,data_rnea,q,v,VectorXd::Zero (model.nv));
127
128	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_nle.isApprox(tau_rnea, 1e-12));
129		2	}
130
131	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE (test_rnea_with_fext)
132			{
133			using namespace Eigen;
134			using namespace pinocchio;
135
136	✓✗	4	Model model;
137	✓✗	2	buildModels::humanoidRandom(model);
138
139	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
140	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill( 1.);
141
142	✓✗	4	Data data_rnea_fext(model);
143	✓✗	4	Data data_rnea(model);
144
145	✓✗	4	VectorXd q = randomConfiguration(model);
146
147	✓✗✓✗	4	VectorXd v (VectorXd::Random(model.nv));
148	✓✗✓✗	4	VectorXd a (VectorXd::Random(model.nv));
149
150	✓✗✓✗	4	PINOCCHIO_ALIGNED_STD_VECTOR(Force) fext(model.joints.size(), Force::Zero());
151
152	✓✗✓✗ ✓✗	2	JointIndex rf = model.getJointId("rleg6_joint"); Force Frf = Force::Random();
153	✓✗	2	fext[rf] = Frf;
154	✓✗✓✗ ✓✗	2	JointIndex lf = model.getJointId("lleg6_joint"); Force Flf = Force::Random();
155	✓✗	2	fext[lf] = Flf;
156
157	✓✗	2	rnea(model,data_rnea,q,v,a);
158	✓✗	4	VectorXd tau_ref(data_rnea.tau);
159	✓✗✓✗	4	Data::Matrix6x Jrf(Data::Matrix6x::Zero(6,model.nv));
160	✓✗	2	computeJointJacobian(model,data_rnea,q,rf,Jrf);
161	✓✗✓✗ ✓✗✓✗	2	tau_ref -= Jrf.transpose() * Frf.toVector();
162
163	✓✗✓✗	4	Data::Matrix6x Jlf(Data::Matrix6x::Zero(6,model.nv));
164	✓✗	2	computeJointJacobian(model,data_rnea,q,lf,Jlf);
165	✓✗✓✗ ✓✗✓✗	2	tau_ref -= Jlf.transpose() * Flf.toVector();
166
167	✓✗	2	rnea(model,data_rnea_fext,q,v,a,fext);
168
169	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau_ref.isApprox(data_rnea_fext.tau));
170		2	}
171
172	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_compute_gravity)
173			{
174			using namespace Eigen;
175			using namespace pinocchio;
176
177	✓✗	4	Model model;
178	✓✗	2	buildModels::humanoidRandom(model);
179
180	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
181	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill( 1.);
182
183	✓✗	4	Data data_rnea(model);
184	✓✗	4	Data data(model);
185
186	✓✗	4	VectorXd q = randomConfiguration(model);
187
188	✓✗✓✗ ✓✗	2	rnea(model,data_rnea,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv));
189	✓✗	2	computeGeneralizedGravity(model,data,q);
190
191	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data_rnea.tau.isApprox(data.g));
192
193			// Compare with Jcom
194	✓✗	2	crba(model,data_rnea,q);
195	✓✗✓✗	4	Data::Matrix3x Jcom = getJacobianComFromCrba(model,data_rnea);
196
197	✓✗✓✗ ✓✗✓✗	4	VectorXd g_ref(-data_rnea.mass[0]Jcom.transpose()Model::gravity981);
198
199	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(g_ref.isApprox(data.g));
200		2	}
201
202	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_compute_static_torque)
203			{
204			using namespace Eigen;
205			using namespace pinocchio;
206
207	✓✗	4	Model model;
208	✓✗	2	buildModels::humanoidRandom(model);
209
210	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
211	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill( 1.);
212
213	✓✗	4	Data data_rnea(model);
214	✓✗	4	Data data(model);
215
216	✓✗	4	VectorXd q = randomConfiguration(model);
217
218			typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector;
219	✓✗✓✗	4	ForceVector fext((size_t)model.njoints);
220	✓✓	58	for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
221	✓✗	56	(*it).setRandom();
222
223	✓✗✓✗ ✓✗	2	rnea(model,data_rnea,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv),fext);
224	✓✗	2	computeStaticTorque(model,data,q,fext);
225
226	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data_rnea.tau.isApprox(data.tau));
227
228			// Compare with Jcom + Jacobian of joint
229	✓✗	2	crba(model,data_rnea,q);
230	✓✗✓✗	4	Data::Matrix3x Jcom = getJacobianComFromCrba(model,data_rnea);
231
232	✓✗✓✗ ✓✗✓✗	4	VectorXd static_torque_ref = -data_rnea.mass[0]Jcom.transpose()Model::gravity981;
233	✓✗	2	computeJointJacobians(model,data_rnea,q);
234
235	✓✗	4	Data::Matrix6x J_local(6,model.nv);
236	✓✓	56	for(JointIndex joint_id = 1; joint_id < (JointIndex)(model.njoints); ++joint_id)
237			{
238	✓✗	54	J_local.setZero();
239	✓✗	54	getJointJacobian(model, data_rnea, joint_id, LOCAL, J_local);
240	✓✗✓✗ ✓✗✓✗	54	static_torque_ref -= J_local.transpose() * fext[joint_id].toVector();
241			}
242
243	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(static_torque_ref.isApprox(data.tau));
244		2	}
245
246	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_compute_coriolis)
247			{
248			using namespace Eigen;
249			using namespace pinocchio;
250
251		2	const double prec = Eigen::NumTraits<double>::dummy_precision();
252
253	✓✗	4	Model model;
254	✓✗	2	buildModels::humanoidRandom(model);
255
256	✓✗✓✗	2	model.lowerPositionLimit.head<3>().fill(-1.);
257	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill( 1.);
258
259	✓✗	4	Data data_ref(model);
260	✓✗	4	Data data(model);
261
262	✓✗	4	VectorXd q = randomConfiguration(model);
263
264	✓✗✓✗	4	VectorXd v (VectorXd::Random(model.nv));
265	✓✗✓✗	2	computeCoriolisMatrix(model,data,q,Eigen::VectorXd::Zero(model.nv));
266	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.C.isZero(prec));
267
268
269	✓✗	2	model.gravity.setZero();
270	✓✗✓✗	2	rnea(model,data_ref,q,v,VectorXd::Zero(model.nv));
271	✓✗	2	computeJointJacobiansTimeVariation(model,data_ref,q,v);
272	✓✗	2	computeCoriolisMatrix(model,data,q,v);
273
274	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
275	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.J.isApprox(data_ref.J));
276
277	✓✗✓✗	4	VectorXd tau = data.C * v;
278	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(tau.isApprox(data_ref.tau,prec));
279
280	✓✗	2	dccrba(model,data_ref,q,v);
281	✓✗	2	crba(model,data_ref,q);
282
283		2	const Data::Vector3 & com = data_ref.com[0];
284	✓✗✓✗	2	Motion vcom(data_ref.vcom[0],Data::Vector3::Zero());
285	✓✗✓✗ ✓✗	2	SE3 cM1(data.oMi[1]); cM1.translation() -= com;
286
287	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((cM1.toDualActionMatrix()*data_ref.M.topRows<6>()).isApprox(data_ref.Ag,prec));
288
289	✓✗✓✗ ✓✗	2	Force dh_ref = cM1.act(Force(data_ref.tau.head<6>()));
290	✓✗✓✗	2	Force dh(data_ref.dAg * v);
291	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dh.isApprox(dh_ref,prec));
292
293			{
294	✓✗✓✗	4	Data data_ref(model), data_ref_plus(model);
295	✓✗✓✗ ✓✗	4	Eigen::MatrixXd dM(data.C + data.C.transpose());
296
297		2	const double alpha = 1e-8;
298	✓✗	4	Eigen::VectorXd q_plus(model.nq);
299	✓✗✓✗	2	q_plus = integrate(model,q,alpha*v);
300
301	✓✗	2	crba(model,data_ref,q);
302	✓✗✓✗ ✓✗✓✗	2	data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
303	✓✗	2	crba(model,data_ref_plus,q_plus);
304	✓✗✓✗ ✓✗✓✗	2	data_ref_plus.M.triangularView<Eigen::StrictlyLower>() = data_ref_plus.M.transpose().triangularView<Eigen::StrictlyLower>();
305
306	✓✗✓✗ ✓✗	4	Eigen::MatrixXd dM_ref = (data_ref_plus.M - data_ref.M)/alpha;
307	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dM.isApprox(dM_ref,sqrt(alpha)));
308			}
309
310		2	}
311
312			BOOST_AUTO_TEST_SUITE_END()