Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/com.cpp	Lines:	107	107	100.0 %
Date:	2024-01-23 21:41:47	Branches:	395	782	50.5 %


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

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

#include <iostream>

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

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

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

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

  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);

  crba(model,data,q);


	/* Test COM against CRBA*/
  Vector3d com = centerOfMass(model,data,q);
  BOOST_CHECK(data.com[0].isApprox(getComFromCrba(model,data), 1e-12));

	/* Test COM against Jcom (both use different way to compute the COM). */
  com = centerOfMass(model,data,q);
  jacobianCenterOfMass(model,data,q);
  BOOST_CHECK(com.isApprox(data.com[0], 1e-12));

	/* Test COM against Jcom (both use different way to compute the COM). */
  centerOfMass(model,data,q,v,a);
  BOOST_CHECK(com.isApprox(data.com[0], 1e-12));

  /* Test vCoM against nle algorithm without gravity field */
  a.setZero();
  model.gravity.setZero();
  centerOfMass(model,data,q,v,a);
  nonLinearEffects(model, data, q, v);

  pinocchio::SE3::Vector3 acom_from_nle (data.nle.head <3> ()/data.mass[0]);
  BOOST_CHECK((data.liMi[1].rotation() * acom_from_nle).isApprox(data.acom[0], 1e-12));

	/* Test Jcom against CRBA  */
  Eigen::MatrixXd Jcom = jacobianCenterOfMass(model,data,q);
  BOOST_CHECK(data.Jcom.isApprox(getJacobianComFromCrba(model,data), 1e-12));

  /* Test CoM velocity againt jacobianCenterOfMass */
  BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));


  centerOfMass(model,data,q,v);
  /* Test CoM velocity againt jacobianCenterOfMass */
  BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));


//  std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
//  std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl;
//  std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl;
//  std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl;
}

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

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

  double mass = computeTotalMass(model);

  BOOST_CHECK(mass == mass); // checking it is not NaN

  double mass_check = 0.0;
  for(size_t i=1; i<(size_t)(model.njoints);++i)
    mass_check += model.inertias[i].mass();

  BOOST_CHECK_CLOSE(mass, mass_check, 1e-12);

  pinocchio::Data data1(model);

  double mass_data = computeTotalMass(model,data1);

  BOOST_CHECK(mass_data == mass_data); // checking it is not NaN
  BOOST_CHECK_CLOSE(mass, mass_data, 1e-12);
  BOOST_CHECK_CLOSE(data1.mass[0], mass_data, 1e-12);

  pinocchio::Data data2(model);
  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  centerOfMass(model,data2,q);

  BOOST_CHECK_CLOSE(data2.mass[0], mass, 1e-12);
}

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

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

  pinocchio::Data data1(model);

  computeSubtreeMasses(model,data1);

  pinocchio::Data data2(model);
  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  centerOfMass(model,data2,q);

  for(size_t i=0; i<(size_t)(model.njoints);++i)
  {
    BOOST_CHECK_CLOSE(data1.mass[i], data2.mass[i], 1e-12);
  }
}

