Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/centroidal-derivatives.cpp	Lines:	112	113	99.1 %
Date:	2024-01-23 21:41:47	Branches:	459	914	50.2 %


//
// Copyright (c) 2018-2019 INRIA
//

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

#include <iostream>

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

template<typename JointModel>
static void addJointAndBody(pinocchio::Model & model,
                            const pinocchio::JointModelBase<JointModel> & joint,
                            const std::string & parent_name,
                            const std::string & name,
                            const pinocchio::SE3 placement = pinocchio::SE3::Random(),
                            bool setRandomLimits = true)
{
  using namespace pinocchio;
  typedef typename JointModel::ConfigVector_t CV;
  typedef typename JointModel::TangentVector_t TV;

  Model::JointIndex idx;

  if (setRandomLimits)
    idx = model.addJoint(model.getJointId(parent_name),joint,
                         SE3::Random(),
                         name + "_joint",
                         TV::Random() + TV::Constant(1),
                         TV::Random() + TV::Constant(1),
                         CV::Random() - CV::Constant(1),
                         CV::Random() + CV::Constant(1)
                         );
  else
    idx = model.addJoint(model.getJointId(parent_name),joint,
                         placement, name + "_joint");

  model.addJointFrame(idx);

  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
  model.addBodyFrame(name + "_body", idx);
}

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_centroidal_derivatives)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  const std::string parent_name = model.names.back();
  const std::string joint_name = "ee_spherical_joint";
  addJointAndBody(model, pinocchio::JointModelSpherical(), parent_name , joint_name);

  pinocchio::Data data(model), data_ref(model);

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

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

  pinocchio::Data::Matrix6x
    dh_dq(6,model.nv),dhdot_dq(6,model.nv), dhdot_dv(6,model.nv), dhdot_da(6,model.nv);
  pinocchio::computeCentroidalDynamicsDerivatives(model,data,q,v,a,
                                                  dh_dq,dhdot_dq,dhdot_dv,dhdot_da);
  pinocchio::ccrba(model,data_ref,q,v);

  for(size_t k = 0; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
    BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
  }
  BOOST_CHECK(dhdot_da.isApprox(data_ref.Ag));

  pinocchio::computeCentroidalMomentumTimeVariation(model,data_ref,q,v,a);
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.ov[k].isApprox(data.oMi[k].act(data_ref.v[k])));
    BOOST_CHECK(data.oa[k].isApprox(data.oMi[k].act(data_ref.a[k])));
    BOOST_CHECK(data.oh[k].isApprox(data.oMi[k].act(data_ref.h[k])));
  }

  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));

  BOOST_CHECK(data.oh[0].isApprox(data_ref.h[0]));
  BOOST_CHECK(data.of[0].isApprox(data_ref.f[0]));

  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
  BOOST_CHECK(data.dhg.isApprox(data_ref.dhg));

  pinocchio::Data data_fd(model);

  const double eps = 1e-8;
  const pinocchio::Force dhg = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v,a);
  const pinocchio::Force hg = data_fd.hg;
  BOOST_CHECK(data.hg.isApprox(data_ref.hg));

  // Check dhdot_dq and dh_dq with finite differences
  Eigen::VectorXd q_plus(model.nq,1);
  Eigen::VectorXd v_eps(model.nv,1); v_eps.setZero();
  pinocchio::Data::Matrix6x dhdot_dq_fd(6,model.nv);
  pinocchio::Data::Matrix6x dh_dq_fd(6,model.nv);

  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_eps[k] = eps;
    q_plus = pinocchio::integrate(model,q,v_eps);

    const pinocchio::Force & dhg_plus
    = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q_plus,v,a);
    const pinocchio::Force hg_plus = data_fd.hg;

    dhdot_dq_fd.col(k) = (dhg_plus - dhg).toVector()/eps;
    dh_dq_fd.col(k) = (hg_plus - hg).toVector()/eps;
    v_eps[k] = 0.;
  }

  BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_fd,sqrt(eps)));
  BOOST_CHECK(dh_dq.isApprox(dh_dq_fd,sqrt(eps)));
  // Check dhdot_dv with finite differences
  Eigen::VectorXd v_plus(v);
  pinocchio::Data::Matrix6x dhdot_dv_fd(6,model.nv);

  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    v_plus[k] += eps;

    const pinocchio::Force & dhg_plus
    = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v_plus,a);
    dhdot_dv_fd.col(k) = (dhg_plus - dhg).toVector()/eps;

    v_plus[k] -= eps;
  }

  BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_fd,sqrt(eps)));

  // Check dhdot_da with finite differences
  Eigen::VectorXd a_plus(a);
  pinocchio::Data::Matrix6x dhdot_da_fd(6,model.nv);

  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
  {
    a_plus[k] += eps;

    const pinocchio::Force & dhg_plus
    = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v,a_plus);
    dhdot_da_fd.col(k) = (dhg_plus - dhg).toVector()/eps;

    a_plus[k] -= eps;
  }

  BOOST_CHECK(dhdot_da.isApprox(dhdot_da_fd,sqrt(eps)));

  pinocchio::computeRNEADerivatives(model,data_ref,q,v,a);
  BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
  BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
}

