unittest/kinematics-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017-2020 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/joint-configuration.hpp"
9 #include "pinocchio/algorithm/kinematics.hpp"
10 #include "pinocchio/algorithm/kinematics-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 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_all)
21 {
22  using namespace Eigen;
23  using namespace pinocchio;
24 
25  Model model;
26  buildModels::humanoidRandom(model);
27 
28  Data data(model), data_ref(model);
29 
30  model.lowerPositionLimit.head<3>().fill(-1.);
31  model.upperPositionLimit.head<3>().fill(1.);
32  VectorXd q = randomConfiguration(model);
33  VectorXd v(VectorXd::Random(model.nv));
34  VectorXd a(VectorXd::Random(model.nv));
35 
36  forwardKinematics(model,data_ref,q,v,a);
37  computeForwardKinematicsDerivatives(model,data,q,v,a);
38 
39  for(size_t i = 1; i < (size_t)model.njoints; ++i)
40  {
41  BOOST_CHECK(data.oMi[i].isApprox(data_ref.oMi[i]));
42  BOOST_CHECK(data.v[i].isApprox(data_ref.v[i]));
43  BOOST_CHECK(data.ov[i].isApprox(data_ref.oMi[i].act(data_ref.v[i])));
44  BOOST_CHECK(data.a[i].isApprox(data_ref.a[i]));
45  BOOST_CHECK(data.oa[i].isApprox(data_ref.oMi[i].act(data_ref.a[i])));
46  }
47 
48  computeJointJacobians(model,data_ref,q);
49  BOOST_CHECK(data.J.isApprox(data_ref.J));
50 
51  computeJointJacobiansTimeVariation(model, data_ref, q, v);
52  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
53 }
54 
55 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_velocity)
56 {
57  using namespace Eigen;
58  using namespace pinocchio;
59 
60  Model model;
61  buildModels::humanoidRandom(model);
62 
63  Data data(model), data_ref(model);
64 
65  model.lowerPositionLimit.head<3>().fill(-1.);
66  model.upperPositionLimit.head<3>().fill(1.);
67  VectorXd q = randomConfiguration(model);
68  VectorXd v(VectorXd::Random(model.nv));
69  VectorXd a(VectorXd::Random(model.nv));
70 
71  computeForwardKinematicsDerivatives(model,data,q,v,a);
72 
73  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
74  Data::Matrix6x partial_dq(6,model.nv); partial_dq.setZero();
75  Data::Matrix6x partial_dq_local_world_aligned(6,model.nv); partial_dq_local_world_aligned.setZero();
76  Data::Matrix6x partial_dq_local(6,model.nv); partial_dq_local.setZero();
77  Data::Matrix6x partial_dv(6,model.nv); partial_dv.setZero();
78  Data::Matrix6x partial_dv_local_world_aligned(6,model.nv); partial_dv_local_world_aligned.setZero();
79  Data::Matrix6x partial_dv_local(6,model.nv); partial_dv_local.setZero();
80 
81  getJointVelocityDerivatives(model,data,jointId,WORLD,
82  partial_dq,partial_dv);
83 
84  getJointVelocityDerivatives(model,data,jointId,LOCAL_WORLD_ALIGNED,
85  partial_dq_local_world_aligned,partial_dv_local_world_aligned);
86 
87  getJointVelocityDerivatives(model,data,jointId,LOCAL,
88  partial_dq_local,partial_dv_local);
89 
90  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
91  Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero();
92  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
93  computeJointJacobians(model,data_ref,q);
94  getJointJacobian(model,data_ref,jointId,WORLD,J_ref);
95  getJointJacobian(model,data_ref,jointId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
96  getJointJacobian(model,data_ref,jointId,LOCAL,J_ref_local);
97 
98  BOOST_CHECK(partial_dv.isApprox(J_ref));
99  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(J_ref_local_world_aligned));
100  BOOST_CHECK(partial_dv_local.isApprox(J_ref_local));
101 
102  // Check against finite differences
103  Data::Matrix6x partial_dq_fd(6,model.nv); partial_dq_fd.setZero();
104  Data::Matrix6x partial_dq_fd_local_world_aligned(6,model.nv); partial_dq_fd_local_world_aligned.setZero();
105  Data::Matrix6x partial_dq_fd_local(6,model.nv); partial_dq_fd_local.setZero();
106  Data::Matrix6x partial_dv_fd(6,model.nv); partial_dv_fd.setZero();
107  Data::Matrix6x partial_dv_fd_local_world_aligned(6,model.nv); partial_dv_fd_local_world_aligned.setZero();
108  Data::Matrix6x partial_dv_fd_local(6,model.nv); partial_dv_fd_local.setZero();
109  const double alpha = 1e-8;
110 
111  // dvel/dv
112  Eigen::VectorXd v_plus(v);
113  Data data_plus(model);
114  forwardKinematics(model,data_ref,q,v);
115  SE3 oMi_rot(SE3::Identity());
116  oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
117  Motion v0(data_ref.oMi[jointId].act(data_ref.v[jointId]));
118  Motion v0_local_world_aligned(oMi_rot.act(data_ref.v[jointId]));
119  Motion v0_local(data_ref.v[jointId]);
120  for(int k = 0; k < model.nv; ++k)
121  {
122  v_plus[k] += alpha;
123  forwardKinematics(model,data_plus,q,v_plus);
124 
125  partial_dv_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha;
126  partial_dv_fd_local_world_aligned.col(k) = (oMi_rot.act(data_plus.v[jointId]) - v0_local_world_aligned).toVector()/alpha;
127  partial_dv_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha;
128  v_plus[k] -= alpha;
129  }
130 
131  BOOST_CHECK(partial_dv.isApprox(partial_dv_fd,sqrt(alpha)));
132  BOOST_CHECK(partial_dv_local_world_aligned.isApprox(partial_dv_fd_local_world_aligned,sqrt(alpha)));
133  BOOST_CHECK(partial_dv_local.isApprox(partial_dv_fd_local,sqrt(alpha)));
134 
135  // dvel/dq
136  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
137  forwardKinematics(model,data_ref,q,v);
138  v0 = data_ref.oMi[jointId].act(data_ref.v[jointId]);
139 
140  for(int k = 0; k < model.nv; ++k)
141  {
142  v_eps[k] += alpha;
143  q_plus = integrate(model,q,v_eps);
144  forwardKinematics(model,data_plus,q_plus,v);
145 
146  SE3 oMi_plus_rot = data_plus.oMi[jointId];
147  oMi_plus_rot.translation().setZero();
148 
149  Motion v_plus_local_world_aligned = oMi_plus_rot.act(data_plus.v[jointId]);
150  SE3::Vector3 trans = data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation();
151  v_plus_local_world_aligned.linear() -= v_plus_local_world_aligned.angular().cross(trans);
152  partial_dq_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.v[jointId]) - v0).toVector()/alpha;
153  partial_dq_fd_local_world_aligned.col(k) = (v_plus_local_world_aligned - v0_local_world_aligned).toVector()/alpha;
154  partial_dq_fd_local.col(k) = (data_plus.v[jointId] - v0_local).toVector()/alpha;
155  v_eps[k] -= alpha;
156  }
157 
158  BOOST_CHECK(partial_dq.isApprox(partial_dq_fd,sqrt(alpha)));
159  BOOST_CHECK(partial_dq_local_world_aligned.isApprox(partial_dq_fd_local_world_aligned,sqrt(alpha)));
160  BOOST_CHECK(partial_dq_local.isApprox(partial_dq_fd_local,sqrt(alpha)));
161 
162 }
163 
164 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_acceleration)
165 {
166  using namespace Eigen;
167  using namespace pinocchio;
168 
169  Model model;
170  buildModels::humanoidRandom(model);
171 
172  Data data(model), data_ref(model);
173 
174  model.lowerPositionLimit.head<3>().fill(-1.);
175  model.upperPositionLimit.head<3>().fill(1.);
176  VectorXd q = randomConfiguration(model);
177  VectorXd v(VectorXd::Random(model.nv));
178  VectorXd a(VectorXd::Random(model.nv));
179 
180  computeForwardKinematicsDerivatives(model,data,q,v,a);
181 
182  const Model::JointIndex jointId = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
183 
184  Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
185  Data::Matrix6x v_partial_dq_local(6,model.nv); v_partial_dq_local.setZero();
186  Data::Matrix6x v_partial_dq_local_world_aligned(6,model.nv); v_partial_dq_local_world_aligned.setZero();
187  Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
188  Data::Matrix6x a_partial_dq_local_world_aligned(6,model.nv); a_partial_dq_local_world_aligned.setZero();
189  Data::Matrix6x a_partial_dq_local(6,model.nv); a_partial_dq_local.setZero();
190  Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
191  Data::Matrix6x a_partial_dv_local_world_aligned(6,model.nv); a_partial_dv_local_world_aligned.setZero();
192  Data::Matrix6x a_partial_dv_local(6,model.nv); a_partial_dv_local.setZero();
193  Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();
194  Data::Matrix6x a_partial_da_local_world_aligned(6,model.nv); a_partial_da_local_world_aligned.setZero();
195  Data::Matrix6x a_partial_da_local(6,model.nv); a_partial_da_local.setZero();
196 
197  getJointAccelerationDerivatives(model,data,jointId,WORLD,
198  v_partial_dq,
199  a_partial_dq,a_partial_dv,a_partial_da);
200 
201  getJointAccelerationDerivatives(model,data,jointId,LOCAL_WORLD_ALIGNED,
202  v_partial_dq_local_world_aligned,
203  a_partial_dq_local_world_aligned,
204  a_partial_dv_local_world_aligned,
205  a_partial_da_local_world_aligned);
206 
207  getJointAccelerationDerivatives(model,data,jointId,LOCAL,
208  v_partial_dq_local,
209  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);
210 
211  // Check v_partial_dq against getJointVelocityDerivatives
212  {
213  Data data_v(model);
214  computeForwardKinematicsDerivatives(model,data_v,q,v,a);
215 
216  Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
217  Data::Matrix6x v_partial_dq_ref_local_world_aligned(6,model.nv); v_partial_dq_ref_local_world_aligned.setZero();
218  Data::Matrix6x v_partial_dq_ref_local(6,model.nv); v_partial_dq_ref_local.setZero();
219  Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
220  Data::Matrix6x v_partial_dv_ref_local_world_aligned(6,model.nv); v_partial_dv_ref_local_world_aligned.setZero();
221  Data::Matrix6x v_partial_dv_ref_local(6,model.nv); v_partial_dv_ref_local.setZero();
222 
223  getJointVelocityDerivatives(model,data_v,jointId,WORLD,
224  v_partial_dq_ref,v_partial_dv_ref);
225 
226  BOOST_CHECK(v_partial_dq.isApprox(v_partial_dq_ref));
227  BOOST_CHECK(a_partial_da.isApprox(v_partial_dv_ref));
228 
229  getJointVelocityDerivatives(model,data_v,jointId,LOCAL_WORLD_ALIGNED,
230  v_partial_dq_ref_local_world_aligned,v_partial_dv_ref_local_world_aligned);
231 
232  BOOST_CHECK(v_partial_dq_local_world_aligned.isApprox(v_partial_dq_ref_local_world_aligned));
233  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(v_partial_dv_ref_local_world_aligned));
234 
235  getJointVelocityDerivatives(model,data_v,jointId,LOCAL,
236  v_partial_dq_ref_local,v_partial_dv_ref_local);
237 
238  BOOST_CHECK(v_partial_dq_local.isApprox(v_partial_dq_ref_local));
239  BOOST_CHECK(a_partial_da_local.isApprox(v_partial_dv_ref_local));
240  }
241 
242  Data::Matrix6x J_ref(6,model.nv); J_ref.setZero();
243  Data::Matrix6x J_ref_local(6,model.nv); J_ref_local.setZero();
244  Data::Matrix6x J_ref_local_world_aligned(6,model.nv); J_ref_local_world_aligned.setZero();
245  computeJointJacobians(model,data_ref,q);
246  getJointJacobian(model,data_ref,jointId,WORLD,J_ref);
247  getJointJacobian(model,data_ref,jointId,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
248  getJointJacobian(model,data_ref,jointId,LOCAL,J_ref_local);
249 
250  BOOST_CHECK(a_partial_da.isApprox(J_ref));
251  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(J_ref_local_world_aligned));
252  BOOST_CHECK(a_partial_da_local.isApprox(J_ref_local));
253 
254  // Check against finite differences
255  Data::Matrix6x a_partial_da_fd(6,model.nv); a_partial_da_fd.setZero();
256  Data::Matrix6x a_partial_da_fd_local_world_aligned(6,model.nv); a_partial_da_fd_local_world_aligned.setZero();
257  Data::Matrix6x a_partial_da_fd_local(6,model.nv); a_partial_da_fd_local.setZero();
258  const double alpha = 1e-8;
259 
260  Eigen::VectorXd v_plus(v), a_plus(a);
261  Data data_plus(model);
262  forwardKinematics(model,data_ref,q,v,a);
263  SE3 oMi_rot(SE3::Identity());
264  oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
265 
266  // dacc/da
267  Motion a0(data_ref.oMi[jointId].act(data_ref.a[jointId]));
268  Motion a0_local_world_aligned(oMi_rot.act(data_ref.a[jointId]));
269  Motion a0_local(data_ref.a[jointId]);
270  for(int k = 0; k < model.nv; ++k)
271  {
272  a_plus[k] += alpha;
273  forwardKinematics(model,data_plus,q,v,a_plus);
274 
275  a_partial_da_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha;
276  a_partial_da_fd_local_world_aligned.col(k) = (oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector()/alpha;
277  a_partial_da_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector()/alpha;
278  a_plus[k] -= alpha;
279  }
280  BOOST_CHECK(a_partial_da.isApprox(a_partial_da_fd,sqrt(alpha)));
281  BOOST_CHECK(a_partial_da_local_world_aligned.isApprox(a_partial_da_fd_local_world_aligned,sqrt(alpha)));
282  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha)));
283  motionSet::se3Action(data_ref.oMi[jointId].inverse(),a_partial_da,a_partial_da_local);
284  BOOST_CHECK(a_partial_da_local.isApprox(a_partial_da_fd_local,sqrt(alpha)));
285 
286  // dacc/dv
287  Data::Matrix6x a_partial_dv_fd(6,model.nv); a_partial_dv_fd.setZero();
288  Data::Matrix6x a_partial_dv_fd_local_world_aligned(6,model.nv); a_partial_dv_fd_local_world_aligned.setZero();
289  Data::Matrix6x a_partial_dv_fd_local(6,model.nv); a_partial_dv_fd_local.setZero();
290  for(int k = 0; k < model.nv; ++k)
291  {
292  v_plus[k] += alpha;
293  forwardKinematics(model,data_plus,q,v_plus,a);
294 
295  a_partial_dv_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha; a_partial_dv_fd_local_world_aligned.col(k) = (oMi_rot.act(data_plus.a[jointId]) - a0_local_world_aligned).toVector()/alpha;
296  a_partial_dv_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector()/alpha;
297  v_plus[k] -= alpha;
298  }
299 
300  BOOST_CHECK(a_partial_dv.isApprox(a_partial_dv_fd,sqrt(alpha)));
301  BOOST_CHECK(a_partial_dv_local_world_aligned.isApprox(a_partial_dv_fd_local_world_aligned,sqrt(alpha)));
302  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha)));
303  motionSet::se3Action(data_ref.oMi[jointId].inverse(),a_partial_dv,a_partial_dv_local);
304  BOOST_CHECK(a_partial_dv_local.isApprox(a_partial_dv_fd_local,sqrt(alpha)));
305 
306  // dacc/dq
307  a_partial_dq.setZero();
308  a_partial_dv.setZero();
309  a_partial_da.setZero();
310 
311  a_partial_dq_local_world_aligned.setZero();
312  a_partial_dv_local_world_aligned.setZero();
313  a_partial_da_local_world_aligned.setZero();
314 
315  a_partial_dq_local.setZero();
316  a_partial_dv_local.setZero();
317  a_partial_da_local.setZero();
318 
319  Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
320  Data::Matrix6x a_partial_dq_fd_local_world_aligned(6,model.nv); a_partial_dq_fd_local_world_aligned.setZero();
321  Data::Matrix6x a_partial_dq_fd_local(6,model.nv); a_partial_dq_fd_local.setZero();
322 
323  computeForwardKinematicsDerivatives(model,data,q,v,a);
324  getJointAccelerationDerivatives(model,data,jointId,WORLD,
325  v_partial_dq,
326  a_partial_dq,a_partial_dv,a_partial_da);
327 
328  getJointAccelerationDerivatives(model,data,jointId,LOCAL_WORLD_ALIGNED,
329  v_partial_dq_local_world_aligned,
330  a_partial_dq_local_world_aligned,
331  a_partial_dv_local_world_aligned,
332  a_partial_da_local_world_aligned);
333 
334  getJointAccelerationDerivatives(model,data,jointId,LOCAL,
335  v_partial_dq_local,
336  a_partial_dq_local,a_partial_dv_local,a_partial_da_local);
337 
338  Eigen::VectorXd q_plus(q), v_eps(Eigen::VectorXd::Zero(model.nv));
339  forwardKinematics(model,data_ref,q,v,a);
340  a0 = data_ref.oMi[jointId].act(data_ref.a[jointId]);
341  oMi_rot.rotation() = data_ref.oMi[jointId].rotation();
342  a0_local_world_aligned = oMi_rot.act(data_ref.a[jointId]);
343  a0_local = data_ref.a[jointId];
344 
345  for(int k = 0; k < model.nv; ++k)
346  {
347  v_eps[k] += alpha;
348  q_plus = integrate(model,q,v_eps);
349  forwardKinematics(model,data_plus,q_plus,v,a);
350 
351  SE3 oMi_plus_rot = data_plus.oMi[jointId];
352  oMi_plus_rot.translation().setZero();
353 
354  Motion a_plus_local_world_aligned = oMi_plus_rot.act(data_plus.a[jointId]);
355  const SE3::Vector3 trans = data_plus.oMi[jointId].translation() - data_ref.oMi[jointId].translation();
356  a_plus_local_world_aligned.linear() -= a_plus_local_world_aligned.angular().cross(trans);
357  a_partial_dq_fd.col(k) = (data_plus.oMi[jointId].act(data_plus.a[jointId]) - a0).toVector()/alpha;
358  a_partial_dq_fd_local_world_aligned.col(k) = (a_plus_local_world_aligned - a0_local_world_aligned).toVector()/alpha;
359  a_partial_dq_fd_local.col(k) = (data_plus.a[jointId] - a0_local).toVector()/alpha;
360  v_eps[k] -= alpha;
361  }
362 
363  BOOST_CHECK(a_partial_dq.isApprox(a_partial_dq_fd,sqrt(alpha)));
364  BOOST_CHECK(a_partial_dq_local_world_aligned.isApprox(a_partial_dq_fd_local_world_aligned,sqrt(alpha)));
365  BOOST_CHECK(a_partial_dq_local.isApprox(a_partial_dq_fd_local,sqrt(alpha)));
366 
367 }
368 
369 BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_against_classic_formula)
370 {
371  using namespace Eigen;
372  using namespace pinocchio;
373 
374  Model model;
375  buildModels::humanoidRandom(model,true);
376 
377  Data data(model), data_ref(model);
378 
379  model.lowerPositionLimit.head<3>().fill(-1.);
380  model.upperPositionLimit.head<3>().fill(1.);
381  VectorXd q = randomConfiguration(model);
382  VectorXd v(VectorXd::Random(model.nv));
383  VectorXd a(VectorXd::Random(model.nv));
384 
385  const Model::JointIndex jointId = model.existJointName("rarm4_joint")?model.getJointId("rarm4_joint"):(Model::Index)(model.njoints-1);
386  Data::Matrix6x v_partial_dq(6,model.nv); v_partial_dq.setZero();
387  Data::Matrix6x v_partial_dq_ref(6,model.nv); v_partial_dq_ref.setZero();
388  Data::Matrix6x v_partial_dv_ref(6,model.nv); v_partial_dv_ref.setZero();
389  Data::Matrix6x a_partial_dq(6,model.nv); a_partial_dq.setZero();
390  Data::Matrix6x a_partial_dv(6,model.nv); a_partial_dv.setZero();
391  Data::Matrix6x a_partial_da(6,model.nv); a_partial_da.setZero();
392 
393  // LOCAL: da/dv == dJ/dt + dv/dq
394 // {
395 // Data::Matrix6x rhs(6,model.nv); rhs.setZero();
396 //
397 // v_partial_dq.setZero();
398 // a_partial_dq.setZero();
399 // a_partial_dv.setZero();
400 // a_partial_da.setZero();
401 //
402 // computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
403 // computeForwardKinematicsDerivatives(model,data,q,v,a);
404 //
405 // getJointAccelerationDerivatives<LOCAL>(model,data,jointId,
406 // v_partial_dq,
407 // a_partial_dq,a_partial_dv,a_partial_da);
408 //
409 // getJointJacobianTimeVariation<LOCAL>(model,data_ref,jointId,rhs);
410 //
411 // v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
412 // getJointVelocityDerivatives<LOCAL>(model,data_ref,jointId,
413 // v_partial_dq_ref,v_partial_dv_ref);
414 // rhs += v_partial_dq_ref;
415 // BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12));
416 //
417 // std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
418 // std::cout << "rhs\n" << rhs << std::endl;
419 // }
420 
421  // WORLD: da/dv == dJ/dt + dv/dq
422  {
423  Data::Matrix6x rhs(6,model.nv); rhs.setZero();
424 
425  v_partial_dq.setZero();
426  a_partial_dq.setZero();
427  a_partial_dv.setZero();
428  a_partial_da.setZero();
429 
430  computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
431  computeForwardKinematicsDerivatives(model,data,q,v,a);
432 
433  getJointAccelerationDerivatives(model,data,jointId,WORLD,
434  v_partial_dq,
435  a_partial_dq,a_partial_dv,a_partial_da);
436 
437  getJointJacobianTimeVariation(model,data_ref,jointId,WORLD,rhs);
438 
439  v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
440  getJointVelocityDerivatives(model,data_ref,jointId,WORLD,
441  v_partial_dq_ref,v_partial_dv_ref);
442  rhs += v_partial_dq_ref;
443  BOOST_CHECK(a_partial_dv.isApprox(rhs,1e-12));
444 
445 // std::cout << "a_partial_dv\n" << a_partial_dv << std::endl;
446 // std::cout << "rhs\n" << rhs << std::endl;
447  }
448 
449 // // WORLD: da/dq == d/dt(dv/dq)
450 // {
451 // const double alpha = 1e-8;
452 // Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);
453 //
454 // Data data_plus(model);
455 // v_plus = v + alpha * a;
456 // q_plus = integrate(model,q,alpha*v);
457 //
458 // computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a);
459 // computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
460 //
461 // Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero();
462 // Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero();
463 //
464 // v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
465 //
466 // v_partial_dq.setZero();
467 // a_partial_dq.setZero();
468 // a_partial_dv.setZero();
469 // a_partial_da.setZero();
470 //
471 // getJointVelocityDerivatives<WORLD>(model,data_ref,jointId,
472 // v_partial_dq_ref,v_partial_dv_ref);
473 // getJointVelocityDerivatives<WORLD>(model,data_plus,jointId,
474 // v_partial_dq_plus,v_partial_dv_plus);
475 //
476 // getJointAccelerationDerivatives<WORLD>(model,data_ref,jointId,
477 // v_partial_dq,
478 // a_partial_dq,a_partial_dv,a_partial_da);
479 //
480 // Data::Matrix6x a_partial_dq_fd(6,model.nv); a_partial_dq_fd.setZero();
481 // {
482 // Data data_fd(model);
483 // VectorXd q_fd(model.nq), v_eps(model.nv); v_eps.setZero();
484 // for(int k = 0; k < model.nv; ++k)
485 // {
486 // v_eps[k] += alpha;
487 // q_fd = integrate(model,q,v_eps);
488 // forwardKinematics(model,data_fd,q_fd,v,a);
489 // a_partial_dq_fd.col(k) = (data_fd.oMi[jointId].act(data_fd.a[jointId]) - data_ref.oa[jointId]).toVector()/alpha;
490 // v_eps[k] = 0.;
491 // }
492 // }
493 //
494 // Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha;
495 // BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha)));
496 //
497 // std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
498 // std::cout << "a_partial_dq_fd\n" << a_partial_dq_fd << std::endl;
499 // std::cout << "rhs\n" << rhs << std::endl;
500 // }
501 
502  // LOCAL: da/dq == d/dt(dv/dq)
503  {
504  const double alpha = 1e-8;
505  Eigen::VectorXd q_plus(model.nq), v_plus(model.nv);
506 
507  Data data_plus(model);
508  v_plus = v + alpha * a;
509  q_plus = integrate(model,q,alpha*v);
510 
511  computeForwardKinematicsDerivatives(model,data_plus,q_plus,v_plus,a);
512  computeForwardKinematicsDerivatives(model,data_ref,q,v,a);
513 
514  Data::Matrix6x v_partial_dq_plus(6,model.nv); v_partial_dq_plus.setZero();
515  Data::Matrix6x v_partial_dv_plus(6,model.nv); v_partial_dv_plus.setZero();
516 
517  v_partial_dq_ref.setZero(); v_partial_dv_ref.setZero();
518 
519  v_partial_dq.setZero();
520  a_partial_dq.setZero();
521  a_partial_dv.setZero();
522  a_partial_da.setZero();
523 
524  getJointVelocityDerivatives(model,data_ref,jointId,LOCAL,
525  v_partial_dq_ref,v_partial_dv_ref);
526  getJointVelocityDerivatives(model,data_plus,jointId,LOCAL,
527  v_partial_dq_plus,v_partial_dv_plus);
528 
529  getJointAccelerationDerivatives(model,data_ref,jointId,LOCAL,
530  v_partial_dq,
531  a_partial_dq,a_partial_dv,a_partial_da);
532 
533  Data::Matrix6x rhs = (v_partial_dq_plus - v_partial_dq_ref)/alpha;
534  BOOST_CHECK(a_partial_dq.isApprox(rhs,sqrt(alpha)));
535 
536 // std::cout << "a_partial_dq\n" << a_partial_dq << std::endl;
537 // std::cout << "rhs\n" << rhs << std::endl;
538  }
539 
540 
541 }
542 
543 BOOST_AUTO_TEST_CASE(test_kinematics_hessians)
544 {
545  using namespace Eigen;
546  using namespace pinocchio;
547 
548  Model model;
549  buildModels::humanoidRandom(model,true);
550 
551  Data data(model), data_ref(model), data_plus(model);
552 
553  model.lowerPositionLimit.head<3>().fill(-1.);
554  model.upperPositionLimit.head<3>().fill(1.);
555  VectorXd q = randomConfiguration(model);
556 
557  const Model::JointIndex joint_id = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
558 
559  computeJointJacobians(model,data,q);
560  computeJointKinematicHessians(model,data);
561 
562  Data data2(model);
563  computeJointKinematicHessians(model,data2,q);
564  BOOST_CHECK(data2.J.isApprox(data.J));
565 
566  const Eigen::DenseIndex matrix_offset = 6 * model.nv;
567 
568  for(int k = 0; k < model.nv; ++k)
569  {
570  Eigen::Map<Data::Matrix6x> dJ(data.kinematic_hessians.data() + k*matrix_offset,6,model.nv);
571  Eigen::Map<Data::Matrix6x> dJ2(data2.kinematic_hessians.data() + k*matrix_offset,6,model.nv);
572 
573  BOOST_CHECK(dJ2.isApprox(dJ));
574  }
575 
576  for(int i = 0; i < model.nv; ++i)
577  {
578  for(int j = i; j < model.nv; ++j)
579  {
580  bool j_is_children_of_i = false;
581  for(int parent = j; parent >= 0; parent = data.parents_fromRow[(size_t)parent])
582  {
583  if(parent == i)
584  {
585  j_is_children_of_i = true;
586  break;
587  }
588  }
589 
590  if(j_is_children_of_i)
591  {
592  if(i==j)
593  {
594  Eigen::Map<Data::Motion::Vector6> SixSi( data.kinematic_hessians.data()
595  + i * matrix_offset
596  + i * 6);
597  BOOST_CHECK(SixSi.isZero());
598  }
599  else
600  {
601  Eigen::Map<Data::Motion::Vector6> SixSj( data.kinematic_hessians.data()
602  + i * matrix_offset
603  + j * 6);
604 
605  Eigen::Map<Data::Motion::Vector6> SjxSi( data.kinematic_hessians.data()
606  + j * matrix_offset
607  + i * 6);
608 
609  BOOST_CHECK(SixSj.isApprox(-SjxSi));
610  }
611  }
612  else
613  {
614  Eigen::Map<Data::Motion::Vector6> SixSj( data.kinematic_hessians.data()
615  + i * matrix_offset
616  + j * 6);
617 
618  Eigen::Map<Data::Motion::Vector6> SjxSi( data.kinematic_hessians.data()
619  + j * matrix_offset
620  + i * 6);
621 
622  BOOST_CHECK(SixSj.isZero());
623  BOOST_CHECK(SjxSi.isZero());
624  }
625  }
626  }
627 
628  const double eps = 1e-8;
629  Data::Matrix6x J_ref(6,model.nv), J_plus(6,model.nv);
630  J_ref.setZero(); J_plus.setZero();
631 
632  computeJointJacobians(model,data_ref,q);
633  VectorXd v_plus(VectorXd::Zero(model.nv));
634 
635  const Eigen::DenseIndex outer_offset = model.nv * 6;
636 
637  // WORLD
638  getJointJacobian(model,data_ref,joint_id,WORLD,J_ref);
639  Data::Tensor3x kinematic_hessian_world = getJointKinematicHessian(model,data,joint_id,WORLD);
640  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
641  {
642  v_plus[k] = eps;
643  const VectorXd q_plus = integrate(model,q,v_plus);
644  computeJointJacobians(model,data_plus,q_plus);
645  J_plus.setZero();
646  getJointJacobian(model,data_plus,joint_id,WORLD,J_plus);
647 
648  Data::Matrix6x dJ_dq_ref = (J_plus - J_ref)/eps;
649  Eigen::Map<Data::Matrix6x> dJ_dq(kinematic_hessian_world.data() + k * outer_offset,6,model.nv);
650 
651 // std::cout << "k: " << k << std::endl;
652 // std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
653 // std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
654  BOOST_CHECK((dJ_dq_ref-dJ_dq).isZero(sqrt(eps)));
655  v_plus[k] = 0.;
656  }
657 
658  // LOCAL_WORLD_ALIGNED
659  computeJointJacobians(model,data_ref,q);
660  getJointJacobian(model,data_ref,joint_id,LOCAL_WORLD_ALIGNED,J_ref);
661  Data::Tensor3x kinematic_hessian_local_world_aligned = getJointKinematicHessian(model,data,joint_id,LOCAL_WORLD_ALIGNED);
662  Data::Matrix3x dt_last_fd(3,model.nv);
663  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
664  {
665  v_plus[k] = eps;
666  const VectorXd q_plus = integrate(model,q,v_plus);
667  computeJointJacobians(model,data_plus,q_plus);
668  J_plus.setZero();
669  getJointJacobian(model,data_plus,joint_id,LOCAL_WORLD_ALIGNED,J_plus);
670 
671  SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id];
672  tMt_plus.rotation().setIdentity();
673 
674  dt_last_fd.col(k) = (data_plus.oMi[joint_id].translation() - data_ref.oMi[joint_id].translation())/eps;
675 
676  Data::Matrix6x dJ_dq_ref = (J_plus - J_ref)/eps;
677  Eigen::Map<Data::Matrix6x> dJ_dq(kinematic_hessian_local_world_aligned.data() + k * outer_offset,6,model.nv);
678 
679 // std::cout << "k: " << k << std::endl;
680 // std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
681 // std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
682  BOOST_CHECK((dJ_dq_ref-dJ_dq).isZero(sqrt(eps)));
683  v_plus[k] = 0.;
684  }
685 
686  Data::Matrix6x J_world(6,model.nv); J_world.setZero();
687  getJointJacobian(model,data_ref,joint_id,LOCAL_WORLD_ALIGNED,J_world);
688 
689  BOOST_CHECK(dt_last_fd.isApprox(J_world.topRows<3>(),sqrt(eps)));
690 
691  // LOCAL
692  computeJointJacobians(model,data_ref,q);
693  getJointJacobian(model,data_ref,joint_id,LOCAL,J_ref);
694  Data::Tensor3x kinematic_hessian_local = getJointKinematicHessian(model,data,joint_id,LOCAL);
695  for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
696  {
697  v_plus[k] = eps;
698  const VectorXd q_plus = integrate(model,q,v_plus);
699  computeJointJacobians(model,data_plus,q_plus);
700  J_plus.setZero();
701  getJointJacobian(model,data_plus,joint_id,LOCAL,J_plus);
702 
703 // const SE3 tMt_plus = data_ref.oMi[joint_id].inverse() * data_plus.oMi[joint_id];
704 
705  Data::Matrix6x dJ_dq_ref = (J_plus - J_ref)/eps;
706  Eigen::Map<Data::Matrix6x> dJ_dq(kinematic_hessian_local.data() + k * outer_offset,6,model.nv);
707 
708 // std::cout << "k: " << k << std::endl;
709 // std::cout << "dJ_dq:\n" << dJ_dq << std::endl;
710 // std::cout << "dJ_dq_ref:\n" << dJ_dq_ref << std::endl;
711  BOOST_CHECK((dJ_dq_ref-dJ_dq).isZero(sqrt(eps)));
712  v_plus[k] = 0.;
713  }
714 }
715 
716 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: src/multibody/data.hpp:286
pinocchio::WORLD
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
Definition: src/multibody/fwd.hpp:46
lambdas.parent
parent
Definition: lambdas.py:16
q
q
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: src/multibody/data.hpp:283
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:54
pinocchio::getJointVelocityDerivatives
void getJointVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
Computes the partial derivaties of the spatial velocity of a given with respect to the joint configur...
pinocchio::DataTpl::kinematic_hessians
Tensor3x kinematic_hessians
Tensor containing the kinematic Hessian of all the joints.
Definition: src/multibody/data.hpp:378
pinocchio::isZero
bool isZero(const Eigen::MatrixBase< MatrixLike > &m, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Definition: math/matrix.hpp:56
dcrba.eps
int eps
Definition: dcrba.py:384
pinocchio::ModelTpl::njoints
int njoints
Number of joints.
Definition: src/multibody/model.hpp:82
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: src/multibody/model.hpp:139
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:224
pinocchio::ModelTpl::existJointName
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or i...
pinocchio::MotionBase::linear
ConstLinearType linear() const
Definition: motion-base.hpp:22
fill
fill
pinocchio::MotionBase::angular
ConstAngularType angular() const
Definition: motion-base.hpp:21
pinocchio::Tensor::data
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar * data()
Definition: tensor.hpp:112
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.
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_all)
Definition: unittest/kinematics-derivatives.cpp:20
pinocchio::forceSet::se3Action
static void se3Action(const SE3Tpl< Scalar, Options > &m, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
SE3 action on a set of forces, represented by a 6xN matrix whose each column represent a spatial forc...
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: spatial/se3-tpl.hpp:115
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: src/multibody/model.hpp:49
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...
pinocchio::computeJointJacobiansTimeVariation
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobiansTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depen...
pinocchio::getJointAccelerationDerivatives
void getJointAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, 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 derivaties of the spatial acceleration of a given with respect to the joint conf...
a
Vec3f a
pinocchio::getJointJacobianTimeVariation
void getJointJacobianTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6Like > &dJ)
Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (r...
pinocchio::SE3Tpl< double, 0 >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:53
pinocchio::computeJointKinematicHessians
void computeJointKinematicHessians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes all the terms required to compute the second order derivatives of the placement information...
pinocchio::SE3Base::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::ModelTpl::getJointId
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
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::LOCAL_WORLD_ALIGNED
The LOCAL_WORLD_ALIGNED frame convention corresponds to the frame centered on the moving part (Joint...
Definition: src/multibody/fwd.hpp:48
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: src/multibody/model.hpp:79
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: src/multibody/data.hpp:75
pinocchio::SE3Base::act
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:90
pinocchio::Index
std::size_t Index
Definition: src/multibody/fwd.hpp:25
pinocchio::getJointKinematicHessian
void getJointKinematicHessian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const ReferenceFrame rf, Tensor< Scalar, 3, Options > &kinematic_hessian)
Retrieves the kinematic Hessian of a given joint according to the values aleardy computed by computeJ...
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: src/multibody/model.hpp:142
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::python::VectorXd
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: src/multibody/data.hpp:73
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76
pinocchio::SE3Base::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:38

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:31