//BOOST_AUTO_TEST_CASE ( test_timings )
//{
//  using namespace Eigen;
//  using namespace pinocchio;
//
//  pinocchio::Model model;
//  pinocchio::buildModels::humanoidRandom(model);
//  pinocchio::Data data(model);
//
//  long flag = BOOST_BINARY(1111);
//  PinocchioTicToc timer(PinocchioTicToc::US);
//  #ifdef NDEBUG
//    #ifdef _INTENSE_TESTING_
//      const size_t NBT = 1000*1000;
//    #else
//      const size_t NBT = 10;
//    #endif
//  #else
//    const size_t NBT = 1;
//    std::cout << "(the time score in debug mode is not relevant)  " ;
//  #endif
//
//  bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
//  if(verbose) std::cout <<"--" << std::endl;
//  Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
//
//  if( flag >> 0 & 1 )
//  {
//    timer.tic();
//    SMOOTH(NBT)
//    {
//      centerOfMass(model,data,q);
//    }
//    if(verbose) std::cout << "COM =\t";
//    timer.toc(std::cout,NBT);
//  }
//
//  if( flag >> 1 & 1 )
//  {
//    timer.tic();
//    SMOOTH(NBT)
//    {
//      centerOfMass(model,data,q,false);
//    }
//    if(verbose) std::cout << "Without sub-tree =\t";
//    timer.toc(std::cout,NBT);
//  }
//
//  if( flag >> 2 & 1 )
//  {
//    timer.tic();
//    SMOOTH(NBT)
//    {
//      jacobianCenterOfMass(model,data,q);
//    }
//    if(verbose) std::cout << "Jcom =\t";
//    timer.toc(std::cout,NBT);
//  }
//}

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

  Model model;
  buildModels::humanoidRandom(model);
  Data data(model);

  model.upperPositionLimit.head<3>().fill(1000);
  model.lowerPositionLimit.head<3>() = -model.upperPositionLimit.head<3>();
  VectorXd q = pinocchio::randomConfiguration(model);
  VectorXd v = VectorXd::Random(model.nv);

  Data data_ref(model);
  jacobianCenterOfMass(model,data_ref,q,true);

  centerOfMass(model, data, q, v);
  Data::Matrix3x Jcom(3,model.nv); Jcom.setZero();
  jacobianSubtreeCenterOfMass(model, data, 0, Jcom);

  BOOST_CHECK(Jcom.isApprox(data_ref.Jcom));

  centerOfMass(model, data_ref, q, v, true);
  computeJointJacobians(model, data_ref, q);
  Data::Matrix3x Jcom_extracted(3,model.nv), Jcom_fd(3,model.nv);
  Data data_extracted(model), data_fd(model);
  const double eps = 1e-8;
  jacobianCenterOfMass(model,data_extracted,q);

  // Get subtree jacobian and check that it is consistent with the com velocity
  for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; joint_id++)
  {
    SE3::Vector3 subtreeComVelocityInWorld_ref = data_ref.oMi[joint_id].rotation() * data_ref.vcom[joint_id];
    Jcom.setZero();
    data.J.setZero();
    jacobianSubtreeCenterOfMass(model, data, joint_id, Jcom);

    BOOST_CHECK(data.J.middleCols(model.joints[joint_id].idx_v(),data.nvSubtree[joint_id]).isApprox(data_ref.J.middleCols(model.joints[joint_id].idx_v(),data.nvSubtree[joint_id])));
    SE3::Vector3 subtreeComVelocityInWorld = Jcom * v;

    Jcom_extracted.setZero();
    getJacobianSubtreeCenterOfMass(model,data_extracted,joint_id,Jcom_extracted);

    // Check with finite differences
    Eigen::VectorXd v_plus(model.nv); v_plus.setZero();
    centerOfMass(model,data_fd,q);
    const SE3::Vector3 com = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
    Jcom_fd.setZero();
    for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
    {
      v_plus[k] = eps;
      Eigen::VectorXd q_plus = integrate(model,q,v_plus);
      centerOfMass(model,data_fd,q_plus);
      const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
      Jcom_fd.col(k) = (com_plus - com)/eps;
      v_plus[k] = 0.;
    }

//    Eigen::VectorXd q_plus = integrate(model,q,v*eps);
//    centerOfMass(model,data_fd,q_plus);
//    const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
//
//    const SE3::Vector3 vcom_subtree_fd = (com_plus - com)/eps;

    BOOST_CHECK(Jcom.isApprox(Jcom_fd,sqrt(eps)));
    BOOST_CHECK(Jcom_extracted.isApprox(Jcom_fd,sqrt(eps)));
    BOOST_CHECK(Jcom_extracted.isApprox(Jcom));

    BOOST_CHECK(std::fabs(data.mass[joint_id] - data_ref.mass[joint_id]) <= 1e-12);
    BOOST_CHECK(data.com[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.com[joint_id])));
    BOOST_CHECK(subtreeComVelocityInWorld.isApprox(subtreeComVelocityInWorld_ref));
  }
}

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/multibody/model.hpp"
6			#include "pinocchio/multibody/data.hpp"
7			#include "pinocchio/algorithm/jacobian.hpp"
8			#include "pinocchio/algorithm/joint-configuration.hpp"
9			#include "pinocchio/algorithm/crba.hpp"
10			#include "pinocchio/algorithm/compute-all-terms.hpp"
11			#include "pinocchio/algorithm/rnea.hpp"
12			#include "pinocchio/algorithm/center-of-mass.hpp"
13			#include "pinocchio/utils/timer.hpp"
14			#include "pinocchio/parsers/sample-models.hpp"
15
16			#include <iostream>
17
18			#include <boost/test/unit_test.hpp>
19			#include <boost/utility/binary.hpp>
20
21			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
22
23	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_com )
24			{
25			using namespace Eigen;
26			using namespace pinocchio;
27
28	✓✗	4	pinocchio::Model model;
29	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
30	✓✗	4	pinocchio::Data data(model);
31
32	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
33	✓✗✓✗	2	q.middleRows<4> (3).normalize();
34	✓✗✓✗	4	VectorXd v = VectorXd::Ones(model.nv);
35	✓✗✓✗	4	VectorXd a = VectorXd::Ones(model.nv);
36
37	✓✗	2	crba(model,data,q);
38
39
40			/* Test COM against CRBA*/
41	✓✗✓✗	2	Vector3d com = centerOfMass(model,data,q);
42	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(data.com[0].isApprox(getComFromCrba(model,data), 1e-12));
43
44			/* Test COM against Jcom (both use different way to compute the COM). */
45	✓✗✓✗	2	com = centerOfMass(model,data,q);
46	✓✗	2	jacobianCenterOfMass(model,data,q);
47	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(com.isApprox(data.com[0], 1e-12));
48
49			/* Test COM against Jcom (both use different way to compute the COM). */
50	✓✗	2	centerOfMass(model,data,q,v,a);
51	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(com.isApprox(data.com[0], 1e-12));
52
53			/* Test vCoM against nle algorithm without gravity field */
54	✓✗	2	a.setZero();
55	✓✗	2	model.gravity.setZero();
56	✓✗	2	centerOfMass(model,data,q,v,a);
57	✓✗	2	nonLinearEffects(model, data, q, v);
58
59	✓✗✓✗ ✓✗	2	pinocchio::SE3::Vector3 acom_from_nle (data.nle.head <3> ()/data.mass[0]);
60	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK((data.liMi[1].rotation() * acom_from_nle).isApprox(data.acom[0], 1e-12));
61
62			/* Test Jcom against CRBA */
63	✓✗✓✗	4	Eigen::MatrixXd Jcom = jacobianCenterOfMass(model,data,q);
64	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(data.Jcom.isApprox(getJacobianComFromCrba(model,data), 1e-12));
65
66			/* Test CoM velocity againt jacobianCenterOfMass */
67	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));
68
69
70	✓✗	2	centerOfMass(model,data,q,v);
71			/* Test CoM velocity againt jacobianCenterOfMass */
72	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12));
73
74
75			// std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl;
76			// std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl;
77			// std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl;
78			// std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl;
79		2	}
80
81	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_mass )
82			{
83			using namespace Eigen;
84			using namespace pinocchio;
85
86	✓✗	4	pinocchio::Model model;
87	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
88
89	✓✗	2	double mass = computeTotalMass(model);
90
91	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(mass == mass); // checking it is not NaN
92
93		2	double mass_check = 0.0;
94	✓✓	56	for(size_t i=1; i<(size_t)(model.njoints);++i)
95		54	mass_check += model.inertias[i].mass();
96
97	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK_CLOSE(mass, mass_check, 1e-12);
98
99	✓✗	4	pinocchio::Data data1(model);
100
101	✓✗	2	double mass_data = computeTotalMass(model,data1);
102
103	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(mass_data == mass_data); // checking it is not NaN
104	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK_CLOSE(mass, mass_data, 1e-12);
105	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK_CLOSE(data1.mass[0], mass_data, 1e-12);
106
107	✓✗	4	pinocchio::Data data2(model);
108	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
109	✓✗✓✗	2	q.middleRows<4> (3).normalize();
110	✓✗	2	centerOfMass(model,data2,q);
111
112	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK_CLOSE(data2.mass[0], mass, 1e-12);
113		2	}
114
115	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_subtree_masses )
116			{
117			using namespace Eigen;
118			using namespace pinocchio;
119
120	✓✗	4	pinocchio::Model model;
121	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
122
123	✓✗	4	pinocchio::Data data1(model);
124
125	✓✗	2	computeSubtreeMasses(model,data1);
126
127	✓✗	4	pinocchio::Data data2(model);
128	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
129	✓✗✓✗	2	q.middleRows<4> (3).normalize();
130	✓✗	2	centerOfMass(model,data2,q);
131
132	✓✓	58	for(size_t i=0; i<(size_t)(model.njoints);++i)
133			{
134	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	56	BOOST_CHECK_CLOSE(data1.mass[i], data2.mass[i], 1e-12);
135			}
136		2	}
137
138			//BOOST_AUTO_TEST_CASE ( test_timings )
139			//{
140			// using namespace Eigen;
141			// using namespace pinocchio;
142			//
143			// pinocchio::Model model;
144			// pinocchio::buildModels::humanoidRandom(model);
145			// pinocchio::Data data(model);
146			//
147			// long flag = BOOST_BINARY(1111);
148			// PinocchioTicToc timer(PinocchioTicToc::US);
149			// #ifdef NDEBUG
150			// #ifdef _INTENSE_TESTING_
151			// const size_t NBT = 1000*1000;
152			// #else
153			// const size_t NBT = 10;
154			// #endif
155			// #else
156			// const size_t NBT = 1;
157			// std::cout << "(the time score in debug mode is not relevant) " ;
158			// #endif
159			//
160			// bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
161			// if(verbose) std::cout <<"--" << std::endl;
162			// Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
163			//
164			// if( flag >> 0 & 1 )
165			// {
166			// timer.tic();
167			// SMOOTH(NBT)
168			// {
169			// centerOfMass(model,data,q);
170			// }
171			// if(verbose) std::cout << "COM =\t";
172			// timer.toc(std::cout,NBT);
173			// }
174			//
175			// if( flag >> 1 & 1 )
176			// {
177			// timer.tic();
178			// SMOOTH(NBT)
179			// {
180			// centerOfMass(model,data,q,false);
181			// }
182			// if(verbose) std::cout << "Without sub-tree =\t";
183			// timer.toc(std::cout,NBT);
184			// }
185			//
186			// if( flag >> 2 & 1 )
187			// {
188			// timer.tic();
189			// SMOOTH(NBT)
190			// {
191			// jacobianCenterOfMass(model,data,q);
192			// }
193			// if(verbose) std::cout << "Jcom =\t";
194			// timer.toc(std::cout,NBT);
195			// }
196			//}
197
198	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE(test_subtree_com_jacobian)
199			{
200			using namespace Eigen;
201			using namespace pinocchio;
202
203	✓✗	4	Model model;
204	✓✗	2	buildModels::humanoidRandom(model);
205	✓✗	4	Data data(model);
206
207	✓✗✓✗	2	model.upperPositionLimit.head<3>().fill(1000);
208	✓✗✓✗ ✓✗✓✗	2	model.lowerPositionLimit.head<3>() = -model.upperPositionLimit.head<3>();
209	✓✗	4	VectorXd q = pinocchio::randomConfiguration(model);
210	✓✗✓✗	4	VectorXd v = VectorXd::Random(model.nv);
211
212	✓✗	4	Data data_ref(model);
213	✓✗	2	jacobianCenterOfMass(model,data_ref,q,true);
214
215	✓✗	2	centerOfMass(model, data, q, v);
216	✓✗✓✗	4	Data::Matrix3x Jcom(3,model.nv); Jcom.setZero();
217	✓✗	2	jacobianSubtreeCenterOfMass(model, data, 0, Jcom);
218
219	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(Jcom.isApprox(data_ref.Jcom));
220
221	✓✗	2	centerOfMass(model, data_ref, q, v, true);
222	✓✗	2	computeJointJacobians(model, data_ref, q);
223	✓✗✓✗	4	Data::Matrix3x Jcom_extracted(3,model.nv), Jcom_fd(3,model.nv);
224	✓✗✓✗	4	Data data_extracted(model), data_fd(model);
225		2	const double eps = 1e-8;
226	✓✗	2	jacobianCenterOfMass(model,data_extracted,q);
227
228			// Get subtree jacobian and check that it is consistent with the com velocity
229	✓✓	56	for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; joint_id++)
230			{
231	✓✗✓✗ ✓✗	54	SE3::Vector3 subtreeComVelocityInWorld_ref = data_ref.oMi[joint_id].rotation() * data_ref.vcom[joint_id];
232	✓✗	54	Jcom.setZero();
233	✓✗	54	data.J.setZero();
234	✓✗	54	jacobianSubtreeCenterOfMass(model, data, joint_id, Jcom);
235
236	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(data.J.middleCols(model.joints[joint_id].idx_v(),data.nvSubtree[joint_id]).isApprox(data_ref.J.middleCols(model.joints[joint_id].idx_v(),data.nvSubtree[joint_id])));
237	✓✗✓✗	54	SE3::Vector3 subtreeComVelocityInWorld = Jcom * v;
238
239	✓✗	54	Jcom_extracted.setZero();
240	✓✗	54	getJacobianSubtreeCenterOfMass(model,data_extracted,joint_id,Jcom_extracted);
241
242			// Check with finite differences
243	✓✗✓✗	108	Eigen::VectorXd v_plus(model.nv); v_plus.setZero();
244	✓✗	54	centerOfMass(model,data_fd,q);
245	✓✗	54	const SE3::Vector3 com = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
246	✓✗	54	Jcom_fd.setZero();
247	✓✓	1782	for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
248			{
249	✓✗	1728	v_plus[k] = eps;
250	✓✗	1728	Eigen::VectorXd q_plus = integrate(model,q,v_plus);
251	✓✗	1728	centerOfMass(model,data_fd,q_plus);
252	✓✗	1728	const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
253	✓✗✓✗ ✓✗✓✗	1728	Jcom_fd.col(k) = (com_plus - com)/eps;
254	✓✗	1728	v_plus[k] = 0.;
255			}
256
257			// Eigen::VectorXd q_plus = integrate(model,q,v*eps);
258			// centerOfMass(model,data_fd,q_plus);
259			// const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]);
260			//
261			// const SE3::Vector3 vcom_subtree_fd = (com_plus - com)/eps;
262
263	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(Jcom.isApprox(Jcom_fd,sqrt(eps)));
264	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(Jcom_extracted.isApprox(Jcom_fd,sqrt(eps)));
265	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(Jcom_extracted.isApprox(Jcom));
266
267	✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(std::fabs(data.mass[joint_id] - data_ref.mass[joint_id]) <= 1e-12);
268	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	54	BOOST_CHECK(data.com[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.com[joint_id])));
269	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	54	BOOST_CHECK(subtreeComVelocityInWorld.isApprox(subtreeComVelocityInWorld_ref));
270			}
271		2	}
272
273			BOOST_AUTO_TEST_SUITE_END ()