GCC Code Coverage Report

Directory: ./
File: unittest/finite-differences.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 101 101 100.0%
Branches: 191 386 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 34 static JointModel_ run()
70 {
71 34 JointModel_ jmodel;
72 34 jmodel.setIndexes(0, 0, 0);
73 34 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 JointModel_>
199 struct init<pinocchio::JointModelMimic<JointModel_>>
200 {
201 typedef pinocchio::JointModelMimic<JointModel_> JointModel;
202
203 6 static JointModel run()
204 {
205
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 JointModel_ jmodel_ref = init<JointModel_>::run();
206
207
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 JointModel jmodel(jmodel_ref, 1., 0.);
208
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 jmodel.setIndexes(0, 0, 0);
209
210 12 return jmodel;
211 }
212 };
213
214 struct FiniteDiffJoint
215 {
216 1 void operator()(JointModelComposite & /*jmodel*/) const
217 {
218 1 }
219
220 template<typename JointModel>
221 50 void operator()(JointModelBase<JointModel> & /*jmodel*/) const
222 {
223 typedef typename JointModel::ConfigVector_t CV;
224 typedef typename JointModel::TangentVector_t TV;
225 typedef typename LieGroup<JointModel>::type LieGroupType;
226
227
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 JointModel jmodel = init<JointModel>::run();
228
4/8
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25 times.
✗ Branch 11 not taken.
 50 std::cout << "name: " << jmodel.classname() << std::endl;
229
230
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 typename JointModel::JointDataDerived jdata_ = jmodel.createData();
231 typedef JointDataBase<typename JointModel::JointDataDerived> DataBaseType;
232 50 DataBaseType & jdata = static_cast<DataBaseType &>(jdata_);
233
234
2/4
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 50 CV q = LieGroupType().random();
235
1/2
✓ Branch 2 taken 25 times.
✗ Branch 3 not taken.
 50 jmodel.calc(jdata.derived(), q);
236
2/4
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 50 SE3 M_ref(jdata.M());
237
238
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 CV q_int(q);
239
2/4
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
 50 const Eigen::DenseIndex nv = jdata.S().nv();
240
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 TV v(nv);
241
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 v.setZero();
242 50 double eps = 1e-8;
243
244
4/8
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 50 Eigen::Matrix<double, 6, JointModel::NV> S(6, nv), S_ref(jdata.S().matrix());
245
246
2/2
✓ Branch 0 taken 39 times.
✓ Branch 1 taken 25 times.
 128 for (int k = 0; k < nv; ++k)
247 {
248
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 78 v[k] = eps;
249
2/4
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
 78 q_int = LieGroupType().integrate(q, v);
250
1/2
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
 78 jmodel.calc(jdata.derived(), q_int);
251
2/4
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
 78 SE3 M_int = jdata.M();
252
253
6/12
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 39 times.
✗ Branch 17 not taken.
 78 S.col(k) = log6(M_ref.inverse() * M_int).toVector();
254
2/4
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
 78 S.col(k) /= eps;
255
256
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 78 v[k] = 0.;
257 }
258
259
7/14
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 25 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 25 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 25 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 25 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 25 times.
 50 BOOST_CHECK(S.isApprox(S_ref, eps * 1e1));
260
3/6
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25 times.
✗ Branch 8 not taken.
 50 std::cout << "S_ref:\n" << S_ref << std::endl;
261
3/6
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25 times.
✗ Branch 8 not taken.
 50 std::cout << "S:\n" << S << std::endl;
262 50 }
263 };
264
265 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
266
267
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)
268 {
269 2 boost::mpl::for_each<JointModelVariant::types>(FiniteDiffJoint());
270 2 }
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_jacobian_vs_finit_diff)
273 {
274
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Model model;
275
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::buildModels::humanoidRandom(model);
276
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 pinocchio::Data data(model);
277
278
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);
279
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();
280
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 computeJointJacobians(model, data, q);
281
282 Model::Index idx =
283
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);
284
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x Jrh(6, model.nv);
285
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Jrh.fill(0);
286
287
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data, idx, WORLD, Jrh);
288
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model, data, q, idx);
289
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)));
290
291
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 getJointJacobian(model, data, idx, LOCAL, Jrh);
292
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Jrh_finite_diff = finiteDiffJacobian<true>(model, data, q, idx);
293
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)));
294 2 }
295
296 BOOST_AUTO_TEST_SUITE_END()
297