GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/frames-derivatives.cpp Lines: 237 237 100.0 %
Date: 2024-04-26 13:14:21 Branches: 763 1524 50.1 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2020 INRIA
3
//
4
5
#include "pinocchio/algorithm/jacobian.hpp"
6
#include "pinocchio/algorithm/joint-configuration.hpp"
7
#include "pinocchio/algorithm/kinematics.hpp"
8
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
9
#include "pinocchio/algorithm/frames.hpp"
10
#include "pinocchio/algorithm/frames-derivatives.hpp"
11
#include "pinocchio/parsers/sample-models.hpp"
12
13
#include <iostream>
14
15
#include <boost/test/unit_test.hpp>
16
#include <boost/utility/binary.hpp>
17
18
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
19
20
















4
BOOST_AUTO_TEST_CASE(test_frames_derivatives_velocity)
21
{
22
  using namespace Eigen;
23
  using namespace pinocchio;
24
25
4
  Model model;
26
2
  buildModels::humanoidRandom(model);
27
28




2
  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
29


6
  Frame frame("rand",jointId,0,SE3::Random(),OP_FRAME);
30
2
  FrameIndex frameId = model.addFrame(frame);
31
32




2
  BOOST_CHECK(model.getFrameId("rand") == frameId);
33



2
  BOOST_CHECK(model.frames[frameId].parent == jointId);
34
35

4
  Data data(model), data_ref(model);
36
37

2
  model.lowerPositionLimit.head<3>().fill(-1.);
38

2
  model.upperPositionLimit.head<3>().fill(1.);
39
4
  VectorXd q = randomConfiguration(model);
40

4
  VectorXd v(VectorXd::Random(model.nv));
41

4
  VectorXd a(VectorXd::Random(model.nv));
42
43
2
  computeForwardKinematicsDerivatives(model,data,q,v,a);
44
45

4
  Data::Matrix6x partial_dq(6,model.nv); partial_dq.setZero();
46

4
  Data::Matrix6x partial_dq_local_world_aligned(6,model.nv); partial_dq_local_world_aligned.setZero();
47

4
  Data::Matrix6x partial_dq_local(6,model.nv); partial_dq_local.setZero();
48

4
  Data::Matrix6x partial_dv(6,model.nv); partial_dv.setZero();
49

4
  Data::Matrix6x partial_dv_local_world_aligned(6,model.nv); partial_dv_local_world_aligned.setZero();
50

4
  Data::Matrix6x partial_dv_local(6,model.nv); partial_dv_local.setZero();
51
52
2
  getFrameVelocityDerivatives(model,data,frameId,WORLD,
53
                              partial_dq,partial_dv);
54
55
2
  getFrameVelocityDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
56
                              partial_dq_local_world_aligned,partial_dv_local_world_aligned);
57
58
2
  getFrameVelocityDerivatives(model,data,frameId,LOCAL,
59
                              partial_dq_local,partial_dv_local);
60
61

4
  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
62

4
  Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero();
63

4
  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
64
2
  computeJointJacobians(model,data_ref,q);
65
2
  getFrameJacobian(model,data_ref,frameId,WORLD,J_ref);
66
2
  getFrameJacobian(model,data_ref,frameId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
67
2
  getFrameJacobian(model,data_ref,frameId,LOCAL,J_ref_local);
68
69



2
  BOOST_CHECK(data_ref.oMf[frameId].isApprox(data.oMf[frameId]));
70



2
  BOOST_CHECK(partial_dv.isApprox(J_ref));
71



2
  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned));
72



2
  BOOST_CHECK(partial_dv_local.isApprox(J_ref_local));
73
74
  // Check against finite differences
75

4
  Data::Matrix6x partial_dq_fd(6,model.nv); partial_dq_fd.setZero();
76

4
  Data::Matrix6x partial_dq_fd_local_world_aligned(6,model.nv); partial_dq_fd_local_world_aligned.setZero();
77

4
  Data::Matrix6x partial_dq_fd_local(6,model.nv); partial_dq_fd_local.setZero();
78

4
  Data::Matrix6x partial_dv_fd(6,model.nv); partial_dv_fd.setZero();
79

4
  Data::Matrix6x partial_dv_fd_local_world_aligned(6,model.nv); partial_dv_fd_local_world_aligned.setZero();
80

4
  Data::Matrix6x partial_dv_fd_local(6,model.nv); partial_dv_fd_local.setZero();
81
2
  const double alpha = 1e-8;
