frames.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016-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/frames.hpp"
9 #include "pinocchio/algorithm/rnea.hpp"
10 #include "pinocchio/spatial/act-on-set.hpp"
11 #include "pinocchio/parsers/sample-models.hpp"
12 #include "pinocchio/utils/timer.hpp"
13 #include "pinocchio/algorithm/joint-configuration.hpp"
14 
15 #include <iostream>
16 
17 #include <boost/test/unit_test.hpp>
18 #include <boost/utility/binary.hpp>
19 
20 template<typename Derived>
21 inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
22 {
23  return ((x - x).array() == (x - x).array()).all();
24 }
25 
26 
27 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
28 
29 BOOST_AUTO_TEST_CASE(frame_basic)
30 {
31  using namespace pinocchio;
32  Model model;
33  buildModels::humanoidRandom(model);
34 
35  BOOST_CHECK(model.frames.size() >= size_t(model.njoints));
36  for(Model::FrameVector::const_iterator it = model.frames.begin();
37  it != model.frames.end(); ++it)
38  {
39  const Frame & frame = *it;
40  BOOST_CHECK(frame == frame);
41  Frame frame_copy(frame);
42  BOOST_CHECK(frame_copy == frame);
43  }
44 
45  std::ostringstream os;
46  os << Frame("toto",0,0,SE3::Random(),OP_FRAME) << std::endl;
47  BOOST_CHECK(!os.str().empty());
48 }
49 
50 BOOST_AUTO_TEST_CASE(cast)
51 {
52  using namespace pinocchio;
53  Frame frame("toto",0,0,SE3::Random(),OP_FRAME);
54 
55  BOOST_CHECK(frame.cast<double>() == frame);
56  BOOST_CHECK(frame.cast<double>().cast<long double>() == frame.cast<long double>());
57 }
58 
59 BOOST_AUTO_TEST_CASE ( test_kinematics )
60 {
61  using namespace Eigen;
62  using namespace pinocchio;
63 
64  pinocchio::Model model;
65  pinocchio::buildModels::humanoidRandom(model);
66  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
67  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
68  const SE3 & framePlacement = SE3::Random();
69  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
70  pinocchio::Data data(model);
71 
72  VectorXd q = VectorXd::Ones(model.nq);
73  q.middleRows<4> (3).normalize();
74  framesForwardKinematics(model, data, q);
75 
76  BOOST_CHECK(data.oMf[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));
77 
78 }
79 
80 BOOST_AUTO_TEST_CASE ( test_update_placements )
81 {
82  using namespace Eigen;
83  using namespace pinocchio;
84 
85  pinocchio::Model model;
86  pinocchio::buildModels::humanoidRandom(model);
87  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
88  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
89  const SE3 & framePlacement = SE3::Random();
90  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
91  pinocchio::Data data(model);
92  pinocchio::Data data_ref(model);
93 
94  VectorXd q = VectorXd::Ones(model.nq);
95  q.middleRows<4> (3).normalize();
96 
97  forwardKinematics(model, data, q);
98  updateFramePlacements(model, data);
99 
100  framesForwardKinematics(model, data_ref, q);
101 
102  BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
103 }
104 
105 BOOST_AUTO_TEST_CASE ( test_update_single_placement )
106 {
107  using namespace Eigen;
108  using namespace pinocchio;
109 
110  pinocchio::Model model;
111  pinocchio::buildModels::humanoidRandom(model);
112  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
113  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
114  const SE3 & framePlacement = SE3::Random();
115  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
116  pinocchio::Data data(model);
117  pinocchio::Data data_ref(model);
118 
119  VectorXd q = VectorXd::Ones(model.nq);
120  q.middleRows<4> (3).normalize();
121 
122  forwardKinematics(model, data, q);
123  updateFramePlacement(model, data, frame_idx);
124 
125  framesForwardKinematics(model, data_ref, q);
126 
127  BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
128 }
129 
130 BOOST_AUTO_TEST_CASE ( test_velocity )
131 {
132  using namespace Eigen;
133  using namespace pinocchio;
134 
135  pinocchio::Model model;
136  pinocchio::buildModels::humanoidRandom(model);
137  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
138  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
139  const SE3 & framePlacement = SE3::Random();
140  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
141  pinocchio::Data data(model);
142 
143  VectorXd q = VectorXd::Ones(model.nq);
144  q.middleRows<4> (3).normalize();
145  VectorXd v = VectorXd::Ones(model.nv);
146  forwardKinematics(model, data, q, v);
147 
148  Motion vf = getFrameVelocity(model, data, frame_idx);
149 
150  BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx])));
151 
152  pinocchio::Data data_ref(model);
153  forwardKinematics(model, data_ref, q, v);
154  updateFramePlacements(model, data_ref);
155  Motion v_ref = getFrameVelocity(model, data_ref, frame_idx);
156 
157  BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model,data,frame_idx)));
158  BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model,data,frame_idx,LOCAL)));
159  BOOST_CHECK(data_ref.oMf[frame_idx].act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,WORLD)));
160  BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,LOCAL_WORLD_ALIGNED)));
161 }
162 
163 BOOST_AUTO_TEST_CASE ( test_acceleration )
164 {
165  using namespace Eigen;
166  using namespace pinocchio;
167 
168  pinocchio::Model model;
169  pinocchio::buildModels::humanoidRandom(model);
170  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
171  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
172  const SE3 & framePlacement = SE3::Random();
173  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
174  pinocchio::Data data(model);
175 
176  VectorXd q = VectorXd::Ones(model.nq);
177  q.middleRows<4> (3).normalize();
178  VectorXd v = VectorXd::Ones(model.nv);
179  VectorXd a = VectorXd::Ones(model.nv);
180  forwardKinematics(model, data, q, v, a);
181 
182  Motion af = getFrameAcceleration(model, data, frame_idx);
183 
184  BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
185 
186  pinocchio::Data data_ref(model);
187  forwardKinematics(model, data_ref, q, v, a);
188  updateFramePlacements(model, data_ref);
189  Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx);
190 
191  BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx)));
192  BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx,LOCAL)));
193  BOOST_CHECK(data_ref.oMf[frame_idx].act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,WORLD)));
194  BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,LOCAL_WORLD_ALIGNED)));
195 }
196 
197 BOOST_AUTO_TEST_CASE ( test_classic_acceleration )
198 {
199  using namespace Eigen;
200  using namespace pinocchio;
201 
202  pinocchio::Model model;
203  pinocchio::buildModels::humanoidRandom(model);
204  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
205  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
206  const SE3 & framePlacement = SE3::Random();
207  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
208  pinocchio::Data data(model);
209 
210  VectorXd q = VectorXd::Ones(model.nq);
211  q.middleRows<4> (3).normalize();
212  VectorXd v = VectorXd::Ones(model.nv);
213  VectorXd a = VectorXd::Ones(model.nv);
214  forwardKinematics(model, data, q, v, a);
215 
216  Motion vel = framePlacement.actInv(data.v[parent_idx]);
217  Motion acc = framePlacement.actInv(data.a[parent_idx]);
218  Vector3d linear;
219 
220  Motion acc_classical_local = acc;
221  linear = acc.linear() + vel.angular().cross(vel.linear());
222  acc_classical_local.linear() = linear;
223 
224  Motion af = getFrameClassicalAcceleration(model, data, frame_idx);
225 
226  BOOST_CHECK(af.isApprox(acc_classical_local));
227 
228  pinocchio::Data data_ref(model);
229  forwardKinematics(model, data_ref, q, v, a);
230  updateFramePlacements(model, data_ref);
231 
232  SE3 T_ref = data_ref.oMf[frame_idx];
233  Motion v_ref = getFrameVelocity(model, data_ref, frame_idx);
234  Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx);
235 
236  Motion acc_classical_local_ref = a_ref;
237  linear = a_ref.linear() + v_ref.angular().cross(v_ref.linear());
238  acc_classical_local_ref.linear() = linear;
239 
240  BOOST_CHECK(acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx)));
241  BOOST_CHECK(acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,LOCAL)));
242 
243  Motion vel_world_ref = T_ref.act(v_ref);
244  Motion acc_classical_world_ref = T_ref.act(a_ref);
245  linear = acc_classical_world_ref.linear() + vel_world_ref.angular().cross(vel_world_ref.linear());
246  acc_classical_world_ref.linear() = linear;
247 
248  BOOST_CHECK(acc_classical_world_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,WORLD)));
249 
250  Motion vel_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(v_ref);
251  Motion acc_classical_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(a_ref);
252  linear = acc_classical_aligned_ref.linear() + vel_aligned_ref.angular().cross(vel_aligned_ref.linear());
253  acc_classical_aligned_ref.linear() = linear;
254 
255  BOOST_CHECK(acc_classical_aligned_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,LOCAL_WORLD_ALIGNED)));
256 }
257 
258 BOOST_AUTO_TEST_CASE(test_frame_getters)
259 {
260  using namespace Eigen;
261  using namespace pinocchio;
262 
263  // Build a simple 1R planar model
264  Model model;
265  JointIndex parentId = model.addJoint(0, JointModelRZ(), SE3::Identity(), "Joint1");
266  FrameIndex frameId = model.addFrame(Frame("Frame1", parentId, 0, SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), OP_FRAME));
267 
268  Data data(model);
269 
270  // Predetermined configuration values
271  VectorXd q(model.nq);
272  q << M_PI/2;
273 
274  VectorXd v(model.nv);
275  v << 1.0;
276 
277  VectorXd a(model.nv);
278  a << 0.0;
279 
280  // Expected velocity
281  Motion v_local;
282  v_local.linear() = Vector3d(0.0, 1.0, 0.0);
283  v_local.angular() = Vector3d(0.0, 0.0, 1.0);
284 
285  Motion v_world;
286  v_world.linear() = Vector3d::Zero();
287  v_world.angular() = Vector3d(0.0, 0.0, 1.0);
288 
289  Motion v_align;
290  v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
291  v_align.angular() = Vector3d(0.0, 0.0, 1.0);
292 
293  // Expected classical acceleration
294  Motion ac_local;
295  ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
296  ac_local.angular() = Vector3d::Zero();
297 
298  Motion ac_world = Motion::Zero();
299 
300  Motion ac_align;
301  ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
302  ac_align.angular() = Vector3d::Zero();
303 
304  // Perform kinematics
305  forwardKinematics(model,data,q,v,a);
306 
307  // Check output velocity
308  BOOST_CHECK(v_local.isApprox(getFrameVelocity(model,data,frameId)));
309  BOOST_CHECK(v_local.isApprox(getFrameVelocity(model,data,frameId,LOCAL)));
310  BOOST_CHECK(v_world.isApprox(getFrameVelocity(model,data,frameId,WORLD)));
311  BOOST_CHECK(v_align.isApprox(getFrameVelocity(model,data,frameId,LOCAL_WORLD_ALIGNED)));
312 
313  // Check output acceleration (all zero)
314  BOOST_CHECK(getFrameAcceleration(model,data,frameId).isZero());
315  BOOST_CHECK(getFrameAcceleration(model,data,frameId,LOCAL).isZero());
316  BOOST_CHECK(getFrameAcceleration(model,data,frameId,WORLD).isZero());
317  BOOST_CHECK(getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED).isZero());
318 
319  // Check output classical acceleration
320  BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model,data,frameId)));
321  BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model,data,frameId,LOCAL)));
322  BOOST_CHECK(ac_world.isApprox(getFrameClassicalAcceleration(model,data,frameId,WORLD)));
323  BOOST_CHECK(ac_align.isApprox(getFrameClassicalAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED)));
324 }
325 
326 BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian )
327 {
328  using namespace Eigen;
329  using namespace pinocchio;
330 
331  Model model;
332  buildModels::humanoidRandom(model);
333  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
334  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
335  const SE3 & framePlacement = SE3::Random();
336  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
337  BOOST_CHECK(model.existFrame(frame_name));
338 
339  pinocchio::Data data(model);
340  pinocchio::Data data_ref(model);
341 
342  model.lowerPositionLimit.head<7>().fill(-1.);
343  model.upperPositionLimit.head<7>().fill( 1.);
344  VectorXd q = randomConfiguration(model);
345  VectorXd v = VectorXd::Ones(model.nv);
346 
348  Model::Index idx = model.getFrameId(frame_name);
349  const Frame & frame = model.frames[idx];
350  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
351  Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
352  Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
353  Data::Matrix6x Jff2(6,model.nv); Jff2.fill(0);
354 
355  computeJointJacobians(model,data,q);
356  updateFramePlacement(model, data, idx);
357  getFrameJacobian(model, data, idx, LOCAL, Jff);
358  computeJointJacobians(model,data_ref,q);
359  getJointJacobian(model, data_ref, parent_idx, LOCAL, Jjj);
360 
361  Motion nu_frame = Motion(Jff*v);
362  Motion nu_joint = Motion(Jjj*v);
363 
364  const SE3::ActionMatrixType jXf = frame.placement.toActionMatrix();
365  Data::Matrix6x Jjj_from_frame(jXf * Jff);
366  BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));
367 
368  BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));
369 
370  // In world frame
371  getFrameJacobian(model,data,idx,WORLD,Jff);
372  getJointJacobian(model, data_ref, parent_idx,WORLD, Jjj);
373  BOOST_CHECK(Jff.isApprox(Jjj));
374 
375  computeFrameJacobian(model,data,q,idx,WORLD,Jff2);
376 
377  BOOST_CHECK(Jff2.isApprox(Jjj));
378 }
379 
380 BOOST_AUTO_TEST_CASE ( test_frame_jacobian )
381 {
382  using namespace Eigen;
383  using namespace pinocchio;
384 
385  Model model;
386  buildModels::humanoidRandom(model);
387  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
388  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
389  const SE3 & framePlacement = SE3::Random();
390  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
391  BOOST_CHECK(model.existFrame(frame_name));
392 
393  pinocchio::Data data(model);
394  pinocchio::Data data_ref(model);
395 
396  model.lowerPositionLimit.head<7>().fill(-1.);
397  model.upperPositionLimit.head<7>().fill( 1.);
398  VectorXd q = randomConfiguration(model);
399  VectorXd v = VectorXd::Ones(model.nv);
400 
401  Model::Index idx = model.getFrameId(frame_name);
402  const Frame & frame = model.frames[idx];
403  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
404  Data::Matrix6x Jf(6,model.nv); Jf.fill(0);
405  Data::Matrix6x Jf2(6,model.nv); Jf2.fill(0);
406  Data::Matrix6x Jf_ref(6,model.nv); Jf_ref.fill(0);
407 
408  computeFrameJacobian(model, data_ref, q, idx, Jf);
409 
410  computeJointJacobians(model, data_ref, q);
411  updateFramePlacement(model, data_ref, idx);
412  getFrameJacobian(model, data_ref, idx, LOCAL, Jf_ref);
413 
414  BOOST_CHECK(Jf.isApprox(Jf_ref));
415 
416  computeFrameJacobian(model,data,q,idx,LOCAL,Jf2);
417 
418  BOOST_CHECK(Jf2.isApprox(Jf_ref));
419 }
420 
421 BOOST_AUTO_TEST_CASE ( test_frame_jacobian_local_world_oriented )
422 {
423  using namespace Eigen;
424  using namespace pinocchio;
425 
426  Model model;
427  buildModels::humanoidRandom(model);
428  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
429  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
430  const SE3 & framePlacement = SE3::Random();
431  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
432  BOOST_CHECK(model.existFrame(frame_name));
433 
434  pinocchio::Data data(model);
435 
436  model.lowerPositionLimit.head<7>().fill(-1.);
437  model.upperPositionLimit.head<7>().fill( 1.);
438  VectorXd q = randomConfiguration(model);
439  VectorXd v = VectorXd::Ones(model.nv);
440 
441  Model::Index idx = model.getFrameId(frame_name);
442  Data::Matrix6x Jf(6,model.nv); Jf.fill(0);
443  Data::Matrix6x Jf2(6,model.nv); Jf2.fill(0);
444  Data::Matrix6x Jf_ref(6,model.nv); Jf_ref.fill(0);
445 
446  computeJointJacobians(model, data, q);
447  updateFramePlacement(model, data, idx);
448  getFrameJacobian(model, data, idx, LOCAL, Jf_ref);
449 
450  // Compute the jacobians.
451  Jf_ref = SE3(data.oMf[idx].rotation(), Eigen::Vector3d::Zero()).toActionMatrix() * Jf_ref;
452  getFrameJacobian(model, data, idx, LOCAL_WORLD_ALIGNED, Jf);
453 
454  BOOST_CHECK(Jf.isApprox(Jf_ref));
455 
456  computeFrameJacobian(model,data,q,idx,LOCAL_WORLD_ALIGNED,Jf2);
457 
458  BOOST_CHECK(Jf2.isApprox(Jf_ref));
459 }
460 
461 BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
462 {
463  using namespace Eigen;
464  using namespace pinocchio;
465 
466  pinocchio::Model model;
467  pinocchio::buildModels::humanoidRandom(model);
468  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
469  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
470  const SE3 & framePlacement = SE3::Random();
471  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
472  pinocchio::Data data(model);
473  pinocchio::Data data_ref(model);
474 
475  VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
476  VectorXd v = VectorXd::Random(model.nv);
477  VectorXd a = VectorXd::Random(model.nv);
478 
479  computeJointJacobiansTimeVariation(model,data,q,v);
480  updateFramePlacements(model,data);
481 
482  forwardKinematics(model,data_ref,q,v,a);
483  updateFramePlacements(model,data_ref);
484 
485  BOOST_CHECK(isFinite(data.dJ));
486 
487  Model::Index idx = model.getFrameId(frame_name);
488  const Frame & frame = model.frames[idx];
489  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
490 
491  Data::Matrix6x J(6,model.nv); J.fill(0.);
492  Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
493 
494  // Regarding to the WORLD origin
495  getFrameJacobian(model,data,idx,WORLD,J);
496  getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ);
497 
498  Motion v_idx(J*v);
499  const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]);
500  const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local);
501 
502  const SE3 & wMf = SE3(data_ref.oMf[idx].rotation(),SE3::Vector3::Zero());
503  const Motion & v_ref_local_world_aligned = wMf.act(v_ref_local);
504  BOOST_CHECK(v_idx.isApprox(v_ref));
505 
506  Motion a_idx(J*a + dJ*v);
507  const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]);
508  const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local);
509  const Motion & a_ref_local_world_aligned = wMf.act(a_ref_local);
510  BOOST_CHECK(a_idx.isApprox(a_ref));
511 
512  J.fill(0.); dJ.fill(0.);
513  // Regarding to the LOCAL frame
514  getFrameJacobian(model,data,idx,LOCAL,J);
515  getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ);
516 
517  v_idx = (Motion::Vector6)(J*v);
518  BOOST_CHECK(v_idx.isApprox(v_ref_local));
519 
520  a_idx = (Motion::Vector6)(J*a + dJ*v);
521  BOOST_CHECK(a_idx.isApprox(a_ref_local));
522 
523  // Regarding to the LOCAL_WORLD_ALIGNED frame
524  getFrameJacobian(model,data,idx,LOCAL_WORLD_ALIGNED,J);
525  getFrameJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ);
526 
527  v_idx = (Motion::Vector6)(J*v);
528  BOOST_CHECK(v_idx.isApprox(v_ref_local_world_aligned));
529 
530  a_idx = (Motion::Vector6)(J*a + dJ*v);
531  BOOST_CHECK(a_idx.isApprox(a_ref_local_world_aligned));
532 
533  // compare to finite differencies
534  {
535  Data data_ref(model), data_ref_plus(model);
536 
537  const double alpha = 1e-8;
538  Eigen::VectorXd q_plus(model.nq);
539  q_plus = integrate(model,q,alpha*v);
540 
541  //data_ref
542  Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv), J_ref_local_world_aligned(6,model.nv);
543  J_ref_world.fill(0.); J_ref_local.fill(0.); J_ref_local_world_aligned.fill(0.);
544  computeJointJacobians(model,data_ref,q);
545  updateFramePlacements(model,data_ref);
546  const SE3 & oMf_q = data_ref.oMf[idx];
547  getFrameJacobian(model,data_ref,idx,WORLD,J_ref_world);
548  getFrameJacobian(model,data_ref,idx,LOCAL,J_ref_local);
549  getFrameJacobian(model,data_ref,idx,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
550 
551  //data_ref_plus
552  Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv), J_ref_plus_local_world_aligned(6,model.nv);
553  J_ref_plus_world.fill(0.); J_ref_plus_local.fill(0.); J_ref_plus_local_world_aligned.fill(0.);
554  computeJointJacobians(model,data_ref_plus,q_plus);
555  updateFramePlacements(model,data_ref_plus);
556  const SE3 & oMf_q_plus = data_ref_plus.oMf[idx];
557  getFrameJacobian(model,data_ref_plus,idx,WORLD,J_ref_plus_world);
558  getFrameJacobian(model,data_ref_plus,idx,LOCAL,J_ref_plus_local);
559  getFrameJacobian(model,data_ref_plus,idx,LOCAL_WORLD_ALIGNED,J_ref_plus_local_world_aligned);
560 
561  //Move J_ref_plus_local to reference frame
562  J_ref_plus_local = (oMf_q.inverse()*oMf_q_plus).toActionMatrix()*(J_ref_plus_local);
563 
564  //Move J_ref_plus_local_world_aligned to reference frame
565  SE3 oMf_translation = SE3::Identity();
566  oMf_translation.translation() = oMf_q_plus.translation() - oMf_q.translation();
567  J_ref_plus_local_world_aligned = oMf_translation.toActionMatrix()*(J_ref_plus_local_world_aligned);
568 
569  Data::Matrix6x dJ_ref_world(6,model.nv), dJ_ref_local(6,model.nv), dJ_ref_local_world_aligned(6,model.nv);
570  dJ_ref_world.fill(0.); dJ_ref_local.fill(0.); dJ_ref_local_world_aligned.fill(0.);
571  dJ_ref_world = (J_ref_plus_world - J_ref_world)/alpha;
572  dJ_ref_local = (J_ref_plus_local - J_ref_local)/alpha;
573  dJ_ref_local_world_aligned = (J_ref_plus_local_world_aligned - J_ref_local_world_aligned)/alpha;
574 
575  //data
576  computeJointJacobiansTimeVariation(model,data,q,v);
577  forwardKinematics(model,data,q,v);
578  updateFramePlacements(model,data);
579  Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv), dJ_local_world_aligned(6,model.nv);
580  dJ_world.fill(0.); dJ_local.fill(0.); dJ_local_world_aligned.fill(0.);
581  getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ_world);
582  getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ_local);
583  getFrameJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ_local_world_aligned);
584 
585  BOOST_CHECK(dJ_world.isApprox(dJ_ref_world,sqrt(alpha)));
586  BOOST_CHECK(dJ_local.isApprox(dJ_ref_local,sqrt(alpha)));
587  BOOST_CHECK(dJ_local_world_aligned.isApprox(dJ_ref_local_world_aligned,sqrt(alpha)));
588  }
589 }
590 
591 BOOST_AUTO_TEST_SUITE_END ()
592 
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: src/multibody/data.hpp:281
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:746
pinocchio::WORLD
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
Definition: src/multibody/fwd.hpp:46
pinocchio::FrameIndex
Index FrameIndex
Definition: src/multibody/fwd.hpp:30
pinocchio::computeFrameJacobian
void computeFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const FrameIndex frameId, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
Computes the Jacobian of a specific Frame expressed in the desired reference_frame given as argument...
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::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: src/multibody/frame.hpp:32
pinocchio::Frame
FrameTpl< double > Frame
Definition: src/multibody/fwd.hpp:33
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
pinocchio::ModelTpl::njoints
int njoints
Number of joints.
Definition: src/multibody/model.hpp:82
pinocchio::ModelTpl::names
std::vector< std::string > names
Name of joint i
Definition: src/multibody/model.hpp:115
pinocchio::getFrameVelocity
MotionTpl< Scalar, Options > getFrameVelocity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf=LOCAL)
Returns the spatial velocity of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement and velocity values in data structure.
pinocchio::cast
NewScalar cast(const Scalar &value)
Definition: cast.hpp:13
pinocchio::ModelTpl::lowerPositionLimit
ConfigVectorType lowerPositionLimit
Lower joint configuration limit.
Definition: src/multibody/model.hpp:139
pinocchio::ModelTpl::frames
FrameVector frames
Vector of operational frames registered on the model.
Definition: src/multibody/model.hpp:145
array
bn::ndarray array()
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::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...
data
data
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:746
pinocchio::motionSet::act
static void act(const Eigen::MatrixBase< Mat > &iV, const ForceDense< ForceDerived > &f, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion set on a force object. The input motion set is represented by a 6xN matrix whose e...
pinocchio::getFrameAcceleration
MotionTpl< Scalar, Options > getFrameAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf=LOCAL)
Returns the spatial acceleration of the Frame expressed in the desired reference frame. You must first call pinocchio::forwardKinematics to update placement, velocity and acceleration values in data structure.
pinocchio::ModelTpl::existJointName
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
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::SE3Base::toActionMatrix
ActionMatrixType toActionMatrix() const
The action matrix of .
Definition: se3-base.hpp:60
pinocchio::SE3Base::actInv
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition: se3-base.hpp:97
pinocchio::ModelTpl::FrameIndex
pinocchio::FrameIndex FrameIndex
Definition: src/multibody/model.hpp:51
pinocchio::MotionDense::isApprox
bool isApprox(const MotionDense< M2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: motion-dense.hpp:162
pinocchio::MotionBase::linear
ConstLinearType linear() const
Definition: motion-base.hpp:22
pinocchio::ModelTpl::addJoint
JointIndex addJoint(const JointIndex parent, const JointModel &joint_model, const SE3 &joint_placement, const std::string &joint_name)
Add a joint to the kinematic tree with infinite bounds.
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: spatial/se3-tpl.hpp:115
isFinite
bool isFinite(const Eigen::MatrixBase< Derived > &x)
Definition: frames.cpp:21
pinocchio::MotionTpl::Zero
static MotionTpl Zero()
Definition: motion-tpl.hpp:89
pinocchio::updateFramePlacement
const DataTpl< Scalar, Options, JointCollectionTpl >::SE3 & updateFramePlacement(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id)
Updates the placement of the given frame.
pinocchio::LOCAL
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
Definition: src/multibody/fwd.hpp:47
pinocchio::JointModelRZ
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
Definition: joint-revolute.hpp:752
pinocchio::Motion
MotionTpl< double, 0 > Motion
Definition: src/spatial/fwd.hpp:57
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...
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(frame_basic)
Definition: frames.cpp:29
pinocchio::ModelTpl::getFrameId
FrameIndex getFrameId(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Returns the index of a frame given by its name.
pinocchio::ModelTpl::existFrame
bool existFrame(const std::string &name, const FrameType &type=(FrameType)(JOINT|FIXED_JOINT|BODY|OP_FRAME|SENSOR)) const
Checks if a frame given by its name exists.
pinocchio::SE3Tpl::isApprox_impl
bool isApprox_impl(const SE3Tpl< Scalar, O2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: spatial/se3-tpl.hpp:267
pinocchio::SE3Base::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:38
pinocchio::SE3Base::act
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:90
pinocchio::ModelTpl::addFrame
FrameIndex addFrame(const Frame &frame, const bool append_inertia=true)
Adds a frame to the kinematic tree. The inertia stored within the frame will be happened to the inert...
pinocchio::FrameTpl::placement
SE3 placement
Placement of the frame wrt the parent joint.
Definition: src/multibody/frame.hpp:129
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:30
pinocchio::JointIndex
Index JointIndex
Definition: src/multibody/fwd.hpp:28
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::ModelTpl::getJointId
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
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.
a
list a
pinocchio::getFrameJacobianTimeVariation
void getFrameJacobianTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xLike > &dJ)
Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the ...
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:609
pinocchio::ModelTpl::upperPositionLimit
ConfigVectorType upperPositionLimit
Upper joint configuration limit.
Definition: src/multibody/model.hpp:142
continuous.x
x
— Training
Definition: continuous.py:157
pinocchio::MotionBase::angular
ConstAngularType angular() const
Definition: motion-base.hpp:21
pinocchio::SE3
SE3Tpl< double, 0 > SE3
Definition: src/spatial/fwd.hpp:53
pinocchio::getFrameJacobian
void getFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xLike > &J)
Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or ...
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:71
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:746
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: src/multibody/model.hpp:76
pinocchio::SE3Base::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
pinocchio::framesForwardKinematics
void framesForwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
First calls the forwardKinematics on the model, then computes the placement of each frame...
pinocchio::getFrameClassicalAcceleration
MotionTpl< Scalar, Options > getFrameClassicalAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf=LOCAL)
Returns the "classical" acceleration of the Frame expressed in the desired reference frame...
pinocchio::FrameTpl::cast
FrameTpl< NewScalar, Options > cast() const
Definition: src/multibody/frame.hpp:105
fill
void fill(Eigen::Ref< MatType > mat, const typename MatType::Scalar &value)

pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:03