unittest/frames-derivatives.cpp
Go to the documentation of this file.
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/multibody/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 BOOST_AUTO_TEST_CASE(test_frames_derivatives_velocity)
21 {
22  using namespace Eigen;
23  using namespace pinocchio;
24 
25  Model model;
26  buildModels::humanoidRandom(model);
27 
28  const Model::JointIndex jointId = model.existJointName("rarm2_joint")
29  ? model.getJointId("rarm2_joint")
30  : (Model::Index)(model.njoints - 1);
31  Frame frame("rand", jointId, 0, SE3::Random(), OP_FRAME);
32  FrameIndex frameId = model.addFrame(frame);
33 
34  BOOST_CHECK(model.getFrameId("rand") == frameId);
35  BOOST_CHECK(model.frames[frameId].parentJoint == jointId);
36 
37  Data data(model), data_ref(model);
38 
39  model.lowerPositionLimit.head<3>().fill(-1.);
40  model.upperPositionLimit.head<3>().fill(1.);
41  VectorXd q = randomConfiguration(model);
42  VectorXd v(VectorXd::Random(model.nv));
43  VectorXd a(VectorXd::Random(model.nv));
44 
45  computeForwardKinematicsDerivatives(model, data, q, v, a);
46 
47  Data::Matrix6x partial_dq(6, model.nv);
48  partial_dq.setZero();
49  Data::Matrix6x partial_dq_local_world_aligned(6, model.nv);
50  partial_dq_local_world_aligned.setZero();
51  Data::Matrix6x partial_dq_local(6, model.nv);
52  partial_dq_local.setZero();
53  Data::Matrix6x partial_dv(6, model.nv);
54  partial_dv.setZero();
55  Data::Matrix6x partial_dv_local_world_aligned(6, model.nv);
56  partial_dv_local_world_aligned.setZero();
57  Data::Matrix6x partial_dv_local(6, model.nv);
58  partial_dv_local.setZero();
59 
60  getFrameVelocityDerivatives(model, data, frameId, WORLD, partial_dq, partial_dv);
61 
62  getFrameVelocityDerivatives(
63  model, data, frameId, LOCAL_WORLD_ALIGNED, partial_dq_local_world_aligned,
64  partial_dv_local_world_aligned);
65 
66  getFrameVelocityDerivatives(model, data, frameId, LOCAL, partial_dq_local, partial_dv_local);
67 
68  Data::Matrix6x J_ref(6, model.nv);
69  J_ref.setZero();
70  Data::Matrix6x J_ref_local_world_aligned(6, model.nv);
71  J_ref_local_world_aligned.setZero();
72  Data::Matrix6x J_ref_local(6, model.nv);
73  J_ref_local.setZero();
74  computeJointJacobians(model, data_ref, q);
75  getFrameJacobian(model, data_ref, frameId, WORLD, J_ref);
76  getFrameJacobian(model, data_ref, frameId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned);
77  getFrameJacobian(model, data_ref, frameId, LOCAL, J_ref_local);
78 
79  BOOST_CHECK(data_ref.oMf[frameId].isApprox(data.oMf[frameId]));
80  BOOST_CHECK(partial_dv.isApprox(J_ref));
81  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned));
82  BOOST_CHECK(partial_dv_local.isApprox(J_ref_local));
83 
84  // Check against finite differences
85  Data::Matrix6x partial_dq_fd(6, model.nv);
86  partial_dq_fd.setZero();
87  Data::Matrix6x partial_dq_fd_local_world_aligned(6, model.nv);
88  partial_dq_fd_local_world_aligned.setZero();
89  Data::Matrix6x partial_dq_fd_local(6, model.nv);
90  partial_dq_fd_local.setZero();
91  Data::Matrix6x partial_dv_fd(6, model.nv);
92  partial_dv_fd.setZero();
93  Data::Matrix6x partial_dv_fd_local_world_aligned(6, model.nv);
94  partial_dv_fd_local_world_aligned.setZero();
95  Data::Matrix6x partial_dv_fd_local(6, model.nv);
96  partial_dv_fd_local.setZero();
97  const double alpha = 1e-8;
98 
99  // dvel/dv
100  Eigen::VectorXd v_plus(v);
101  Data data_plus(model);
102  forwardKinematics(model, data_ref, q, v);
103  Motion v0 = getFrameVelocity(model, data, frameId, WORLD);
104  Motion v0_local_world_aligned = getFrameVelocity(model, data, frameId, LOCAL_WORLD_ALIGNED);
105  Motion v0_local = getFrameVelocity(model, data, frameId, LOCAL);
106  for (int k = 0; k < model.nv; ++k)
107  {
108  v_plus[k] += alpha;
109  forwardKinematics(model, data_plus, q, v_plus);
110 
111  partial_dv_fd.col(k) =
112  (getFrameVelocity(model, data_plus, frameId, WORLD) - v0).toVector() / alpha;
113  partial_dv_fd_local_world_aligned.col(k) =
114  (getFrameVelocity(model, data_plus, frameId, LOCAL_WORLD_ALIGNED) - v0_local_world_aligned)
115  .toVector()
116  / alpha;
117  partial_dv_fd_local.col(k) =
118  (getFrameVelocity(model, data_plus, frameId, LOCAL) - v0_local).toVector() / alpha;
119  v_plus[k] -= alpha;
120  }
121 
122  BOOST_CHECK(partial_dv.isApprox(partial_dv_fd, sqrt(alpha)));
123  BOOST_CHECK(
124  partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned, sqrt(alpha)));
125  BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local, sqrt(alpha)));
126 
127  // dvel/dq
128  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
129  forwardKinematics(model, data_ref, q, v);
130  updateFramePlacements(model, data_ref);
131 
132  for (int k = 0; k < model.nv; ++k)
133  {
134  v_eps[k] += alpha;
135  q_plus = integrate(model, q, v_eps);
136  forwardKinematics(model, data_plus, q_plus, v);
137  updateFramePlacements(model, data_plus);
138 
139  Motion v_plus_local_world_aligned =
140  getFrameVelocity(model, data_plus, frameId, LOCAL_WORLD_ALIGNED);
141  SE3::Vector3 trans = data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
142  v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans);
143  partial_dq_fd.col(k) =
144  (getFrameVelocity(model, data_plus, frameId, WORLD) - v0).toVector() / alpha;
145  partial_dq_fd_local_world_aligned.col(k) =
146  (v_plus_local_world_aligned - v0_local_world_aligned).toVector() / alpha;
147  partial_dq_fd_local.col(k) =
148  (getFrameVelocity(model, data_plus, frameId, LOCAL) - v0_local).toVector() / alpha;
149  v_eps[k] -= alpha;
150  }
151 
152  BOOST_CHECK(partial_dq.isApprox(partial_dq_fd, sqrt(alpha)));
153  BOOST_CHECK(
154  partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned, sqrt(alpha)));
155  BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local, sqrt(alpha)));
156 }
157 
158 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
159 {
160  using namespace Eigen;
161  using namespace pinocchio;
162 
163  Model model;
164  buildModels::humanoidRandom(model);
165 
166  const Model::JointIndex jointId = model.existJointName("rarm2_joint")
167  ? model.getJointId("rarm2_joint")
168  : (Model::Index)(model.njoints - 1);
169  Frame frame("rand", jointId, 0, SE3::Random(), OP_FRAME);
170  FrameIndex frameId = model.addFrame(frame);
171 
172  BOOST_CHECK(model.getFrameId("rand") == frameId);
173  BOOST_CHECK(model.frames[frameId].parentJoint == jointId);
174 
175  Data data(model), data_ref(model);
176 
177  model.lowerPositionLimit.head<3>().fill(-1.);
178  model.upperPositionLimit.head<3>().fill(1.);
179  VectorXd q = randomConfiguration(model);
180  VectorXd v(VectorXd::Random(model.nv));
181  VectorXd a(VectorXd::Random(model.nv));
182 
183  computeForwardKinematicsDerivatives(model, data, q, v, a);
184 
185  Data::Matrix6x v_partial_dq(6, model.nv);
186  v_partial_dq.setZero();
187  Data::Matrix6x v_partial_dq_local(6, model.nv);
188  v_partial_dq_local.setZero();
189  Data::Matrix6x v_partial_dq_local_world_aligned(6, model.nv);
190  v_partial_dq_local_world_aligned.setZero();
191  Data::Matrix6x a_partial_dq(6, model.nv);
192  a_partial_dq.setZero();
193  Data::Matrix6x a_partial_dq_local_world_aligned(6, model.nv);
194  a_partial_dq_local_world_aligned.setZero();
195  Data::Matrix6x a_partial_dq_local(6, model.nv);
196  a_partial_dq_local.setZero();
197  Data::Matrix6x a_partial_dv(6, model.nv);
198  a_partial_dv.setZero();
199  Data::Matrix6x a_partial_dv_local_world_aligned(6, model.nv);
200  a_partial_dv_local_world_aligned.setZero();
201  Data::Matrix6x a_partial_dv_local(6, model.nv);
202  a_partial_dv_local.setZero();
203  Data::Matrix6x a_partial_da(6, model.nv);
204  a_partial_da.setZero();
205  Data::Matrix6x a_partial_da_local_world_aligned(6, model.nv);
206  a_partial_da_local_world_aligned.setZero();
207  Data::Matrix6x a_partial_da_local(6, model.nv);
208  a_partial_da_local.setZero();
209 
210  getFrameAccelerationDerivatives(
211  model, data, frameId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
212 
213  getFrameAccelerationDerivatives(
214  model, data, frameId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned,
215  a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned,
216  a_partial_da_local_world_aligned);
217 
218  getFrameAccelerationDerivatives(
219  model, data, frameId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local,
220  a_partial_da_local);
221 
222  // Check v_partial_dq against getFrameVelocityDerivatives
223  {
224  Data data_v(model);
225  computeForwardKinematicsDerivatives(model, data_v, q, v, a);
226 
227  Data::Matrix6x v_partial_dq_ref(6, model.nv);
228  v_partial_dq_ref.setZero();
229  Data::Matrix6x v_partial_dq_ref_local_world_aligned(6, model.nv);
230  v_partial_dq_ref_local_world_aligned.setZero();
231  Data::Matrix6x v_partial_dq_ref_local(6, model.nv);
232  v_partial_dq_ref_local.setZero();
233  Data::Matrix6x v_partial_dv_ref(6, model.nv);
234  v_partial_dv_ref.setZero();
235  Data::Matrix6x v_partial_dv_ref_local_world_aligned(6, model.nv);
236  v_partial_dv_ref_local_world_aligned.setZero();
237  Data::Matrix6x v_partial_dv_ref_local(6, model.nv);
238  v_partial_dv_ref_local.setZero();
239 
240  getFrameVelocityDerivatives(model, data_v, frameId, WORLD, v_partial_dq_ref, v_partial_dv_ref);
241 
242  BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref));
243  BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref));
244 
245  getFrameVelocityDerivatives(
246  model, data_v, frameId, LOCAL_WORLD_ALIGNED, v_partial_dq_ref_local_world_aligned,
247  v_partial_dv_ref_local_world_aligned);
248 
249  BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned));
250  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned));
251 
252  getFrameVelocityDerivatives(
253  model, data_v, frameId, LOCAL, v_partial_dq_ref_local, v_partial_dv_ref_local);
254 
255  BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
256  BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local));
257  }
258 
259  Data::Matrix6x J_ref(6, model.nv);
260  J_ref.setZero();
261  Data::Matrix6x J_ref_local(6, model.nv);
262  J_ref_local.setZero();
263  Data::Matrix6x J_ref_local_world_aligned(6, model.nv);
264  J_ref_local_world_aligned.setZero();
265  computeJointJacobians(model, data_ref, q);
266  getFrameJacobian(model, data_ref, frameId, WORLD, J_ref);
267  getFrameJacobian(model, data_ref, frameId, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned);
268  getFrameJacobian(model, data_ref, frameId, LOCAL, J_ref_local);
269 
270  BOOST_CHECK(a_partial_da.isApprox(J_ref));
271  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned));
272  BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local));
273 
274  // Check against finite differences
275  Data::Matrix6x a_partial_da_fd(6, model.nv);
276  a_partial_da_fd.setZero();
277  Data::Matrix6x a_partial_da_fd_local_world_aligned(6, model.nv);
278  a_partial_da_fd_local_world_aligned.setZero();
279  Data::Matrix6x a_partial_da_fd_local(6, model.nv);
280  a_partial_da_fd_local.setZero();
281  const double alpha = 1e-8;
282 
283  Eigen::VectorXd v_plus(v), a_plus(a);
284  Data data_plus(model);
285  forwardKinematics(model, data_ref, q, v, a);
286 
287  // dacc/da
288  Motion a0 = getFrameAcceleration(model, data, frameId, WORLD);
289  Motion a0_local_world_aligned = getFrameAcceleration(model, data, frameId, LOCAL_WORLD_ALIGNED);
290  Motion a0_local = getFrameAcceleration(model, data, frameId, LOCAL);
291  for (int k = 0; k < model.nv; ++k)
292  {
293  a_plus[k] += alpha;
294  forwardKinematics(model, data_plus, q, v, a_plus);
295 
296  a_partial_da_fd.col(k) =
297  (getFrameAcceleration(model, data_plus, frameId, WORLD) - a0).toVector() / alpha;
298  a_partial_da_fd_local_world_aligned.col(k) =
299  (getFrameAcceleration(model, data_plus, frameId, LOCAL_WORLD_ALIGNED)
300  - a0_local_world_aligned)
301  .toVector()
302  / alpha;
303  a_partial_da_fd_local.col(k) =
304  (getFrameAcceleration(model, data_plus, frameId, LOCAL) - a0_local).toVector() / alpha;
305  a_plus[k] -= alpha;
306  }
307  BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd, sqrt(alpha)));
308  BOOST_CHECK(
309  a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned, sqrt(alpha)));
310  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local, sqrt(alpha)));
311 
312  // dacc/dv
313  Data::Matrix6x a_partial_dv_fd(6, model.nv);
314  a_partial_dv_fd.setZero();
315  Data::Matrix6x a_partial_dv_fd_local_world_aligned(6, model.nv);
316  a_partial_dv_fd_local_world_aligned.setZero();
317  Data::Matrix6x a_partial_dv_fd_local(6, model.nv);
318  a_partial_dv_fd_local.setZero();
319  for (int k = 0; k < model.nv; ++k)
320  {
321  v_plus[k] += alpha;
322  forwardKinematics(model, data_plus, q, v_plus, a);
323 
324  a_partial_dv_fd.col(k) =
325  (getFrameAcceleration(model, data_plus, frameId, WORLD) - a0).toVector() / alpha;
326  a_partial_dv_fd_local_world_aligned.col(k) =
327  (getFrameAcceleration(model, data_plus, frameId, LOCAL_WORLD_ALIGNED)
328  - a0_local_world_aligned)
329  .toVector()
330  / alpha;
331  a_partial_dv_fd_local.col(k) =
332  (getFrameAcceleration(model, data_plus, frameId, LOCAL) - a0_local).toVector() / alpha;
333  v_plus[k] -= alpha;
334  }
335 
336  BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd, sqrt(alpha)));
337  BOOST_CHECK(
338  a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned, sqrt(alpha)));
339  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local, sqrt(alpha)));
340 
341  // dacc/dq
342  a_partial_dq.setZero();
343  a_partial_dv.setZero();
344  a_partial_da.setZero();
345 
346  a_partial_dq_local_world_aligned.setZero();
347  a_partial_dv_local_world_aligned.setZero();
348  a_partial_da_local_world_aligned.setZero();
349 
350  a_partial_dq_local.setZero();
351  a_partial_dv_local.setZero();
352  a_partial_da_local.setZero();
353 
354  Data::Matrix6x a_partial_dq_fd(6, model.nv);
355  a_partial_dq_fd.setZero();
356  Data::Matrix6x a_partial_dq_fd_local_world_aligned(6, model.nv);
357  a_partial_dq_fd_local_world_aligned.setZero();
358  Data::Matrix6x a_partial_dq_fd_local(6, model.nv);
359  a_partial_dq_fd_local.setZero();
360 
361  computeForwardKinematicsDerivatives(model, data, q, v, a);
362  getFrameAccelerationDerivatives(
363  model, data, frameId, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
364 
365  getFrameAccelerationDerivatives(
366  model, data, frameId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned,
367  a_partial_dq_local_world_aligned, a_partial_dv_local_world_aligned,
368  a_partial_da_local_world_aligned);
369 
370  getFrameAccelerationDerivatives(
371  model, data, frameId, LOCAL, v_partial_dq_local, a_partial_dq_local, a_partial_dv_local,
372  a_partial_da_local);
373 
374  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
375  forwardKinematics(model, data_ref, q, v, a);
376  updateFramePlacements(model, data_ref);
377  a0 = getFrameAcceleration(model, data, frameId, WORLD);
378  a0_local_world_aligned = getFrameAcceleration(model, data, frameId, LOCAL_WORLD_ALIGNED);
379  a0_local = getFrameAcceleration(model, data, frameId, LOCAL);
380 
381  for (int k = 0; k < model.nv; ++k)
382  {
383  v_eps[k] += alpha;
384  q_plus = integrate(model, q, v_eps);
385  forwardKinematics(model, data_plus, q_plus, v, a);
386  updateFramePlacements(model, data_plus);
387 
388  a_partial_dq_fd.col(k) =
389  (getFrameAcceleration(model, data_plus, frameId, WORLD) - a0).toVector() / alpha;
390  Motion a_plus_local_world_aligned =
391  getFrameAcceleration(model, data_plus, frameId, LOCAL_WORLD_ALIGNED);
392  const SE3::Vector3 trans =
393  data_plus.oMf[frameId].translation() - data_ref.oMf[frameId].translation();
394  a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans);
395  a_partial_dq_fd_local_world_aligned.col(k) =
396  (a_plus_local_world_aligned - a0_local_world_aligned).toVector() / alpha;
397  a_partial_dq_fd_local.col(k) =
398  (getFrameAcceleration(model, data_plus, frameId, LOCAL) - a0_local).toVector() / alpha;
399  v_eps[k] -= alpha;
400  }
401 
402  BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd, sqrt(alpha)));
403  BOOST_CHECK(
404  a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned, sqrt(alpha)));
405  BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local, sqrt(alpha)));
406 
407  // Test other signatures
408  Data::Matrix6x v_partial_dq_other(6, model.nv);
409  v_partial_dq_other.setZero();
410  Data::Matrix6x v_partial_dq_local_other(6, model.nv);
411  v_partial_dq_local_other.setZero();
412  Data::Matrix6x v_partial_dq_local_world_aligned_other(6, model.nv);
413  v_partial_dq_local_world_aligned_other.setZero();
414  Data::Matrix6x v_partial_dv_other(6, model.nv);
415  v_partial_dv_other.setZero();
416  Data::Matrix6x v_partial_dv_local_other(6, model.nv);
417  v_partial_dv_local_other.setZero();
418  Data::Matrix6x v_partial_dv_local_world_aligned_other(6, model.nv);
419  v_partial_dv_local_world_aligned_other.setZero();
420  Data::Matrix6x a_partial_dq_other(6, model.nv);
421  a_partial_dq_other.setZero();
422  Data::Matrix6x a_partial_dq_local_world_aligned_other(6, model.nv);
423  a_partial_dq_local_world_aligned_other.setZero();
424  Data::Matrix6x a_partial_dq_local_other(6, model.nv);
425  a_partial_dq_local_other.setZero();
426  Data::Matrix6x a_partial_dv_other(6, model.nv);
427  a_partial_dv_other.setZero();
428  Data::Matrix6x a_partial_dv_local_world_aligned_other(6, model.nv);
429  a_partial_dv_local_world_aligned_other.setZero();
430  Data::Matrix6x a_partial_dv_local_other(6, model.nv);
431  a_partial_dv_local_other.setZero();
432  Data::Matrix6x a_partial_da_other(6, model.nv);
433  a_partial_da_other.setZero();
434  Data::Matrix6x a_partial_da_local_world_aligned_other(6, model.nv);
435  a_partial_da_local_world_aligned_other.setZero();
436  Data::Matrix6x a_partial_da_local_other(6, model.nv);
437  a_partial_da_local_other.setZero();
438 
439  getFrameAccelerationDerivatives(
440  model, data, frameId, WORLD, v_partial_dq_other, v_partial_dv_other, a_partial_dq_other,
441  a_partial_dv_other, a_partial_da_other);
442 
443  BOOST_CHECK(v_partial_dq_other.isApprox(v_partial_dq));
444  BOOST_CHECK(v_partial_dv_other.isApprox(a_partial_da));
445  BOOST_CHECK(a_partial_dq_other.isApprox(a_partial_dq));
446  BOOST_CHECK(a_partial_dv_other.isApprox(a_partial_dv));
447  BOOST_CHECK(a_partial_da_other.isApprox(a_partial_da));
448 
449  getFrameAccelerationDerivatives(
450  model, data, frameId, LOCAL_WORLD_ALIGNED, v_partial_dq_local_world_aligned_other,
451  v_partial_dv_local_world_aligned_other, a_partial_dq_local_world_aligned_other,
452  a_partial_dv_local_world_aligned_other, a_partial_da_local_world_aligned_other);
453 
454  BOOST_CHECK(v_partial_dq_local_world_aligned_other.isApprox(v_partial_dq_local_world_aligned));
455  BOOST_CHECK(v_partial_dv_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));
456  BOOST_CHECK(a_partial_dq_local_world_aligned_other.isApprox(a_partial_dq_local_world_aligned));
457  BOOST_CHECK(a_partial_dv_local_world_aligned_other.isApprox(a_partial_dv_local_world_aligned));
458  BOOST_CHECK(a_partial_da_local_world_aligned_other.isApprox(a_partial_da_local_world_aligned));
459 
460  getFrameAccelerationDerivatives(
461  model, data, frameId, LOCAL, v_partial_dq_local_other, v_partial_dv_local_other,
462  a_partial_dq_local_other, a_partial_dv_local_other, a_partial_da_local_other);
463 
464  BOOST_CHECK(v_partial_dq_local_other.isApprox(v_partial_dq_local));
465  BOOST_CHECK(v_partial_dv_local_other.isApprox(a_partial_da_local));
466  BOOST_CHECK(a_partial_dq_local_other.isApprox(a_partial_dq_local));
467  BOOST_CHECK(a_partial_dv_local_other.isApprox(a_partial_dv_local));
468  BOOST_CHECK(a_partial_da_local_other.isApprox(a_partial_da_local));
469 }
470 
471 BOOST_AUTO_TEST_SUITE_END()
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
pinocchio::FrameIndex
Index FrameIndex
Definition: multibody/fwd.hpp:28
frames.hpp
Eigen
pinocchio::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
pinocchio::DataTpl
Definition: context/generic.hpp:25
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_frames_derivatives_velocity)
Definition: unittest/frames-derivatives.cpp:20
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
pinocchio::getFrameAccelerationDerivatives
void getFrameAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &a_partial_dq, const Eigen::MatrixBase< Matrix6xOut3 > &a_partial_dv, const Eigen::MatrixBase< Matrix6xOut4 > &a_partial_da)
Computes the partial derivatives of the spatial acceleration of a frame given by its relative placeme...
pinocchio::getFrameJacobian
void getFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
Returns the jacobian of the frame given by its relative placement w.r.t. a joint frame,...
meshcat-viewer.v0
int v0
Definition: meshcat-viewer.py:88
pinocchio::getFrameVelocity
MotionTpl< Scalar, Options > getFrameVelocity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
Returns the spatial velocity of the Frame expressed in the desired reference frame....
kinematics.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame....
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
pinocchio::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: multibody/frame.hpp:55
pinocchio::Index
PINOCCHIO_COMPILER_DIAGNOSTIC_POP typedef std::size_t Index
Definition: multibody/fwd.hpp:22
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
pinocchio::computeForwardKinematicsDerivatives
void computeForwardKinematicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Computes all the terms required to compute the derivatives of the placement, spatial velocity and acc...
joint-configuration.hpp
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
pinocchio::updateFramePlacements
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
Definition: joint-configuration.hpp:70
pinocchio::getFrameVelocityDerivatives
void getFrameVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
Computes the partial derivatives of the spatial velocity of a frame given by its relative placement,...
cartpole.v
v
Definition: cartpole.py:153
q
q
contact-cholesky.frame
frame
Definition: contact-cholesky.py:24
a
Vec3f a
pinocchio::OP_FRAME
@ OP_FRAME
operational frame: user-defined frames that are defined at runtime
Definition: multibody/frame.hpp:33
fill
fill
pinocchio::getFrameAcceleration
MotionTpl< Scalar, Options > getFrameAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf=LOCAL)
Returns the spatial acceleration of the Frame expressed in the desired reference frame....
kinematics-derivatives.hpp
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
sample-models.hpp
pinocchio::MotionTpl
Definition: context/casadi.hpp:27
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
cartpole.a0
a0
Definition: cartpole.py:155
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
frames-derivatives.hpp
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:29