82
83
  // dvel/dv
84
4
  Eigen::VectorXd v_plus(v);
85
4
  Data data_plus(model);
86
2
  forwardKinematics(model,data_ref,q,v);
87
2
  Motion v0 = getFrameVelocity(model,data,frameId,WORLD);
88
2
  Motion v0_local_world_aligned = getFrameVelocity(model,data,frameId,LOCAL_WORLD_ALIGNED);
89
2
  Motion v0_local = getFrameVelocity(model,data,frameId,LOCAL);
90
66
  for(int k = 0; k < model.nv; ++k)
91
  {
92
64
    v_plus[k] += alpha;
93
64
    forwardKinematics(model,data_plus,q,v_plus);
94
95



64
    partial_dv_fd.col(k) = (getFrameVelocity(model,data_plus,frameId,WORLD) - v0).toVector()/alpha;
96



64
    partial_dv_fd_local_world_aligned.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - v0_local_world_aligned).toVector()/alpha;
97



64
    partial_dv_fd_local.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL) - v0_local).toVector()/alpha;
98
64
    v_plus[k] -= alpha;
99
  }
100
101



2
  BOOST_CHECK(partial_dv.isApprox(partial_dv_fd,sqrt(alpha)));
102



2
  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned,sqrt(alpha)));
103



2
  BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local,sqrt(alpha)));
104
105
  // dvel/dq
106

4
  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
107
2
  forwardKinematics(model,data_ref,q,v);
108
2
  updateFramePlacements(model,data_ref);
109
110
66
  for(int k = 0; k < model.nv; ++k)
111
  {
112
64
    v_eps[k] += alpha;
113
64
    q_plus = integrate(model,q,v_eps);
114
64
    forwardKinematics(model,data_plus,q_plus,v);
115
64
    updateFramePlacements(model,data_plus);
116
117
64
    Motion v_plus_local_world_aligned = getFrameVelocity(model,data_plus,frameId,LOCAL_WORLD_ALIGNED);
118


64
    SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
119


64
    v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans);
120



64
    partial_dq_fd.col(k) = (getFrameVelocity(model,data_plus,frameId,WORLD) - v0).toVector()/alpha;
121


64
    partial_dq_fd_local_world_aligned.col(k) = (v_plus_local_world_aligned - v0_local_world_aligned).toVector()/alpha;
122



64
    partial_dq_fd_local.col(k) = (getFrameVelocity(model,data_plus,frameId,LOCAL) - v0_local).toVector()/alpha;
123
64
    v_eps[k] -= alpha;
124
  }
125
126



2
  BOOST_CHECK(partial_dq.isApprox(partial_dq_fd,sqrt(alpha)));
127



2
  BOOST_CHECK(partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned,sqrt(alpha)));
128



2
  BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha)));
129
130
2
}
131
132
















4
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
133
{
134
  using namespace Eigen;
135
  using namespace pinocchio;
136
137
4
  Model model;
138
2
  buildModels::humanoidRandom(model);
139
140




2
  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
141


6
  Frame frame("rand",jointId,0,SE3::Random(),OP_FRAME);
142
2
  FrameIndex frameId = model.addFrame(frame);
143
144




2
  BOOST_CHECK(model.getFrameId("rand") == frameId);
145



2
  BOOST_CHECK(model.frames[frameId].parent == jointId);
146
147

4
  Data data(model), data_ref(model);
148
149

2
  model.lowerPositionLimit.head<3>().fill(-1.);
150

2
  model.upperPositionLimit.head<3>().fill(1.);
151
4
  VectorXd q = randomConfiguration(model);
152

4
  VectorXd v(VectorXd::Random(model.nv));
153

4
  VectorXd a(VectorXd::Random(model.nv));
154
155
2
  computeForwardKinematicsDerivatives(model,data,q,v,a);
156
157

4
  Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
158

4
  Data::Matrix6x v_partial_dq_local(6,model.nv); v_partial_dq_local.setZero();
159

4
  Data::Matrix6x v_partial_dq_local_world_aligned(6,model.nv); v_partial_dq_local_world_aligned.setZero();
160

4
  Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
161

4
  Data::Matrix6x a_partial_dq_local_world_aligned(6,model.nv); a_partial_dq_local_world_aligned.setZero();
162

4
  Data::Matrix6x a_partial_dq_local(6,model.nv); a_partial_dq_local.setZero();
163

4
  Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
164

4
  Data::Matrix6x a_partial_dv_local_world_aligned(6,model.nv); a_partial_dv_local_world_aligned.setZero();
165

4
  Data::Matrix6x a_partial_dv_local(6,model.nv); a_partial_dv_local.setZero();
166

4
  Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();
167

4
  Data::Matrix6x a_partial_da_local_world_aligned(6,model.nv); a_partial_da_local_world_aligned.setZero();
168

4
  Data::Matrix6x a_partial_da_local(6,model.nv); a_partial_da_local.setZero();
169
170
2
  getFrameAccelerationDerivatives(model,data,frameId,WORLD,
171
                                  v_partial_dq,
172
                                  a_partial_dq,a_partial_dv,a_partial_da);
173
174
2
  getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
175
                                  v_partial_dq_local_world_aligned,
176
                                  a_partial_dq_local_world_aligned,
177
                                  a_partial_dv_local_world_aligned,
178
                                  a_partial_da_local_world_aligned);