BOOST_AUTO_TEST_CASE(test_retrieve_centroidal_derivatives)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model), data_ref(model);

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

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

  pinocchio::Data::Matrix6x
    dh_dq(6,model.nv), dhdot_dq(6,model.nv), dhdot_dv(6,model.nv), dhdot_da(6,model.nv);
  pinocchio::Data::Matrix6x
    dh_dq_ref(6,model.nv), dhdot_dq_ref(6,model.nv), dhdot_dv_ref(6,model.nv), dhdot_da_ref(6,model.nv);

  pinocchio::computeCentroidalDynamicsDerivatives(model,data_ref,q,v,a,
                                                  dh_dq_ref, dhdot_dq_ref,dhdot_dv_ref,dhdot_da_ref);

  pinocchio::computeRNEADerivatives(model,data,q,v,a);
  pinocchio::getCentroidalDynamicsDerivatives(model,data,
                                              dh_dq, dhdot_dq,dhdot_dv,dhdot_da);

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

  for(pinocchio::Model::JointIndex k = 1; k < (pinocchio::Model::JointIndex)model.njoints; ++k)
  {
    BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
    pinocchio::Force force_ref = data_ref.of[k];
    pinocchio::Force gravity_contribution = data.oYcrb[k] * (-model.gravity);
    pinocchio::Force force = data.of[k] - gravity_contribution;
    BOOST_CHECK(force.isApprox(force_ref));
  }

  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));

  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
  BOOST_CHECK(data.dhg.isApprox(data_ref.dhg));

  BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdq));
  BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv));
  BOOST_CHECK(data.dFda.isApprox(data_ref.dFda));
  BOOST_CHECK(dh_dq.isApprox(dh_dq_ref));
  BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_ref));
  BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_ref));
  BOOST_CHECK(dhdot_da.isApprox(dhdot_da_ref));

}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2018-2019 INRIA
3			//
4
5			#include "pinocchio/multibody/model.hpp"
6			#include "pinocchio/multibody/data.hpp"
7			#include "pinocchio/multibody/joint/joint-spherical.hpp"
8			#include "pinocchio/algorithm/crba.hpp"
9			#include "pinocchio/algorithm/centroidal.hpp"
10			#include "pinocchio/algorithm/centroidal-derivatives.hpp"
11			#include "pinocchio/algorithm/rnea-derivatives.hpp"
12			#include "pinocchio/algorithm/jacobian.hpp"
13			#include "pinocchio/algorithm/center-of-mass.hpp"
14			#include "pinocchio/algorithm/joint-configuration.hpp"
15			#include "pinocchio/parsers/sample-models.hpp"
16			#include "pinocchio/utils/timer.hpp"
17
18			#include <iostream>
19
20			#include <boost/test/unit_test.hpp>
21			#include <boost/utility/binary.hpp>
22
23			template<typename JointModel>
24		1	static void addJointAndBody(pinocchio::Model & model,
25			const pinocchio::JointModelBase<JointModel> & joint,
26			const std::string & parent_name,
27			const std::string & name,
28			const pinocchio::SE3 placement = pinocchio::SE3::Random(),
29			bool setRandomLimits = true)
30			{
31			using namespace pinocchio;
32			typedef typename JointModel::ConfigVector_t CV;
33			typedef typename JointModel::TangentVector_t TV;
34
35			Model::JointIndex idx;
36
37	✓✗	1	if (setRandomLimits)
38	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	1	idx = model.addJoint(model.getJointId(parent_name),joint,
39			SE3::Random(),
40			name + "_joint",
41			TV::Random() + TV::Constant(1),
42			TV::Random() + TV::Constant(1),
43			CV::Random() - CV::Constant(1),
44			CV::Random() + CV::Constant(1)
45			);
46			else
47			idx = model.addJoint(model.getJointId(parent_name),joint,
48			placement, name + "_joint");
49
50	✓✗	1	model.addJointFrame(idx);
51
52	✓✗✓✗ ✓✗	1	model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
53	✓✗✓✗ ✓✗	1	model.addBodyFrame(name + "_body", idx);
54		1	}
55
56			BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
57
58	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_centroidal_derivatives)
59			{
60	✓✗	4	pinocchio::Model model;
61	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
62	✓✗	4	const std::string parent_name = model.names.back();
63	✓✗	4	const std::string joint_name = "ee_spherical_joint";
64	✓✗✓✗ ✓✗	2	addJointAndBody(model, pinocchio::JointModelSpherical(), parent_name , joint_name);
65
66	✓✗✓✗	4	pinocchio::Data data(model), data_ref(model);
67
68	✓✗✓✗	2	model.lowerPositionLimit.head<7>().fill(-1.);
69	✓✗✓✗	2	model.upperPositionLimit.head<7>().fill( 1.);
70
71	✓✗	4	Eigen::VectorXd q = pinocchio::randomConfiguration(model);
72	✓✗✓✗	4	Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
73	✓✗✓✗	4	Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
74
75			pinocchio::Data::Matrix6x
76	✓✗✓✗ ✓✗✓✗	4	dh_dq(6,model.nv),dhdot_dq(6,model.nv), dhdot_dv(6,model.nv), dhdot_da(6,model.nv);
77	✓✗	2	pinocchio::computeCentroidalDynamicsDerivatives(model,data,q,v,a,
78			dh_dq,dhdot_dq,dhdot_dv,dhdot_da);
79	✓✗	2	pinocchio::ccrba(model,data_ref,q,v);
80
81	✓✓	60	for(size_t k = 0; k < (size_t)model.njoints; ++k)
82			{
83	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	58	BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k]));
84	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	58	BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
85			}
86	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dhdot_da.isApprox(data_ref.Ag));
87
88	✓✗	2	pinocchio::computeCentroidalMomentumTimeVariation(model,data_ref,q,v,a);
89	✓✓	58	for(size_t k = 1; k < (size_t)model.njoints; ++k)
90			{
91	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	56	BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
92	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	56	BOOST_CHECK(data.ov[k].isApprox(data.oMi[k].act(data_ref.v[k])));
93	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	56	BOOST_CHECK(data.oa[k].isApprox(data.oMi[k].act(data_ref.a[k])));
94	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	56	BOOST_CHECK(data.oh[k].isApprox(data.oMi[k].act(data_ref.h[k])));
95			}
96
97	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
98	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
99
100	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.oh[0].isApprox(data_ref.h[0]));
101	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.of[0].isApprox(data_ref.f[0]));
102
103	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.hg.isApprox(data_ref.hg));
104	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.dhg.isApprox(data_ref.dhg));
105
106	✓✗	4	pinocchio::Data data_fd(model);
107
108		2	const double eps = 1e-8;
109	✓✗✓✗	2	const pinocchio::Force dhg = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v,a);
110	✓✗	2	const pinocchio::Force hg = data_fd.hg;
111	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.hg.isApprox(data_ref.hg));
112
113			// Check dhdot_dq and dh_dq with finite differences
114	✓✗	4	Eigen::VectorXd q_plus(model.nq,1);
115	✓✗✓✗	4	Eigen::VectorXd v_eps(model.nv,1); v_eps.setZero();
116	✓✗	4	pinocchio::Data::Matrix6x dhdot_dq_fd(6,model.nv);
117	✓✗	4	pinocchio::Data::Matrix6x dh_dq_fd(6,model.nv);
118
119	✓✓	72	for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
120			{
121	✓✗	70	v_eps[k] = eps;
122	✓✗	70	q_plus = pinocchio::integrate(model,q,v_eps);
123
124			const pinocchio::Force & dhg_plus
125	✓✗	70	= pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q_plus,v,a);
126	✓✗	70	const pinocchio::Force hg_plus = data_fd.hg;
127
128	✓✗✓✗ ✓✗✓✗ ✓✗	70	dhdot_dq_fd.col(k) = (dhg_plus - dhg).toVector()/eps;
129	✓✗✓✗ ✓✗✓✗ ✓✗	70	dh_dq_fd.col(k) = (hg_plus - hg).toVector()/eps;
130	✓✗	70	v_eps[k] = 0.;
131			}
132
133	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_fd,sqrt(eps)));
134	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dh_dq.isApprox(dh_dq_fd,sqrt(eps)));
135			// Check dhdot_dv with finite differences
136	✓✗	4	Eigen::VectorXd v_plus(v);
137	✓✗	4	pinocchio::Data::Matrix6x dhdot_dv_fd(6,model.nv);
138
139	✓✓	72	for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
140			{
141	✓✗	70	v_plus[k] += eps;
142
143			const pinocchio::Force & dhg_plus
144	✓✗	70	= pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v_plus,a);
145	✓✗✓✗ ✓✗✓✗ ✓✗	70	dhdot_dv_fd.col(k) = (dhg_plus - dhg).toVector()/eps;
146
147	✓✗	70	v_plus[k] -= eps;
148			}
149
150	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_fd,sqrt(eps)));
151
152			// Check dhdot_da with finite differences
153	✓✗	4	Eigen::VectorXd a_plus(a);
154	✓✗	4	pinocchio::Data::Matrix6x dhdot_da_fd(6,model.nv);
155
156	✓✓	72	for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
157			{
158	✓✗	70	a_plus[k] += eps;
159
160			const pinocchio::Force & dhg_plus
161	✓✗	70	= pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v,a_plus);
162	✓✗✓✗ ✓✗✓✗ ✓✗	70	dhdot_da_fd.col(k) = (dhg_plus - dhg).toVector()/eps;
163
164	✓✗	70	a_plus[k] -= eps;
165			}
166
167	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dhdot_da.isApprox(dhdot_da_fd,sqrt(eps)));
168
169	✓✗	2	pinocchio::computeRNEADerivatives(model,data_ref,q,v,a);
170	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv));
171	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq));
172	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.J.isApprox(data_ref.J));
173	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
174	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq));
175		2	}
176
177	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_retrieve_centroidal_derivatives)
178			{
179	✓✗	4	pinocchio::Model model;
180	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
181	✓✗✓✗	4	pinocchio::Data data(model), data_ref(model);
182
183	✓✗✓✗	2	model.lowerPositionLimit.head<7>().fill(-1.);
184	✓✗✓✗	2	model.upperPositionLimit.head<7>().fill( 1.);
185
186	✓✗	4	Eigen::VectorXd q = pinocchio::randomConfiguration(model);
187	✓✗✓✗	4	Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
188	✓✗✓✗	4	Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
189
190			pinocchio::Data::Matrix6x
191	✓✗✓✗ ✓✗✓✗	4	dh_dq(6,model.nv), dhdot_dq(6,model.nv), dhdot_dv(6,model.nv), dhdot_da(6,model.nv);
192			pinocchio::Data::Matrix6x
193	✓✗✓✗ ✓✗✓✗	4	dh_dq_ref(6,model.nv), dhdot_dq_ref(6,model.nv), dhdot_dv_ref(6,model.nv), dhdot_da_ref(6,model.nv);
194
195	✓✗	2	pinocchio::computeCentroidalDynamicsDerivatives(model,data_ref,q,v,a,
196			dh_dq_ref, dhdot_dq_ref,dhdot_dv_ref,dhdot_da_ref);
197
198	✓✗	2	pinocchio::computeRNEADerivatives(model,data,q,v,a);
199	✓✗	2	pinocchio::getCentroidalDynamicsDerivatives(model,data,
200			dh_dq, dhdot_dq,dhdot_dv,dhdot_da);
201
202	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.J.isApprox(data_ref.J));
203
204	✓✓	56	for(pinocchio::Model::JointIndex k = 1; k < (pinocchio::Model::JointIndex)model.njoints; ++k)
205			{
206	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k]));
207	✓✗	54	pinocchio::Force force_ref = data_ref.of[k];
208	✓✗✓✗	54	pinocchio::Force gravity_contribution = data.oYcrb[k] * (-model.gravity);
209	✓✗	54	pinocchio::Force force = data.of[k] - gravity_contribution;
210	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(force.isApprox(force_ref));
211			}
212
213	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
214
215	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.hg.isApprox(data_ref.hg));
216	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.dhg.isApprox(data_ref.dhg));
217
218	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.Fcrb[0].isApprox(data_ref.dFdq));
219	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv));
220	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data.dFda.isApprox(data_ref.dFda));
221	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dh_dq.isApprox(dh_dq_ref));
222	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dhdot_dq.isApprox(dhdot_dq_ref));
223	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dhdot_dv.isApprox(dhdot_dv_ref));
224	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dhdot_da.isApprox(dhdot_da_ref));
225
226		2	}
227
228			BOOST_AUTO_TEST_SUITE_END()