Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/joint-jacobian.cpp	Lines:	144	144	100.0 %
Date:	2024-01-23 21:41:47	Branches:	401	834	48.1 %


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

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include <iostream>

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

template<typename Derived>
inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
{
  return ((x - x).array() == (x - x).array()).all();
}

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

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

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

  VectorXd q = VectorXd::Zero(model.nq);
  computeJointJacobians(model,data,q);

  Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
  Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
  getJointJacobian(model,data,idx,WORLD,Jrh);

   /* Test J*q == v */
  VectorXd qdot = VectorXd::Random(model.nv);
  VectorXd qddot = VectorXd::Zero(model.nv);
  rnea( model,data,q,qdot,qddot );
  Motion v = data.oMi[idx].act( data.v[idx] );
  BOOST_CHECK(v.toVector().isApprox(Jrh*qdot,1e-12));


  /* Test local jacobian: rhJrh == rhXo oJrh */
  Data::Matrix6x rhJrh(6,model.nv); rhJrh.fill(0);
  getJointJacobian(model,data,idx,LOCAL,rhJrh);
  Data::Matrix6x XJrh(6,model.nv);
  motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
  BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));

  XJrh.setZero();
  Data data_jointJacobian(model);
  computeJointJacobian(model,data_jointJacobian,q,idx,XJrh);
  BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));

  /* Test computeJointJacobians with pre-computation of the forward kinematics */
  Data data_fk(model);
  forwardKinematics(model, data_fk, q);
  computeJointJacobians(model, data_fk);

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

}

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

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

  VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd a = VectorXd::Random(model.nv);

  computeJointJacobiansTimeVariation(model,data,q,v);

  BOOST_CHECK(isFinite(data.dJ));

  forwardKinematics(model,data_ref,q,v,a);
  Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);

  Data::Matrix6x J(6,model.nv); J.fill(0.);
  Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);

  // Regarding to the WORLD origin
  getJointJacobian(model,data,idx,WORLD,J);
  getJointJacobianTimeVariation(model,data,idx,WORLD,dJ);

  Motion v_idx(J*v);
  BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx])));

  Motion a_idx(J*a + dJ*v);
  const Motion & a_ref = data_ref.oMi[idx].act(data_ref.a[idx]);
  BOOST_CHECK(a_idx.isApprox(a_ref));

  // Regarding to the LOCAL frame
  getJointJacobian(model,data,idx,LOCAL,J);
  getJointJacobianTimeVariation(model,data,idx,LOCAL,dJ);

  v_idx = (Motion::Vector6)(J*v);
  BOOST_CHECK(v_idx.isApprox(data_ref.v[idx]));

  a_idx = (Motion::Vector6)(J*a + dJ*v);
  BOOST_CHECK(a_idx.isApprox(data_ref.a[idx]));

  // Regarding to the LOCAL_WORLD_ALIGNED frame
  getJointJacobian(model,data,idx,LOCAL_WORLD_ALIGNED,J);
  getJointJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ);

  Data::SE3 worldMlocal = data.oMi[idx];
  worldMlocal.translation().setZero();

  v_idx = (Motion::Vector6)(J*v);
  BOOST_CHECK(v_idx.isApprox(worldMlocal.act(data_ref.v[idx])));

  a_idx = (Motion::Vector6)(J*a + dJ*v);
  BOOST_CHECK(a_idx.isApprox(worldMlocal.act(data_ref.a[idx])));

  // compare to finite differencies : WORLD
  {
    Data data_ref(model), data_ref_plus(model);

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

    Data::Matrix6x J_ref(6,model.nv); J_ref.fill(0.);
    computeJointJacobians(model,data_ref,q);
    getJointJacobian(model,data_ref,idx,WORLD,J_ref);

    Data::Matrix6x J_ref_plus(6,model.nv); J_ref_plus.fill(0.);
    computeJointJacobians(model,data_ref_plus,q_plus);
    getJointJacobian(model,data_ref_plus,idx,WORLD,J_ref_plus);

    Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.fill(0.);
    dJ_ref = (J_ref_plus - J_ref)/alpha;

    computeJointJacobiansTimeVariation(model,data,q,v);
    Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
    getJointJacobianTimeVariation(model,data,idx,WORLD,dJ);

    BOOST_CHECK(dJ.isApprox(dJ_ref,sqrt(alpha)));
  }

  // compare to finite differencies : LOCAL
  {
    Data data_ref(model), data_ref_plus(model);

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

    Data::Matrix6x J_ref(6,model.nv); J_ref.fill(0.);
    computeJointJacobians(model,data_ref,q);
    getJointJacobian(model,data_ref,idx,LOCAL,J_ref);

    Data::Matrix6x J_ref_plus(6,model.nv); J_ref_plus.fill(0.);
    computeJointJacobians(model,data_ref_plus,q_plus);
    getJointJacobian(model,data_ref_plus,idx,LOCAL,J_ref_plus);

    const Data::SE3 M_plus = data_ref.oMi[idx].inverse()*data_ref_plus.oMi[idx];

    Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.fill(0.);
    dJ_ref = (M_plus.toActionMatrix()*J_ref_plus - J_ref)/alpha;

    computeJointJacobiansTimeVariation(model,data,q,v);
    Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
    getJointJacobianTimeVariation(model,data,idx,LOCAL,dJ);

    BOOST_CHECK(dJ.isApprox(dJ_ref,sqrt(alpha)));
  }
}

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)
    {
      computeJointJacobians(model,data,q);
    }
    if(verbose) std::cout << "Compute =\t";
    timer.toc(std::cout,NBT);
  }

  if( flag >> 1 & 1 )
  {
    computeJointJacobians(model,data,q);
    Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
    Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);

    timer.tic();
    SMOOTH(NBT)
    {
      getJointJacobian(model,data,idx,WORLD,Jrh);
    }
    if(verbose) std::cout << "Copy =\t";
    timer.toc(std::cout,NBT);
  }

  if( flag >> 2 & 1 )
  {
    computeJointJacobians(model,data,q);
    Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
    Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);

    timer.tic();
    SMOOTH(NBT)
    {
      getJointJacobian(model,data,idx,LOCAL,Jrh);
    }
    if(verbose) std::cout << "Change frame =\t";
    timer.toc(std::cout,NBT);
  }

  if( flag >> 3 & 1 )
  {
    computeJointJacobians(model,data,q);
    Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
    Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);

    timer.tic();
    SMOOTH(NBT)
    {
      computeJointJacobian(model,data,q,idx,Jrh);
    }
    if(verbose) std::cout << "Single jacobian =\t";
    timer.toc(std::cout,NBT);
  }
}

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/kinematics.hpp"
9			#include "pinocchio/algorithm/rnea.hpp"
10			#include "pinocchio/spatial/act-on-set.hpp"
11			#include "pinocchio/parsers/sample-models.hpp"
12			#include "pinocchio/utils/timer.hpp"
13			#include "pinocchio/algorithm/joint-configuration.hpp"
14
15			#include <iostream>
16
17			#include <boost/test/unit_test.hpp>
18			#include <boost/utility/binary.hpp>
19
20			template<typename Derived>
21		1	inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
22			{
23	✓✗✓✗ ✓✗✓✗ ✓✗	1	return ((x - x).array() == (x - x).array()).all();
24			}
25
26			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
27
28	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_jacobian )
29			{
30			using namespace Eigen;
31			using namespace pinocchio;
32
33	✓✗	4	pinocchio::Model model;
34	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
35	✓✗	4	pinocchio::Data data(model);
36
37	✓✗✓✗	4	VectorXd q = VectorXd::Zero(model.nq);
38	✓✗	2	computeJointJacobians(model,data,q);
39
40	✓✗✓✗ ✗✓✗✗ ✗✗✗✓ ✗✓✗✗ ✗✗	2	Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
41	✓✗✓✗	4	Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
42	✓✗	2	getJointJacobian(model,data,idx,WORLD,Jrh);
43
44			/* Test Jq == v /
45	✓✗✓✗	4	VectorXd qdot = VectorXd::Random(model.nv);
46	✓✗✓✗	4	VectorXd qddot = VectorXd::Zero(model.nv);
47	✓✗	2	rnea( model,data,q,qdot,qddot );
48	✓✗	2	Motion v = data.oMi[idx].act( data.v[idx] );
49	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v.toVector().isApprox(Jrh*qdot,1e-12));
50
51
52			/* Test local jacobian: rhJrh == rhXo oJrh */
53	✓✗✓✗	4	Data::Matrix6x rhJrh(6,model.nv); rhJrh.fill(0);
54	✓✗	2	getJointJacobian(model,data,idx,LOCAL,rhJrh);
55	✓✗	4	Data::Matrix6x XJrh(6,model.nv);
56	✓✗✓✗	2	motionSet::se3Action( data.oMi[idx].inverse(), Jrh,XJrh );
57	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
58
59	✓✗	2	XJrh.setZero();
60	✓✗	4	Data data_jointJacobian(model);
61	✓✗	2	computeJointJacobian(model,data_jointJacobian,q,idx,XJrh);
62	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(XJrh.isApprox(rhJrh,1e-12));
63
64			/* Test computeJointJacobians with pre-computation of the forward kinematics */
65	✓✗	4	Data data_fk(model);
66	✓✗	2	forwardKinematics(model, data_fk, q);
67	✓✗	2	computeJointJacobians(model, data_fk);
68
69	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(data_fk.J.isApprox(data.J));
70
71		2	}
72
73	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_jacobian_time_variation )
74			{
75			using namespace Eigen;
76			using namespace pinocchio;
77
78	✓✗	4	pinocchio::Model model;
79	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
80	✓✗	4	pinocchio::Data data(model);
81	✓✗	4	pinocchio::Data data_ref(model);
82
83	✓✗✓✗ ✓✗✓✗	4	VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
84	✓✗✓✗	4	VectorXd v = VectorXd::Random(model.nv);
85	✓✗✓✗	4	VectorXd a = VectorXd::Random(model.nv);
86
87	✓✗	2	computeJointJacobiansTimeVariation(model,data,q,v);
88
89	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(isFinite(data.dJ));
90
91	✓✗	2	forwardKinematics(model,data_ref,q,v,a);
92	✓✗✓✗ ✗✓✗✗ ✗✗✗✓ ✗✓✗✗ ✗✗	2	Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
93
94	✓✗✓✗	4	Data::Matrix6x J(6,model.nv); J.fill(0.);
95	✓✗✓✗	4	Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
96
97			// Regarding to the WORLD origin
98	✓✗	2	getJointJacobian(model,data,idx,WORLD,J);
99	✓✗	2	getJointJacobianTimeVariation(model,data,idx,WORLD,dJ);
100
101	✓✗✓✗	2	Motion v_idx(J*v);
102	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(v_idx.isApprox(data_ref.oMi[idx].act(data_ref.v[idx])));
103
104	✓✗✓✗ ✓✗✓✗	2	Motion a_idx(Ja + dJv);
105	✓✗	2	const Motion & a_ref = data_ref.oMi[idx].act(data_ref.a[idx]);
106	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(a_idx.isApprox(a_ref));
107
108			// Regarding to the LOCAL frame
109	✓✗	2	getJointJacobian(model,data,idx,LOCAL,J);
110	✓✗	2	getJointJacobianTimeVariation(model,data,idx,LOCAL,dJ);
111
112	✓✗✓✗ ✓✗	2	v_idx = (Motion::Vector6)(J*v);
113	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(v_idx.isApprox(data_ref.v[idx]));
114
115	✓✗✓✗ ✓✗✓✗ ✓✗	2	a_idx = (Motion::Vector6)(Ja + dJv);
116	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(a_idx.isApprox(data_ref.a[idx]));
117
118			// Regarding to the LOCAL_WORLD_ALIGNED frame
119	✓✗	2	getJointJacobian(model,data,idx,LOCAL_WORLD_ALIGNED,J);
120	✓✗	2	getJointJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ);
121
122	✓✗	2	Data::SE3 worldMlocal = data.oMi[idx];
123	✓✗✓✗	2	worldMlocal.translation().setZero();
124
125	✓✗✓✗ ✓✗	2	v_idx = (Motion::Vector6)(J*v);
126	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(v_idx.isApprox(worldMlocal.act(data_ref.v[idx])));
127
128	✓✗✓✗ ✓✗✓✗ ✓✗	2	a_idx = (Motion::Vector6)(Ja + dJv);
129	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓	2	BOOST_CHECK(a_idx.isApprox(worldMlocal.act(data_ref.a[idx])));
130
131			// compare to finite differencies : WORLD
132			{
133	✓✗✓✗	4	Data data_ref(model), data_ref_plus(model);
134
135		2	const double alpha = 1e-8;
136	✓✗	4	Eigen::VectorXd q_plus(model.nq);
137	✓✗✓✗	2	q_plus = integrate(model,q,alpha*v);
138
139	✓✗✓✗	4	Data::Matrix6x J_ref(6,model.nv); J_ref.fill(0.);
140	✓✗	2	computeJointJacobians(model,data_ref,q);
141	✓✗	2	getJointJacobian(model,data_ref,idx,WORLD,J_ref);
142
143	✓✗✓✗	4	Data::Matrix6x J_ref_plus(6,model.nv); J_ref_plus.fill(0.);
144	✓✗	2	computeJointJacobians(model,data_ref_plus,q_plus);
145	✓✗	2	getJointJacobian(model,data_ref_plus,idx,WORLD,J_ref_plus);
146
147	✓✗✓✗	4	Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.fill(0.);
148	✓✗✓✗ ✓✗	2	dJ_ref = (J_ref_plus - J_ref)/alpha;
149
150	✓✗	2	computeJointJacobiansTimeVariation(model,data,q,v);
151	✓✗✓✗	4	Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
152	✓✗	2	getJointJacobianTimeVariation(model,data,idx,WORLD,dJ);
153
154	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dJ.isApprox(dJ_ref,sqrt(alpha)));
155			}
156
157			// compare to finite differencies : LOCAL
158			{
159	✓✗✓✗	4	Data data_ref(model), data_ref_plus(model);
160
161		2	const double alpha = 1e-8;
162	✓✗	4	Eigen::VectorXd q_plus(model.nq);
163	✓✗✓✗	2	q_plus = integrate(model,q,alpha*v);
164
165	✓✗✓✗	4	Data::Matrix6x J_ref(6,model.nv); J_ref.fill(0.);
166	✓✗	2	computeJointJacobians(model,data_ref,q);
167	✓✗	2	getJointJacobian(model,data_ref,idx,LOCAL,J_ref);
168
169	✓✗✓✗	4	Data::Matrix6x J_ref_plus(6,model.nv); J_ref_plus.fill(0.);
170	✓✗	2	computeJointJacobians(model,data_ref_plus,q_plus);
171	✓✗	2	getJointJacobian(model,data_ref_plus,idx,LOCAL,J_ref_plus);
172
173	✓✗✓✗	2	const Data::SE3 M_plus = data_ref.oMi[idx].inverse()*data_ref_plus.oMi[idx];
174
175	✓✗✓✗	4	Data::Matrix6x dJ_ref(6,model.nv); dJ_ref.fill(0.);
176	✓✗✓✗ ✓✗✓✗ ✓✗	2	dJ_ref = (M_plus.toActionMatrix()*J_ref_plus - J_ref)/alpha;
177
178	✓✗	2	computeJointJacobiansTimeVariation(model,data,q,v);
179	✓✗✓✗	4	Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
180	✓✗	2	getJointJacobianTimeVariation(model,data,idx,LOCAL,dJ);
181
182	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(dJ.isApprox(dJ_ref,sqrt(alpha)));
183			}
184		2	}
185
186	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE ( test_timings )
187			{
188			using namespace Eigen;
189			using namespace pinocchio;
190
191	✓✗	4	pinocchio::Model model;
192	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
193	✓✗	4	pinocchio::Data data(model);
194
195		2	long flag = BOOST_BINARY(1111);
196	✓✗	4	PinocchioTicToc timer(PinocchioTicToc::US);
197			#ifdef NDEBUG
198			#ifdef _INTENSE_TESTING_
199			const size_t NBT = 1000*1000;
200			#else
201			const size_t NBT = 10;
202			#endif
203			#else
204		2	const size_t NBT = 1;
205	✓✗	2	std::cout << "(the time score in debug mode is not relevant) " ;
206			#endif
207
208		2	bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1.
209	✓✗✓✗ ✓✗	2	if(verbose) std::cout <<"--" << std::endl;
210	✓✗✓✗	4	Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);
211
212	✓✗	2	if( flag >> 0 & 1 )
213			{
214	✓✗	2	timer.tic();
215	✓✓	4	SMOOTH(NBT)
216			{
217	✓✗	2	computeJointJacobians(model,data,q);
218			}
219	✓✗✓✗	2	if(verbose) std::cout << "Compute =\t";
220	✓✗	2	timer.toc(std::cout,NBT);
221			}
222
223	✓✗	2	if( flag >> 1 & 1 )
224			{
225	✓✗	2	computeJointJacobians(model,data,q);
226	✓✗✓✗ ✗✓✗✗ ✗✗✗✓ ✗✓✗✗ ✗✗	2	Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
227	✓✗✓✗	4	Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
228
229	✓✗	2	timer.tic();
230	✓✓	4	SMOOTH(NBT)
231			{
232	✓✗	2	getJointJacobian(model,data,idx,WORLD,Jrh);
233			}
234	✓✗✓✗	2	if(verbose) std::cout << "Copy =\t";
235	✓✗	2	timer.toc(std::cout,NBT);
236			}
237
238	✓✗	2	if( flag >> 2 & 1 )
239			{
240	✓✗	2	computeJointJacobians(model,data,q);
241	✓✗✓✗ ✗✓✗✗ ✗✗✗✓ ✗✓✗✗ ✗✗	2	Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
242	✓✗✓✗	4	Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
243
244	✓✗	2	timer.tic();
245	✓✓	4	SMOOTH(NBT)
246			{
247	✓✗	2	getJointJacobian(model,data,idx,LOCAL,Jrh);
248			}
249	✓✗✓✗	2	if(verbose) std::cout << "Change frame =\t";
250	✓✗	2	timer.toc(std::cout,NBT);
251			}
252
253	✓✗	2	if( flag >> 3 & 1 )
254			{
255	✓✗	2	computeJointJacobians(model,data,q);
256	✓✗✓✗ ✗✓✗✗ ✗✗✗✓ ✗✓✗✗ ✗✗	2	Model::Index idx = model.existJointName("rarm6")?model.getJointId("rarm6"):(Model::Index)(model.njoints-1);
257	✓✗✓✗	4	Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
258
259	✓✗	2	timer.tic();
260	✓✓	4	SMOOTH(NBT)
261			{
262	✓✗	2	computeJointJacobian(model,data,q,idx,Jrh);
263			}
264	✓✗✓✗	2	if(verbose) std::cout << "Single jacobian =\t";
265	✓✗	2	timer.toc(std::cout,NBT);
266			}
267		2	}
268
269			BOOST_AUTO_TEST_SUITE_END ()