179
180
2
  getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
181
                                  v_partial_dq_local,
182
                                  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);
183
184
  // Check v_partial_dq against getFrameVelocityDerivatives
185
  {
186
4
    Data data_v(model);
187
2
    computeForwardKinematicsDerivatives(model,data_v,q,v,a);
188
189

4
    Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
190

4
    Data::Matrix6x v_partial_dq_ref_local_world_aligned(6,model.nv); v_partial_dq_ref_local_world_aligned.setZero();
191

4
    Data::Matrix6x v_partial_dq_ref_local(6,model.nv); v_partial_dq_ref_local.setZero();
192

4
    Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
193

4
    Data::Matrix6x v_partial_dv_ref_local_world_aligned(6,model.nv); v_partial_dv_ref_local_world_aligned.setZero();
194

4
    Data::Matrix6x v_partial_dv_ref_local(6,model.nv); v_partial_dv_ref_local.setZero();
195
196
2
    getFrameVelocityDerivatives(model,data_v,frameId,WORLD,
197
                                v_partial_dq_ref,v_partial_dv_ref);
198
199



2
    BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref));
200



2
    BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref));
201
202
2
    getFrameVelocityDerivatives(model,data_v,frameId,LOCAL_WORLD_ALIGNED,
203
                                v_partial_dq_ref_local_world_aligned,v_partial_dv_ref_local_world_aligned);
204
205



2
    BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned));
206



2
    BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned));
207
208
2
    getFrameVelocityDerivatives(model,data_v,frameId,LOCAL,
209
                                v_partial_dq_ref_local,v_partial_dv_ref_local);
210
211



2
    BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
212



2
    BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local));
213
  }
214
215

4
  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
216

4
  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
217

4
  Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero();
218
2
  computeJointJacobians(model,data_ref,q);
219
2
  getFrameJacobian(model,data_ref,frameId,WORLD,J_ref);
220
2
  getFrameJacobian(model,data_ref,frameId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
221
2
  getFrameJacobian(model,data_ref,frameId,LOCAL,J_ref_local);
222
223



2
  BOOST_CHECK(a_partial_da.isApprox(J_ref));
224



2
  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned));
225



2
  BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local));
226
227
  // Check against finite differences
228

4
  Data::Matrix6x a_partial_da_fd(6,model.nv); a_partial_da_fd.setZero();
229

4
  Data::Matrix6x a_partial_da_fd_local_world_aligned(6,model.nv); a_partial_da_fd_local_world_aligned.setZero();
230

4
  Data::Matrix6x a_partial_da_fd_local(6,model.nv); a_partial_da_fd_local.setZero();
231
2
  const double alpha = 1e-8;
232
233

4
  Eigen::VectorXd v_plus(v), a_plus(a);
234
4
  Data data_plus(model);
235
2
  forwardKinematics(model,data_ref,q,v,a);
236
237
  // dacc/da
238
2
  Motion a0 = getFrameAcceleration(model,data,frameId,WORLD);
239
2
  Motion a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED);
240
2
  Motion a0_local = getFrameAcceleration(model,data,frameId,LOCAL);
241
66
  for(int k = 0; k < model.nv; ++k)
242
  {
243
64
    a_plus[k] += alpha;
244
64
    forwardKinematics(model,data_plus,q,v,a_plus);
245
246



64
    a_partial_da_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;
247



64
    a_partial_da_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha;
248



64
    a_partial_da_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
249
64
    a_plus[k] -= alpha;
250
  }
251



2
  BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd,sqrt(alpha)));
