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/model.hpp"
8 #include "pinocchio/algorithm/jacobian.hpp"
9 #include "pinocchio/algorithm/frames.hpp"
10 #include "pinocchio/algorithm/rnea.hpp"
11 #include "pinocchio/algorithm/crba.hpp"
12 #include "pinocchio/spatial/act-on-set.hpp"
13 #include "pinocchio/parsers/sample-models.hpp"
14 #include "pinocchio/utils/timer.hpp"
15 #include "pinocchio/algorithm/joint-configuration.hpp"
16 
17 #include <iostream>
18 
19 #include <boost/test/unit_test.hpp>
20 #include <boost/utility/binary.hpp>
21 
22 template<typename Derived>
23 inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
24 {
25  return ((x - x).array() == (x - x).array()).all();
26 }
27 
28 
29 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
30 
31 BOOST_AUTO_TEST_CASE(frame_basic)
32 {
33  using namespace pinocchio;
34  Model model;
35  buildModels::humanoidRandom(model);
36 
37  BOOST_CHECK(model.frames.size() >= size_t(model.njoints));
38  for(Model::FrameVector::const_iterator it = model.frames.begin();
39  it != model.frames.end(); ++it)
40  {
41  const Frame & frame = *it;
42  BOOST_CHECK(frame == frame);
43  Frame frame_copy(frame);
44  BOOST_CHECK(frame_copy == frame);
45  }
46 
47  std::ostringstream os;
48  os << Frame("toto",0,0,SE3::Random(),OP_FRAME) << std::endl;
49  BOOST_CHECK(!os.str().empty());
50 }
51 
52 BOOST_AUTO_TEST_CASE(cast)
53 {
54  using namespace pinocchio;
55  Frame frame("toto",0,0,SE3::Random(),OP_FRAME);
56 
57  BOOST_CHECK(frame.cast<double>() == frame);
58  BOOST_CHECK(frame.cast<double>().cast<long double>() == frame.cast<long double>());
59 }
60 
61 BOOST_AUTO_TEST_CASE ( test_kinematics )
62 {
63  using namespace Eigen;
64  using namespace pinocchio;
65 
66  pinocchio::Model model;
67  pinocchio::buildModels::humanoidRandom(model);
68  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
69  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
70  const SE3 & framePlacement = SE3::Random();
71  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
72  pinocchio::Data data(model);
73 
74  VectorXd q = VectorXd::Ones(model.nq);
75  q.middleRows<4> (3).normalize();
76  framesForwardKinematics(model, data, q);
77 
78  BOOST_CHECK(data.oMf[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));
79 
80 }
81 
82 BOOST_AUTO_TEST_CASE ( test_update_placements )
83 {
84  using namespace Eigen;
85  using namespace pinocchio;
86 
87  pinocchio::Model model;
88  pinocchio::buildModels::humanoidRandom(model);
89  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
90  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
91  const SE3 & framePlacement = SE3::Random();
92  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
93  pinocchio::Data data(model);
94  pinocchio::Data data_ref(model);
95 
96  VectorXd q = VectorXd::Ones(model.nq);
97  q.middleRows<4> (3).normalize();
98 
99  forwardKinematics(model, data, q);
100  updateFramePlacements(model, data);
101 
102  framesForwardKinematics(model, data_ref, q);
103 
104  BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
105 }
106 
107 BOOST_AUTO_TEST_CASE ( test_update_single_placement )
108 {
109  using namespace Eigen;
110  using namespace pinocchio;
111 
112  pinocchio::Model model;
113  pinocchio::buildModels::humanoidRandom(model);
114  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
115  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
116  const SE3 & framePlacement = SE3::Random();
117  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
118  pinocchio::Data data(model);
119  pinocchio::Data data_ref(model);
120 
121  VectorXd q = VectorXd::Ones(model.nq);
122  q.middleRows<4> (3).normalize();
123 
124  forwardKinematics(model, data, q);
125  updateFramePlacement(model, data, frame_idx);
126 
127  framesForwardKinematics(model, data_ref, q);
128 
129  BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
130 }
131 
132 BOOST_AUTO_TEST_CASE ( test_velocity )
133 {
134  using namespace Eigen;
135  using namespace pinocchio;
136 
137  pinocchio::Model model;
138  pinocchio::buildModels::humanoidRandom(model);
139  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
140  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
141  const SE3 & framePlacement = SE3::Random();
142  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
143  pinocchio::Data data(model);
144 
145  VectorXd q = VectorXd::Ones(model.nq);
146  q.middleRows<4> (3).normalize();
147  VectorXd v = VectorXd::Ones(model.nv);
148  forwardKinematics(model, data, q, v);
149 
150  Motion vf = getFrameVelocity(model, data, frame_idx);
151 
152  BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx])));
153 
154  pinocchio::Data data_ref(model);
155  forwardKinematics(model, data_ref, q, v);
156  updateFramePlacements(model, data_ref);
157  Motion v_ref = getFrameVelocity(model, data_ref, frame_idx);
158 
159  BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model,data,frame_idx)));
160  BOOST_CHECK(v_ref.isApprox(getFrameVelocity(model,data,frame_idx,LOCAL)));
161  BOOST_CHECK(data_ref.oMf[frame_idx].act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,WORLD)));
162  BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(v_ref).isApprox(getFrameVelocity(model,data,frame_idx,LOCAL_WORLD_ALIGNED)));
163 }
164 
165 BOOST_AUTO_TEST_CASE ( test_acceleration )
166 {
167  using namespace Eigen;
168  using namespace pinocchio;
169 
170  pinocchio::Model model;
171  pinocchio::buildModels::humanoidRandom(model);
172  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
173  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
174  const SE3 & framePlacement = SE3::Random();
175  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
176  pinocchio::Data data(model);
177 
178  VectorXd q = VectorXd::Ones(model.nq);
179  q.middleRows<4> (3).normalize();
180  VectorXd v = VectorXd::Ones(model.nv);
181  VectorXd a = VectorXd::Ones(model.nv);
182  forwardKinematics(model, data, q, v, a);
183 
184  Motion af = getFrameAcceleration(model, data, frame_idx);
185 
186  BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
187 
188  pinocchio::Data data_ref(model);
189  forwardKinematics(model, data_ref, q, v, a);
190  updateFramePlacements(model, data_ref);
191  Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx);
192 
193  BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx)));
194  BOOST_CHECK(a_ref.isApprox(getFrameAcceleration(model,data,frame_idx,LOCAL)));
195  BOOST_CHECK(data_ref.oMf[frame_idx].act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,WORLD)));
196  BOOST_CHECK(SE3(data_ref.oMf[frame_idx].rotation(), Eigen::Vector3d::Zero()).act(a_ref).isApprox(getFrameAcceleration(model,data,frame_idx,LOCAL_WORLD_ALIGNED)));
197 }
198 
199 BOOST_AUTO_TEST_CASE ( test_classic_acceleration )
200 {
201  using namespace Eigen;
202  using namespace pinocchio;
203 
204  pinocchio::Model model;
205  pinocchio::buildModels::humanoidRandom(model);
206  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
207  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
208  const SE3 & framePlacement = SE3::Random();
209  Model::FrameIndex frame_idx = model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
210  pinocchio::Data data(model);
211 
212  VectorXd q = VectorXd::Ones(model.nq);
213  q.middleRows<4> (3).normalize();
214  VectorXd v = VectorXd::Ones(model.nv);
215  VectorXd a = VectorXd::Ones(model.nv);
216  forwardKinematics(model, data, q, v, a);
217 
218  Motion vel = framePlacement.actInv(data.v[parent_idx]);
219  Motion acc = framePlacement.actInv(data.a[parent_idx]);
220  Vector3d linear;
221 
222  Motion acc_classical_local = acc;
223  linear = acc.linear() + vel.angular().cross(vel.linear());
224  acc_classical_local.linear() = linear;
225 
226  Motion af = getFrameClassicalAcceleration(model, data, frame_idx);
227 
228  BOOST_CHECK(af.isApprox(acc_classical_local));
229 
230  pinocchio::Data data_ref(model);
231  forwardKinematics(model, data_ref, q, v, a);
232  updateFramePlacements(model, data_ref);
233 
234  SE3 T_ref = data_ref.oMf[frame_idx];
235  Motion v_ref = getFrameVelocity(model, data_ref, frame_idx);
236  Motion a_ref = getFrameAcceleration(model, data_ref, frame_idx);
237 
238  Motion acc_classical_local_ref = a_ref;
239  linear = a_ref.linear() + v_ref.angular().cross(v_ref.linear());
240  acc_classical_local_ref.linear() = linear;
241 
242  BOOST_CHECK(acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx)));
243  BOOST_CHECK(acc_classical_local_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,LOCAL)));
244 
245  Motion vel_world_ref = T_ref.act(v_ref);
246  Motion acc_classical_world_ref = T_ref.act(a_ref);
247  linear = acc_classical_world_ref.linear() + vel_world_ref.angular().cross(vel_world_ref.linear());
248  acc_classical_world_ref.linear() = linear;
249 
250  BOOST_CHECK(acc_classical_world_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,WORLD)));
251 
252  Motion vel_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(v_ref);
253  Motion acc_classical_aligned_ref = SE3(T_ref.rotation(), Eigen::Vector3d::Zero()).act(a_ref);
254  linear = acc_classical_aligned_ref.linear() + vel_aligned_ref.angular().cross(vel_aligned_ref.linear());
255  acc_classical_aligned_ref.linear() = linear;
256 
257  BOOST_CHECK(acc_classical_aligned_ref.isApprox(getFrameClassicalAcceleration(model,data,frame_idx,LOCAL_WORLD_ALIGNED)));
258 }
259 
260 BOOST_AUTO_TEST_CASE(test_frame_getters)
261 {
262  using namespace Eigen;
263  using namespace pinocchio;
264 
265  // Build a simple 1R planar model
266  Model model;
267  JointIndex parentId = model.addJoint(0, JointModelRZ(), SE3::Identity(), "Joint1");
268  FrameIndex frameId = model.addFrame(Frame("Frame1", parentId, 0, SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), OP_FRAME));
269 
270  Data data(model);
271 
272  // Predetermined configuration values
273  VectorXd q(model.nq);
274  q << M_PI/2;
275 
276  VectorXd v(model.nv);
277  v << 1.0;
278 
279  VectorXd a(model.nv);
280  a << 0.0;
281 
282  // Expected velocity
283  Motion v_local;
284  v_local.linear() = Vector3d(0.0, 1.0, 0.0);
285  v_local.angular() = Vector3d(0.0, 0.0, 1.0);
286 
287  Motion v_world;
288  v_world.linear() = Vector3d::Zero();
289  v_world.angular() = Vector3d(0.0, 0.0, 1.0);
290 
291  Motion v_align;
292  v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
293  v_align.angular() = Vector3d(0.0, 0.0, 1.0);
294 
295  // Expected classical acceleration
296  Motion ac_local;
297  ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
298  ac_local.angular() = Vector3d::Zero();
299 
300  Motion ac_world = Motion::Zero();
301 
302  Motion ac_align;
303  ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
304  ac_align.angular() = Vector3d::Zero();
305 
306  // Perform kinematics
307  forwardKinematics(model,data,q,v,a);
308 
309  // Check output velocity
310  BOOST_CHECK(v_local.isApprox(getFrameVelocity(model,data,frameId)));
311  BOOST_CHECK(v_local.isApprox(getFrameVelocity(model,data,frameId,LOCAL)));
312  BOOST_CHECK(v_world.isApprox(getFrameVelocity(model,data,frameId,WORLD)));
313  BOOST_CHECK(v_align.isApprox(getFrameVelocity(model,data,frameId,LOCAL_WORLD_ALIGNED)));
314 
315  // Check output acceleration (all zero)
316  BOOST_CHECK(getFrameAcceleration(model,data,frameId).isZero());
317  BOOST_CHECK(getFrameAcceleration(model,data,frameId,LOCAL).isZero());
318  BOOST_CHECK(getFrameAcceleration(model,data,frameId,WORLD).isZero());
319  BOOST_CHECK(getFrameAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED).isZero());
320 
321  // Check output classical acceleration
322  BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model,data,frameId)));
323  BOOST_CHECK(ac_local.isApprox(getFrameClassicalAcceleration(model,data,frameId,LOCAL)));
324  BOOST_CHECK(ac_world.isApprox(getFrameClassicalAcceleration(model,data,frameId,WORLD)));
325  BOOST_CHECK(ac_align.isApprox(getFrameClassicalAcceleration(model,data,frameId,LOCAL_WORLD_ALIGNED)));
326 }
327 
328 BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian )
329 {
330  using namespace Eigen;
331  using namespace pinocchio;
332 
333  Model model;
334  buildModels::humanoidRandom(model);
335  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
336  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
337  const SE3 & framePlacement = SE3::Random();
338  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
339  BOOST_CHECK(model.existFrame(frame_name));
340 
341  pinocchio::Data data(model);
342  pinocchio::Data data_ref(model);
343 
344  model.lowerPositionLimit.head<7>().fill(-1.);
345  model.upperPositionLimit.head<7>().fill( 1.);
346  VectorXd q = randomConfiguration(model);
347  VectorXd v = VectorXd::Ones(model.nv);
348 
350  Model::Index idx = model.getFrameId(frame_name);
351  const Frame & frame = model.frames[idx];
352  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
353  Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
354  Data::Matrix6x Jff(6,model.nv); Jff.fill(0);
355  Data::Matrix6x Jff2(6,model.nv); Jff2.fill(0);
356 
357  computeJointJacobians(model,data,q);
358  updateFramePlacement(model, data, idx);
359  getFrameJacobian(model, data, idx, LOCAL, Jff);
360  computeJointJacobians(model,data_ref,q);
361  getJointJacobian(model, data_ref, parent_idx, LOCAL, Jjj);
362 
363  Motion nu_frame = Motion(Jff*v);
364  Motion nu_joint = Motion(Jjj*v);
365 
366  const SE3::ActionMatrixType jXf = frame.placement.toActionMatrix();
367  Data::Matrix6x Jjj_from_frame(jXf * Jff);
368  BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));
369 
370  BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));
371 
372  // In world frame
373  getFrameJacobian(model,data,idx,WORLD,Jff);
374  getJointJacobian(model, data_ref, parent_idx,WORLD, Jjj);
375  BOOST_CHECK(Jff.isApprox(Jjj));
376 
377  computeFrameJacobian(model,data,q,idx,WORLD,Jff2);
378 
379  BOOST_CHECK(Jff2.isApprox(Jjj));
380 }
381 
382 BOOST_AUTO_TEST_CASE ( test_frame_jacobian )
383 {
384  using namespace Eigen;
385  using namespace pinocchio;
386 
387  Model model;
388  buildModels::humanoidRandom(model);
389  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
390  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
391  const SE3 & framePlacement = SE3::Random();
392  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
393  BOOST_CHECK(model.existFrame(frame_name));
394 
395  pinocchio::Data data(model);
396  pinocchio::Data data_ref(model);
397 
398  model.lowerPositionLimit.head<7>().fill(-1.);
399  model.upperPositionLimit.head<7>().fill( 1.);
400  VectorXd q = randomConfiguration(model);
401  VectorXd v = VectorXd::Ones(model.nv);
402 
403  Model::Index idx = model.getFrameId(frame_name);
404  const Frame & frame = model.frames[idx];
405  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
406  Data::Matrix6x Jf(6,model.nv); Jf.fill(0);
407  Data::Matrix6x Jf2(6,model.nv); Jf2.fill(0);
408  Data::Matrix6x Jf_ref(6,model.nv); Jf_ref.fill(0);
409 
410  computeFrameJacobian(model, data_ref, q, idx, Jf);
411 
412  computeJointJacobians(model, data_ref, q);
413  updateFramePlacement(model, data_ref, idx);
414  getFrameJacobian(model, data_ref, idx, LOCAL, Jf_ref);
415 
416  BOOST_CHECK(Jf.isApprox(Jf_ref));
417 
418  computeFrameJacobian(model,data,q,idx,LOCAL,Jf2);
419 
420  BOOST_CHECK(Jf2.isApprox(Jf_ref));
421 }
422 
423 BOOST_AUTO_TEST_CASE ( test_frame_jacobian_local_world_oriented )
424 {
425  using namespace Eigen;
426  using namespace pinocchio;
427 
428  Model model;
429  buildModels::humanoidRandom(model);
430  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
431  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
432  const SE3 & framePlacement = SE3::Random();
433  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
434  BOOST_CHECK(model.existFrame(frame_name));
435 
436  pinocchio::Data data(model);
437 
438  model.lowerPositionLimit.head<7>().fill(-1.);
439  model.upperPositionLimit.head<7>().fill( 1.);
440  VectorXd q = randomConfiguration(model);
441  VectorXd v = VectorXd::Ones(model.nv);
442 
443  Model::Index idx = model.getFrameId(frame_name);
444  Data::Matrix6x Jf(6,model.nv); Jf.fill(0);
445  Data::Matrix6x Jf2(6,model.nv); Jf2.fill(0);
446  Data::Matrix6x Jf_ref(6,model.nv); Jf_ref.fill(0);
447 
448  computeJointJacobians(model, data, q);
449  updateFramePlacement(model, data, idx);
450  getFrameJacobian(model, data, idx, LOCAL, Jf_ref);
451 
452  // Compute the jacobians.
453  Jf_ref = SE3(data.oMf[idx].rotation(), Eigen::Vector3d::Zero()).toActionMatrix() * Jf_ref;
454  getFrameJacobian(model, data, idx, LOCAL_WORLD_ALIGNED, Jf);
455 
456  BOOST_CHECK(Jf.isApprox(Jf_ref));
457 
458  computeFrameJacobian(model,data,q,idx,LOCAL_WORLD_ALIGNED,Jf2);
459 
460  BOOST_CHECK(Jf2.isApprox(Jf_ref));
461 }
462 
463 BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
464 {
465  using namespace Eigen;
466  using namespace pinocchio;
467 
468  pinocchio::Model model;
469  pinocchio::buildModels::humanoidRandom(model);
470  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
471  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
472  const SE3 & framePlacement = SE3::Random();
473  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
474  pinocchio::Data data(model);
475  pinocchio::Data data_ref(model);
476 
477  VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
478  VectorXd v = VectorXd::Random(model.nv);
479  VectorXd a = VectorXd::Random(model.nv);
480 
481  computeJointJacobiansTimeVariation(model,data,q,v);
482  updateFramePlacements(model,data);
483 
484  forwardKinematics(model,data_ref,q,v,a);
485  updateFramePlacements(model,data_ref);
486 
487  BOOST_CHECK(isFinite(data.dJ));
488 
489  Model::Index idx = model.getFrameId(frame_name);
490  const Frame & frame = model.frames[idx];
491  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
492 
493  Data::Matrix6x J(6,model.nv); J.fill(0.);
494  Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
495 
496  // Regarding to the WORLD origin
497  getFrameJacobian(model,data,idx,WORLD,J);
498  getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ);
499 
500  Motion v_idx(J*v);
501  const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]);
502  const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local);
503 
504  const SE3 & wMf = SE3(data_ref.oMf[idx].rotation(),SE3::Vector3::Zero());
505  const Motion & v_ref_local_world_aligned = wMf.act(v_ref_local);
506  BOOST_CHECK(v_idx.isApprox(v_ref));
507 
508  Motion a_idx(J*a + dJ*v);
509  const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]);
510  const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local);
511  const Motion & a_ref_local_world_aligned = wMf.act(a_ref_local);
512  BOOST_CHECK(a_idx.isApprox(a_ref));
513 
514  J.fill(0.); dJ.fill(0.);
515  // Regarding to the LOCAL frame
516  getFrameJacobian(model,data,idx,LOCAL,J);
517  getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ);
518 
519  v_idx = (Motion::Vector6)(J*v);
520  BOOST_CHECK(v_idx.isApprox(v_ref_local));
521 
522  a_idx = (Motion::Vector6)(J*a + dJ*v);
523  BOOST_CHECK(a_idx.isApprox(a_ref_local));
524 
525  // Regarding to the LOCAL_WORLD_ALIGNED frame
526  getFrameJacobian(model,data,idx,LOCAL_WORLD_ALIGNED,J);
527  getFrameJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ);
528 
529  v_idx = (Motion::Vector6)(J*v);
530  BOOST_CHECK(v_idx.isApprox(v_ref_local_world_aligned));
531 
532  a_idx = (Motion::Vector6)(J*a + dJ*v);
533  BOOST_CHECK(a_idx.isApprox(a_ref_local_world_aligned));
534 
535  // compare to finite differencies
536  {
537  Data data_ref(model), data_ref_plus(model);
538 
539  const double alpha = 1e-8;
540  Eigen::VectorXd q_plus(model.nq);
541  q_plus = integrate(model,q,alpha*v);
542 
543  //data_ref
544  Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv), J_ref_local_world_aligned(6,model.nv);
545  J_ref_world.fill(0.); J_ref_local.fill(0.); J_ref_local_world_aligned.fill(0.);
546  computeJointJacobians(model,data_ref,q);
547  updateFramePlacements(model,data_ref);
548  const SE3 & oMf_q = data_ref.oMf[idx];
549  getFrameJacobian(model,data_ref,idx,WORLD,J_ref_world);
550  getFrameJacobian(model,data_ref,idx,LOCAL,J_ref_local);
551  getFrameJacobian(model,data_ref,idx,LOCAL_WORLD_ALIGNED,J_ref_local_world_aligned);
552 
553  //data_ref_plus
554  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);
555  J_ref_plus_world.fill(0.); J_ref_plus_local.fill(0.); J_ref_plus_local_world_aligned.fill(0.);
556  computeJointJacobians(model,data_ref_plus,q_plus);
557  updateFramePlacements(model,data_ref_plus);
558  const SE3 & oMf_q_plus = data_ref_plus.oMf[idx];
559  getFrameJacobian(model,data_ref_plus,idx,WORLD,J_ref_plus_world);
560  getFrameJacobian(model,data_ref_plus,idx,LOCAL,J_ref_plus_local);
561  getFrameJacobian(model,data_ref_plus,idx,LOCAL_WORLD_ALIGNED,J_ref_plus_local_world_aligned);
562 
563  //Move J_ref_plus_local to reference frame
564  J_ref_plus_local = (oMf_q.inverse()*oMf_q_plus).toActionMatrix()*(J_ref_plus_local);
565 
566  //Move J_ref_plus_local_world_aligned to reference frame
567  SE3 oMf_translation = SE3::Identity();
568  oMf_translation.translation() = oMf_q_plus.translation() - oMf_q.translation();
569  J_ref_plus_local_world_aligned = oMf_translation.toActionMatrix()*(J_ref_plus_local_world_aligned);
570 
571  Data::Matrix6x dJ_ref_world(6,model.nv), dJ_ref_local(6,model.nv), dJ_ref_local_world_aligned(6,model.nv);
572  dJ_ref_world.fill(0.); dJ_ref_local.fill(0.); dJ_ref_local_world_aligned.fill(0.);
573  dJ_ref_world = (J_ref_plus_world - J_ref_world)/alpha;
574  dJ_ref_local = (J_ref_plus_local - J_ref_local)/alpha;
575  dJ_ref_local_world_aligned = (J_ref_plus_local_world_aligned - J_ref_local_world_aligned)/alpha;
576 
577  //data
578  computeJointJacobiansTimeVariation(model,data,q,v);
579  forwardKinematics(model,data,q,v);
580  updateFramePlacements(model,data);
581  Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv), dJ_local_world_aligned(6,model.nv);
582  dJ_world.fill(0.); dJ_local.fill(0.); dJ_local_world_aligned.fill(0.);
583  getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ_world);
584  getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ_local);
585  getFrameJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ_local_world_aligned);
586 
587  BOOST_CHECK(dJ_world.isApprox(dJ_ref_world,sqrt(alpha)));
588  BOOST_CHECK(dJ_local.isApprox(dJ_ref_local,sqrt(alpha)));
589  BOOST_CHECK(dJ_local_world_aligned.isApprox(dJ_ref_local_world_aligned,sqrt(alpha)));
590  }
591 }
592 
593 BOOST_AUTO_TEST_CASE(test_supported_inertia_and_force)
594 {
595  using namespace Eigen;
596  using namespace pinocchio;
597 
598  // Create a humanoid robot
599  Model model_free;
600  pinocchio::buildModels::humanoid(model_free, true);
601  Data data_free(model_free);
602 
603  // Set freeflyer limits to allow for randomConfiguration
604  model_free.lowerPositionLimit.segment(0, 3) << Vector3d::Constant(-1);
605  model_free.upperPositionLimit.segment(0, 3) << Vector3d::Constant( 1);
606 
607  // Joint of interest (that will be converted to frame)
608  const std::string joint_name = "chest2_joint";
609  const JointIndex joint_id = model_free.getJointId(joint_name);
610 
611  // Duplicate of the model with the joint of interest fixed (to make a frame)
612  pinocchio::Model model_fix = buildReducedModel(model_free, {joint_id}, neutral(model_free));
613  Data data_fix(model_fix);
614  const FrameIndex frame_id = model_fix.getFrameId(joint_name);
615 
616  // Const variable to help convert q, v, a from one model to another
617  const int joint_idx_q(model_free.joints[joint_id].idx_q());
618  const int joint_idx_v(model_free.joints[joint_id].idx_v());
619 
620  // Pick random q, v, a
621  VectorXd q_free = randomConfiguration(model_free);
622  VectorXd v_free(VectorXd::Random(model_free.nv));
623  VectorXd a_free(VectorXd::Random(model_free.nv));
624 
625  // Set joint of interest to q, v, a = 0 to mimic the fixed joint
626  q_free[joint_idx_q] = 0;
627  v_free[joint_idx_v] = 0;
628  a_free[joint_idx_v] = 0;
629 
630  // Adapt configuration for fixed joint model
631  VectorXd q_fix(model_fix.nq);
632  q_fix << q_free.head(joint_idx_q), q_free.tail(model_free.nq - joint_idx_q - 1);
633  VectorXd v_fix(model_fix.nv);
634  v_fix << v_free.head(joint_idx_v), v_free.tail(model_free.nv - joint_idx_v - 1);
635  VectorXd a_fix(model_fix.nv);
636  a_fix << a_free.head(joint_idx_v), a_free.tail(model_free.nv - joint_idx_v - 1);
637 
638  // Compute inertia/force for both models differently
639  forwardKinematics(model_fix, data_fix, q_fix, v_fix, a_fix);
640  crba(model_free, data_free, q_free);
641 
642  Inertia inertia_fix = computeSupportedInertiaByFrame(model_fix, data_fix, frame_id, false);
643  Inertia inertia_free(model_free.inertias[joint_id]);
644  BOOST_CHECK(inertia_fix.isApprox(inertia_free));
645 
646  inertia_fix = computeSupportedInertiaByFrame(model_fix, data_fix, frame_id, true);
647  inertia_free = data_free.Ycrb[joint_id];
648  BOOST_CHECK(inertia_fix.isApprox(inertia_free));
649 
650  rnea(model_fix, data_fix, q_fix, v_fix, a_fix);
651  rnea(model_free, data_free, q_free, v_free, a_free);
652 
653  Force force_fix = computeSupportedForceByFrame(model_fix, data_fix, frame_id);
654  Force force_free(data_free.f[joint_id]);
655  BOOST_CHECK(force_fix.isApprox(force_free));
656 }
657 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::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::ForceDense::isApprox
bool isApprox(const ForceDense< M2 > &f, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: force-dense.hpp:134
pinocchio::FrameIndex
Index FrameIndex
Definition: src/multibody/fwd.hpp:30
pinocchio::ModelTpl::joints
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
Definition: src/multibody/model.hpp:97
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::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
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::SE3Base::actInv
SE3GroupAction< D >::ReturnType actInv(const D &d) const
by = aXb.actInv(ay)
Definition: se3-base.hpp:97
pinocchio::rnea
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Definition: parallel/rnea.hpp:32
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:41
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::computeSupportedInertiaByFrame
InertiaTpl< Scalar, Options > computeSupportedInertiaByFrame(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, bool with_subtree)
Compute the inertia supported by a specific frame (given by frame_id) expressed in the LOCAL frame...
pinocchio::ModelTpl::frames
FrameVector frames
Vector of operational frames registered on the model.
Definition: src/multibody/model.hpp:145
M_PI
#define M_PI
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::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::buildModels::humanoid
void humanoid(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a 28-DOF kinematic chain of a floating humanoid robot.
pinocchio::MotionBase::angular
ConstAngularType angular() const
Definition: motion-base.hpp:21
pinocchio::OP_FRAME
operational frame: user-defined frames that are defined at runtime
Definition: src/multibody/frame.hpp:30
pinocchio::SE3Base::toActionMatrix
ActionMatrixType toActionMatrix() const
The action matrix of .
Definition: se3-base.hpp:60
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::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::ModelTpl::FrameIndex
pinocchio::FrameIndex FrameIndex
Definition: src/multibody/model.hpp:51
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.
isFinite
bool isFinite(const Eigen::MatrixBase< Derived > &x)
Definition: frames.cpp:23
pinocchio::MotionTpl::Zero
static MotionTpl Zero()
Definition: motion-tpl.hpp:91
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: spatial/se3-tpl.hpp:115
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:764
pinocchio::Motion
MotionTpl< double, 0 > Motion
Definition: src/spatial/fwd.hpp:57
pinocchio::computeSupportedForceByFrame
ForceTpl< Scalar, Options > computeSupportedForceByFrame(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id)
Computes the force supported by a specific frame (given by frame_id) expressed in the LOCAL frame...
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::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:31
a
Vec3f a
BOOST_TEST_MODULE
#define BOOST_TEST_MODULE
pinocchio::FrameTpl::cast
FrameTpl< NewScalar, Options > cast() const
Definition: src/multibody/frame.hpp:114
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::SE3Base::rotation
ConstAngularRef rotation() const
Definition: se3-base.hpp:37
pinocchio::FrameTpl::placement
SE3 placement
Placement of the frame wrt the parent joint.
Definition: src/multibody/frame.hpp:138
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:28
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
Definition: joint-configuration.hpp:257
pinocchio::JointIndex
Index JointIndex
Definition: src/multibody/fwd.hpp: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::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::MotionDense::isApprox
bool isApprox(const MotionDense< M2 > &m2, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: motion-dense.hpp:171
pinocchio::SE3Base::act
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:90
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
pinocchio::InertiaBase::isApprox
bool isApprox(const Derived &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision()) const
Definition: src/spatial/inertia.hpp:103
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:73
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::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::buildReducedModel
void buildReducedModel(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, std::vector< JointIndex > list_of_joints_to_lock, const Eigen::MatrixBase< ConfigVectorType > &reference_configuration, ModelTpl< Scalar, Options, JointCollectionTpl > &reduced_model)
Build a reduced model from a given input model and a list of joint to lock.
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::SE3Base::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:38

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