GCC Code Coverage Report

Directory: ./
File: unittest/finite-differences.cpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 98 98 100.0%
Branches: 188 380 49.5%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2019 CNRS INRIA
3 //
4
5 #include "pinocchio/multibody/model.hpp"
6 #include "pinocchio/multibody/data.hpp"
7 #include "pinocchio/multibody/sample-models.hpp"
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9 #include "pinocchio/algorithm/kinematics.hpp"
10 #include "pinocchio/algorithm/jacobian.hpp"
11
12 #include <iostream>
13 #include <boost/test/unit_test.hpp>
14 #include <boost/utility/binary.hpp>
15
16 using namespace pinocchio;
17 using namespace Eigen;
18
19 template<bool local>
20 4 Data::Matrix6x finiteDiffJacobian(
21 const Model & model, Data & data, const Eigen::VectorXd & q, const Model::JointIndex joint_id)
22 {
23
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 Data::Matrix6x res(6, model.nv);
24
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 res.setZero();
25
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 VectorXd q_integrate(model.nq);
26
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 VectorXd v_integrate(model.nv);
27
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 v_integrate.setZero();
28
29
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 forwardKinematics(model, data, q);
30
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 const SE3 oMi_ref = data.oMi[joint_id];
31
32 4 double eps = 1e-8;
33
2/2
✓ Branch 0 taken 64 times.
✓ Branch 1 taken 2 times.
 132 for (int k = 0; k < model.nv; ++k)
34 {
35 // Integrate along kth direction
36
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 128 v_integrate[k] = eps;
37
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 128 q_integrate = integrate(model, q, v_integrate);
38
39
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 128 forwardKinematics(model, data, q_integrate);
40 128 const SE3 & oMi = data.oMi[joint_id];
41
42 if (local)
43
6/12
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 32 times.
✗ Branch 17 not taken.
 64 res.col(k) = log6(oMi_ref.inverse() * oMi).toVector();
44 else
45
7/14
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 32 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 32 times.
✗ Branch 20 not taken.
 64 res.col(k) = oMi_ref.act(log6(oMi_ref.inverse() * oMi)).toVector();
46
47
2/4
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
 128 res.col(k) /= eps;
48
49
1/2
✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
 128 v_integrate[k] = 0.;
50 }
51
52 8 return res;
53 4 }
54
55 template<typename Matrix>
56 void filterValue(MatrixBase<Matrix> & mat, typename Matrix::Scalar value)
57 {
58 for (int k = 0; k < mat.size(); ++k)
59 mat.derived().data()[k] =
60 math::fabs(mat.derived().data()[k]) <= value ? 0 : mat.derived().data()[k];
61 }
62
63 template<typename JointModel_>
64 struct init;
65
66 template<typename JointModel_>
67 struct init
68 {
69 28 static JointModel_ run()
70 {
71 28 JointModel_ jmodel;
72 28 jmodel.setIndexes(0, 0, 0);
73 28 return jmodel;
74 }
75 };
76
77 template<typename Scalar, int Options>
78 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options>>
79 {
80 typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar, Options> JointModel;
81
82 1 static JointModel run()
83 {
84 typedef typename JointModel::Vector3 Vector3;
85
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(Vector3::Random().normalized());
86
87 1 jmodel.setIndexes(0, 0, 0);
88 1 return jmodel;
89 }
90 };
91
92 template<typename Scalar, int Options>
93 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options>>
94 {
95 typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> JointModel;
96
97 1 static JointModel run()
98 {
99 typedef typename JointModel::Vector3 Vector3;
100
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(Vector3::Random().normalized());
101
102 1 jmodel.setIndexes(0, 0, 0);
103 1 return jmodel;
104 }
105 };
106
107 template<typename Scalar, int Options>
108 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options>>
109 {
110 typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar, Options> JointModel;
111
112 1 static JointModel run()
113 {
114 typedef typename JointModel::Vector3 Vector3;
115
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(Vector3::Random().normalized());
116
117 1 jmodel.setIndexes(0, 0, 0);
118 1 return jmodel;
119 }
120 };
121
122 template<typename Scalar, int Options>
123 struct init<pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
124 {
125 typedef pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options> JointModel;
126
127 1 static JointModel run()
128 {
129 typedef typename JointModel::Vector3 Vector3;
130
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(Vector3::Random().normalized());
131
132 1 jmodel.setIndexes(0, 0, 0);
133 1 return jmodel;
134 }
135 };
136
137 template<typename Scalar, int Options>
138 struct init<pinocchio::JointModelUniversalTpl<Scalar, Options>>
139 {
140 typedef pinocchio::JointModelUniversalTpl<Scalar, Options> JointModel;
141
142 1 static JointModel run()
143 {
144 typedef typename JointModel::Vector3 Vector3;
145
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 JointModel jmodel(XAxis::vector(), YAxis::vector());
146
147 1 jmodel.setIndexes(0, 0, 0);
148 1 return jmodel;
149 }
150 };
151
152 template<typename Scalar, int Options, int axis>
153 struct init<pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
154 {
155 typedef pinocchio::JointModelHelicalTpl<Scalar, Options, axis> JointModel;
156
157 6 static JointModel run()
158 {
159
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 JointModel jmodel(static_cast<Scalar>(0.5));
160
161 6 jmodel.setIndexes(0, 0, 0);
162 6 return jmodel;
163 }
164 };
165
166 template<typename Scalar, int Options, template<typename, int> class JointCollection>
167 struct init<pinocchio::JointModelTpl<Scalar, Options, JointCollection>>
168 {
169 typedef pinocchio::JointModelTpl<Scalar, Options, JointCollection> JointModel;
170
171 static JointModel run()
172 {
173 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
174 JointModel jmodel((JointModelRX()));
175
176 jmodel.setIndexes(0, 0, 0);
177 return jmodel;
178 }
179 };
180
181 template<typename Scalar, int Options, template<typename, int> class JointCollection>
182 struct init<pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection>>
183 {
184 typedef pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollection> JointModel;
185
186 static JointModel run()
187 {
188 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
189 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 1> JointModelRY;
190 JointModel jmodel((JointModelRX()));
191 jmodel.addJoint(JointModelRY());
192
193 jmodel.setIndexes(0, 0, 0);
194 return jmodel;
195 }
196 };
197
198 template<typename Scalar, int Options, template<typename, int> class JointCollection>
199 struct init<pinocchio::JointModelMimicTpl<Scalar, Options, JointCollection>>
200 {
201 typedef pinocchio::JointModelMimicTpl<Scalar, Options, JointCollection> JointModel;
202
203 static JointModel run()
204 {
205 typedef pinocchio::JointModelRevoluteTpl<Scalar, Options, 0> JointModelRX;
206 JointModelRX jmodel_ref = init<JointModelRX>::run();
207
208 JointModel jmodel(jmodel_ref, 1., 0.);
209 jmodel.setIndexes(0, 0, 0, 0);
210
211 return jmodel;
212 }
213 };
214
215 struct FiniteDiffJoint
216 {
217 1 void operator()(JointModelComposite & /*jmodel*/) const
218 {
219 1 }
220
221 1 void operator()(JointModelMimic & /*jmodel*/) const
222 {
223 1 }
224
225 template<typename JointModel>
226 44 void operator()(JointModelBase<JointModel> & /*jmodel*/) const
227 {
228 typedef typename JointModel::ConfigVector_t CV;
229 typedef typename JointModel::TangentVector_t TV;
230 typedef typename LieGroup<JointModel>::type LieGroupType;
231
232
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 JointModel jmodel = init<JointModel>::run();
233
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 44 std::cout << "name: " << jmodel.classname() << std::endl;
234
235
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 typename JointModel::JointDataDerived jdata_ = jmodel.createData();
236 typedef JointDataBase<typename JointModel::JointDataDerived> DataBaseType;
237 44 DataBaseType & jdata = static_cast<DataBaseType &>(jdata_);
238
239
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
 44 CV q = LieGroupType().random();
240
1/2
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
 44 jmodel.calc(jdata.derived(), q);
241
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
 44 SE3 M_ref(jdata.M());
242
243
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 CV q_int(q);
244
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
 44 const Eigen::DenseIndex nv = jdata.S().nv();
245
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 TV v(nv);
246
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 44 v.setZero();
247 44 double eps = 1e-8;
248
249
4/8
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 44 Eigen::Matrix<double, 6, JointModel::NV> S(6, nv), S_ref(jdata.S().matrix());
250
251
2/2
✓ Branch 0 taken 36 times.
✓ Branch 1 taken 22 times.
 116 for (int k = 0; k < nv; ++k)
252 {
253
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 v[k] = eps;
254
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 72 q_int = LieGroupType().integrate(q, v);
255
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 72 jmodel.calc(jdata.derived(), q_int);
256
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 72 SE3 M_int = jdata.M();
257
258
6/12
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 36 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 36 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 36 times.
✗ Branch 17 not taken.
 72 S.col(k) = log6(M_ref.inverse() * M_int).toVector();
259
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 72 S.col(k) /= eps;
260
261
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 72 v[k] = 0.;
262 }
263
264
7/14
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 22 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 22 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 22 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 22 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 22 times.
 44 BOOST_CHECK(S.isApprox(S_ref, eps * 1e1));
265
3/6
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
 44 std::cout << "S_ref:\n" << S_ref << std::endl;
266
3/6
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
 44 std::cout << "S:\n" << S << std::endl;
267 44 }
268 };
269
270 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
271
272
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_S_finit_diff)
273 {
274 2 boost::mpl::for_each<JointModelVariant::types>(FiniteDiffJoint());
275 2 }
276
277
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_jacobian_vs_finit_diff)
278 {
279
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
280
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model);
281
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data(model);
282
283
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 VectorXd q = VectorXd::Ones(model.nq);
284
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 q.segment<4>(3).normalize();
285
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data, q);
286
287 Model::Index idx =
288
5/18
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
 2 model.existJointName("rarm2") ? model.getJointId("rarm2") : (Model::Index)(model.njoints - 1);
289
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x Jrh(6, model.nv);
290
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Jrh.fill(0);
291
292
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data, idx, WORLD, Jrh);
293
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model, data, q, idx);
294
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh, sqrt(1e-8)));
295
296
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data, idx, LOCAL, Jrh);
297
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Jrh_finite_diff = finiteDiffJacobian<true>(model, data, q, idx);
298
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
 2 BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh, sqrt(1e-8)));
299 2 }
300
301 BOOST_AUTO_TEST_SUITE_END()
302