252



2
  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned,sqrt(alpha)));
253



2
  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha)));
254
255
  // dacc/dv
256

4
  Data::Matrix6x a_partial_dv_fd(6,model.nv); a_partial_dv_fd.setZero();
257

4
  Data::Matrix6x a_partial_dv_fd_local_world_aligned(6,model.nv); a_partial_dv_fd_local_world_aligned.setZero();
258

4
  Data::Matrix6x a_partial_dv_fd_local(6,model.nv); a_partial_dv_fd_local.setZero();
259
66
  for(int k = 0; k < model.nv; ++k)
260
  {
261
64
    v_plus[k] += alpha;
262
64
    forwardKinematics(model,data_plus,q,v_plus,a);
263
264






64
    a_partial_dv_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;    a_partial_dv_fd_local_world_aligned.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED) - a0_local_world_aligned).toVector()/alpha;
265



64
    a_partial_dv_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
266
64
    v_plus[k] -= alpha;
267
  }
268
269



2
  BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd,sqrt(alpha)));
270



2
  BOOST_CHECK(a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned,sqrt(alpha)));
271



2
  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha)));
272
273
  // dacc/dq
274
2
  a_partial_dq.setZero();
275
2
  a_partial_dv.setZero();
276
2
  a_partial_da.setZero();
277
278
2
  a_partial_dq_local_world_aligned.setZero();
279
2
  a_partial_dv_local_world_aligned.setZero();
280
2
  a_partial_da_local_world_aligned.setZero();
281
282
2
  a_partial_dq_local.setZero();
283
2
  a_partial_dv_local.setZero();
284
2
  a_partial_da_local.setZero();
285
286

4
  Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
287

4
  Data::Matrix6x a_partial_dq_fd_local_world_aligned(6,model.nv); a_partial_dq_fd_local_world_aligned.setZero();
288

4
  Data::Matrix6x a_partial_dq_fd_local(6,model.nv); a_partial_dq_fd_local.setZero();
289
290
2
  computeForwardKinematicsDerivatives(model,data,q,v,a);
291
2
  getFrameAccelerationDerivatives(model,data,frameId,WORLD,
292
                                  v_partial_dq,
293
                                  a_partial_dq,a_partial_dv,a_partial_da);
294
295
2
  getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
296
                                  v_partial_dq_local_world_aligned,
297
                                  a_partial_dq_local_world_aligned,
298
                                  a_partial_dv_local_world_aligned,
299
                                  a_partial_da_local_world_aligned);
300
301
2
  getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
302
                                  v_partial_dq_local,
303
                                  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);
304
305

4
  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
306
2
  forwardKinematics(model,data_ref,q,v,a);
307
2
  updateFramePlacements(model,data_ref);
308
2
  a0 = getFrameAcceleration(model,data,frameId,WORLD);
309
2
  a0_local_world_aligned = getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED);
310
2
  a0_local = getFrameAcceleration(model,data,frameId,LOCAL);
311
312
66
  for(int k = 0; k < model.nv; ++k)
313
  {
314
64
    v_eps[k] += alpha;
315
64
    q_plus = integrate(model,q,v_eps);
316
64
    forwardKinematics(model,data_plus,q_plus,v,a);
317
64
    updateFramePlacements(model,data_plus);
318
319



64
    a_partial_dq_fd.col(k) = (getFrameAcceleration(model,data_plus,frameId,WORLD) - a0).toVector()/alpha;
320
64
    Motion a_plus_local_world_aligned = getFrameAcceleration(model,data_plus,frameId,LOCAL_WORLD_ALIGNED);
321


64
    const SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
322


64
    a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans);
323


64
    a_partial_dq_fd_local_world_aligned.col(k) = (a_plus_local_world_aligned - a0_local_world_aligned).toVector()/alpha;
324



64
    a_partial_dq_fd_local.col(k) = (getFrameAcceleration(model,data_plus,frameId,LOCAL) - a0_local).toVector()/alpha;
325
64
    v_eps[k] -= alpha;
326
  }
327
328



2
  BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd,sqrt(alpha)));
329



2
  BOOST_CHECK(a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned,sqrt(alpha)));
330



2
  BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local,sqrt(alpha)));
331
332
  // Test other signatures
333

4
  Data::Matrix6x v_partial_dq_other(6,model.nv); v_partial_dq_other.setZero();
334

4
  Data::Matrix6x v_partial_dq_local_other(6,model.nv); v_partial_dq_local_other.setZero();
335

4
  Data::Matrix6x v_partial_dq_local_world_aligned_other(6,model.nv); v_partial_dq_local_world_aligned_other.setZero();
336

4
  Data::Matrix6x v_partial_dv_other(6,model.nv); v_partial_dv_other.setZero();
337

4
  Data::Matrix6x v_partial_dv_local_other(6,model.nv); v_partial_dv_local_other.setZero();
338

4
  Data::Matrix6x v_partial_dv_local_world_aligned_other(6,model.nv); v_partial_dv_local_world_aligned_other.setZero();
339

4
  Data::Matrix6x a_partial_dq_other(6,model.nv); a_partial_dq_other.setZero();
340

4
  Data::Matrix6x a_partial_dq_local_world_aligned_other(6,model.nv); a_partial_dq_local_world_aligned_other.setZero();
341

4
  Data::Matrix6x a_partial_dq_local_other(6,model.nv); a_partial_dq_local_other.setZero();
342

4
  Data::Matrix6x a_partial_dv_other(6,model.nv); a_partial_dv_other.setZero();
343

4
  Data::Matrix6x a_partial_dv_local_world_aligned_other(6,model.nv); a_partial_dv_local_world_aligned_other.setZero();
344

4
  Data::Matrix6x a_partial_dv_local_other(6,model.nv); a_partial_dv_local_other.setZero();
345

4
  Data::Matrix6x a_partial_da_other(6,model.nv); a_partial_da_other.setZero();
346

4
  Data::Matrix6x a_partial_da_local_world_aligned_other(6,model.nv); a_partial_da_local_world_aligned_other.setZero();
347

4
  Data::Matrix6x a_partial_da_local_other(6,model.nv); a_partial_da_local_other.setZero();
348
349
2
  getFrameAccelerationDerivatives(model,data,frameId,WORLD,
350
                                  v_partial_dq_other,
351
                                  v_partial_dv_other,
352
                                  a_partial_dq_other,
353
                                  a_partial_dv_other,
354
                                  a_partial_da_other);
355
356



2
  BOOST_CHECK(v_partial_dq_other.isApprox(v_partial_dq));
357



2
  BOOST_CHECK(v_partial_dv_other.isApprox(a_partial_da));
358



2
  BOOST_CHECK(a_partial_dq_other.isApprox(a_partial_dq));
359



2
  BOOST_CHECK(a_partial_dv_other.isApprox(a_partial_dv));
360



2
  BOOST_CHECK(a_partial_da_other.isApprox(a_partial_da));
361
362
2
  getFrameAccelerationDerivatives(model,data,frameId,LOCAL_WORLD_ALIGNED,
363
                                  v_partial_dq_local_world_aligned_other,
364
                                  v_partial_dv_local_world_aligned_other,
365
                                  a_partial_dq_local_world_aligned_other,
366
                                  a_partial_dv_local_world_aligned_other,
367
                                  a_partial_da_local_world_aligned_other);
368
369



2
  BOOST_CHECK(v_partial_dq_local_world_aligned_other.isApprox(v_partial_dq_local_world_aligned));
370



2
  BOOST_CHECK(v_partial_dv_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));
371



2
  BOOST_CHECK(a_partial_dq_local_world_aligned_other.isApprox(a_partial_dq_local_world_aligned));
372



2
  BOOST_CHECK(a_partial_dv_local_world_aligned_other.isApprox(a_partial_dv_local_world_aligned));
373



2
  BOOST_CHECK(a_partial_da_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));
374
375
2
  getFrameAccelerationDerivatives(model,data,frameId,LOCAL,
376
                                  v_partial_dq_local_other,
377
                                  v_partial_dv_local_other,
378
                                  a_partial_dq_local_other,
379
                                  a_partial_dv_local_other,
380
                                  a_partial_da_local_other);
381
382



2
  BOOST_CHECK(v_partial_dq_local_other.isApprox(v_partial_dq_local));
383



2
  BOOST_CHECK(v_partial_dv_local_other.isApprox(a_partial_da_local));
384



2
  BOOST_CHECK(a_partial_dq_local_other.isApprox(a_partial_dq_local));
385



2
  BOOST_CHECK(a_partial_dv_local_other.isApprox(a_partial_dv_local));
386



2
  BOOST_CHECK(a_partial_da_local_other.isApprox(a_partial_da_local));
387
2
}
388
389
BOOST_AUTO_TEST_SUITE_END()