unittest/centroidal.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 //
4 
5 #include "pinocchio/algorithm/crba.hpp"
6 #include "pinocchio/algorithm/centroidal.hpp"
7 #include "pinocchio/algorithm/rnea.hpp"
8 #include "pinocchio/algorithm/jacobian.hpp"
9 #include "pinocchio/algorithm/center-of-mass.hpp"
10 #include "pinocchio/algorithm/joint-configuration.hpp"
11 
12 #include "pinocchio/multibody/sample-models.hpp"
13 
14 #include "pinocchio/utils/timer.hpp"
15 
16 #include <iostream>
17 
18 #include <boost/test/unit_test.hpp>
19 #include <boost/utility/binary.hpp>
20 
21 #include "utils/model-generator.hpp"
22 
23 template<typename JointModel>
24 static void addJointAndBody(
25  pinocchio::Model & model,
26  const pinocchio::JointModelBase<JointModel> & joint,
27  const std::string & parent_name,
28  const std::string & name,
29  const pinocchio::SE3 placement = pinocchio::SE3::Random(),
30  bool setRandomLimits = true)
31 {
32  using namespace pinocchio;
33  typedef typename JointModel::ConfigVector_t CV;
34  typedef typename JointModel::TangentVector_t TV;
35 
36  Model::JointIndex idx;
37 
38  if (setRandomLimits)
39  idx = model.addJoint(
40  model.getJointId(parent_name), joint, SE3::Random(), name + "_joint",
41  TV::Random() + TV::Constant(1), TV::Random() + TV::Constant(1),
42  CV::Random() - CV::Constant(1), CV::Random() + CV::Constant(1));
43  else
44  idx = model.addJoint(model.getJointId(parent_name), joint, placement, name + "_joint");
45 
46  model.addJointFrame(idx);
47 
48  model.appendBodyToJoint(idx, Inertia::Random(), SE3::Identity());
49  model.addBodyFrame(name + "_body", idx);
50 }
51 
52 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
53 
54 BOOST_AUTO_TEST_CASE(test_ccrba)
55 {
56  pinocchio::Model model;
57  pinocchio::buildModels::humanoidRandom(model);
58  pinocchio::Data data(model), data_ref(model);
59 
60  model.lowerPositionLimit.head<3>().fill(-1.);
61  model.upperPositionLimit.head<3>().fill(1.);
62  Eigen::VectorXd q = randomConfiguration(model);
63  Eigen::VectorXd v = Eigen::VectorXd::Ones(model.nv);
64 
65  pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL);
66  data_ref.M.triangularView<Eigen::StrictlyLower>() =
67  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
68 
69  data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]);
70 
71  pinocchio::Data data_ref_other(model);
72  pinocchio::crba(model, data_ref_other, q, pinocchio::Convention::WORLD);
73  data_ref_other.M.triangularView<Eigen::StrictlyLower>() =
74  data_ref_other.M.transpose().triangularView<Eigen::StrictlyLower>();
75  BOOST_CHECK(data_ref_other.M.isApprox(data_ref.M));
76 
77  pinocchio::SE3 cMo(pinocchio::SE3::Matrix3::Identity(), -data_ref.Ycrb[0].lever());
78  BOOST_CHECK(data_ref.Ycrb[0].isApprox(data_ref_other.oYcrb[0]));
79 
80  ccrba(model, data, q, v);
81  BOOST_CHECK(data.com[0].isApprox(-cMo.translation(), 1e-12));
82  BOOST_CHECK(data.oYcrb[0].matrix().isApprox(data_ref.Ycrb[0].matrix(), 1e-12));
83 
84  pinocchio::Inertia Ig_ref(cMo.act(data.oYcrb[0]));
85  BOOST_CHECK(data.Ig.matrix().isApprox(Ig_ref.matrix(), 1e-12));
86 
87  pinocchio::SE3 oM1(data_ref.liMi[1]);
88  pinocchio::SE3 cM1(cMo * oM1);
89 
90  pinocchio::Data::Matrix6x Ag_ref(
91  cM1.inverse().toActionMatrix().transpose() * data_ref.M.topRows<6>());
92  BOOST_CHECK(data.Ag.isApprox(Ag_ref, 1e-12));
93 }
94 
95 BOOST_AUTO_TEST_CASE(test_ccrba_mimic)
96 {
97  for (int i = 0; i < pinocchio::MimicTestCases::N_CASES; i++)
98  {
99  const pinocchio::MimicTestCases mimic_test_case(i);
100  const pinocchio::Model & model_mimic = mimic_test_case.model_mimic;
101  const pinocchio::Model & model_full = mimic_test_case.model_full;
102  const Eigen::MatrixXd & G = mimic_test_case.G;
103 
104  pinocchio::Data data_mimic(model_mimic);
105  pinocchio::Data data_ref_mimic(model_mimic);
106  pinocchio::Data data_full(model_full);
107 
108  Eigen::VectorXd q = pinocchio::randomConfiguration(model_mimic);
109  Eigen::VectorXd v = Eigen::VectorXd::Random(model_mimic.nv);
110  Eigen::VectorXd a = Eigen::VectorXd::Random(model_mimic.nv);
111 
112  Eigen::VectorXd q_full(model_full.nq);
113  Eigen::VectorXd v_full = G * v;
114  Eigen::VectorXd a_full = G * a;
115 
116  mimic_test_case.toFull(q, q_full);
117 
118  pinocchio::crba(model_mimic, data_ref_mimic, q, pinocchio::Convention::LOCAL);
119  data_ref_mimic.M.triangularView<Eigen::StrictlyLower>() =
120  data_ref_mimic.M.transpose().triangularView<Eigen::StrictlyLower>();
121 
122  data_ref_mimic.Ycrb[0] = data_ref_mimic.liMi[1].act(data_ref_mimic.Ycrb[1]);
123  pinocchio::Data data_ref_other(model_mimic);
124  pinocchio::crba(model_mimic, data_ref_other, q, pinocchio::Convention::WORLD);
125  data_ref_other.M.triangularView<Eigen::StrictlyLower>() =
126  data_ref_other.M.transpose().triangularView<Eigen::StrictlyLower>();
127 
128  pinocchio::SE3 cMo(pinocchio::SE3::Matrix3::Identity(), -data_ref_mimic.Ycrb[0].lever());
129  BOOST_CHECK(data_ref_mimic.Ycrb[0].isApprox(data_ref_other.oYcrb[0]));
130 
131  ccrba(model_mimic, data_mimic, q, v);
132  ccrba(model_full, data_full, q_full, v_full);
133  BOOST_CHECK(data_mimic.com[0].isApprox(data_full.com[0], 1e-12));
134  BOOST_CHECK(data_mimic.oYcrb[0].matrix().isApprox(data_full.oYcrb[0].matrix(), 1e-12));
135 
136  BOOST_CHECK(data_mimic.com[0].isApprox(-cMo.translation(), 1e-12));
137  BOOST_CHECK(data_mimic.oYcrb[0].matrix().isApprox(data_ref_mimic.Ycrb[0].matrix(), 1e-12));
138 
139  pinocchio::Inertia Ig_ref(cMo.act(data_mimic.oYcrb[0]));
140  BOOST_CHECK(data_mimic.Ig.matrix().isApprox(Ig_ref.matrix(), 1e-12));
141 
142  pinocchio::SE3 oM1(data_ref_other.liMi[1]);
143  pinocchio::SE3 cM1(cMo * oM1);
144  pinocchio::Data::Matrix6x Ag_ref(
145  cM1.inverse().toActionMatrix().transpose() * data_ref_other.M.topRows<6>());
146  BOOST_CHECK(data_mimic.Ag.isApprox(Ag_ref, 1e-12));
147  }
148 }
149 
150 BOOST_AUTO_TEST_CASE(test_centroidal_mapping)
151 {
152  pinocchio::Model model;
153  pinocchio::buildModels::humanoidRandom(model);
154  pinocchio::Data data(model), data_ref(model);
155 
156  model.lowerPositionLimit.head<3>().fill(-1.);
157  model.upperPositionLimit.head<3>().fill(1.);
158  Eigen::VectorXd q = randomConfiguration(model);
159  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
160 
161  computeCentroidalMap(model, data, q);
162  ccrba(model, data_ref, q, v);
163 
164  BOOST_CHECK(data.J.isApprox(data_ref.J));
165  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
166 
167  computeJointJacobians(model, data_ref, q);
168 
169  BOOST_CHECK(data.J.isApprox(data_ref.J));
170 }
171 
172 BOOST_AUTO_TEST_CASE(test_dccrb)
173 {
174  using namespace pinocchio;
175  Model model;
176  buildModels::humanoidRandom(model);
177  addJointAndBody(model, JointModelSpherical(), "larm6_joint", "larm7");
178  Data data(model), data_ref(model);
179 
180  model.lowerPositionLimit.head<7>().fill(-1.);
181  model.upperPositionLimit.head<7>().fill(1.);
182 
183  Eigen::VectorXd q =
184  randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit);
185  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
186  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
187 
188  const Eigen::VectorXd g = rnea(model, data_ref, q, 0 * v, 0 * a);
189  rnea(model, data_ref, q, v, a);
190 
191  pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL);
192  data_ref.M.triangularView<Eigen::StrictlyLower>() =
193  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
194 
195  SE3 cMo(SE3::Identity());
196  data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]);
197  cMo.translation() = -data_ref.Ycrb[0].lever();
198 
199  SE3 oM1(data_ref.liMi[1]);
200  SE3 cM1(cMo * oM1);
201  Data::Matrix6x Ag_ref(cM1.toDualActionMatrix() * data_ref.M.topRows<6>());
202 
203  Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>())));
204 
205  ccrba(model, data_ref, q, v);
206  dccrba(model, data, q, v);
207  BOOST_CHECK(data.Ag.isApprox(Ag_ref));
208  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
209  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
210 
211  centerOfMass(model, data_ref, q, v, a);
212  BOOST_CHECK(data_ref.com[0].isApprox(data_ref.Ycrb[0].lever()));
213  BOOST_CHECK(data_ref.vcom[0].isApprox(data.hg.linear() / data_ref.M(0, 0)));
214  BOOST_CHECK(data_ref.vcom[0].isApprox(data.vcom[0]));
215  BOOST_CHECK(data_ref.acom[0].isApprox(hdot_ref.linear() / data_ref.M(0, 0)));
216 
217  Force hdot(data.Ag * a + data.dAg * v);
218  BOOST_CHECK(hdot.isApprox(hdot_ref));
219 
220  dccrba(model, data, q, 0 * v);
221  BOOST_CHECK(data.dAg.isZero());
222 
223  // Check that dYcrb is equal to doYcrb
224  {
225  // Compute dYcrb
226  Data data_ref(model), data_ref_plus(model), data(model);
227 
228  const double alpha = 1e-8;
229  Eigen::VectorXd q_plus(model.nq);
230  q_plus = integrate(model, q, alpha * v);
231 
232  forwardKinematics(model, data_ref, q);
233  pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL);
234  pinocchio::crba(model, data_ref_plus, q_plus, pinocchio::Convention::LOCAL);
235  forwardKinematics(model, data_ref_plus, q_plus);
236  dccrba(model, data, q, v);
237 
238  for (size_t i = 1; i < (size_t)model.njoints; ++i)
239  {
240  Inertia::Matrix6 dYcrb = (data_ref_plus.oMi[i].act(data_ref_plus.Ycrb[i]).matrix()
241  - data_ref.oMi[i].act(data_ref.Ycrb[i]).matrix())
242  / alpha;
243  BOOST_CHECK(data.doYcrb[i].isApprox(dYcrb, sqrt(alpha)));
244  }
245  }
246 
247  {
248  Data data(model);
249  ccrba(model, data_ref, q, v);
250  SE3 oMc_ref(SE3::Identity());
251  oMc_ref.translation() = data_ref.com[0];
252  const Data::Matrix6x Ag_ref = oMc_ref.toDualActionMatrix() * data_ref.Ag;
253  pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL);
254  const Data::Matrix6x Ag_ref_from_M =
255  data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>();
256 
257  const double alpha = 1e-8;
258  Eigen::VectorXd q_plus(model.nq);
259  q_plus = integrate(model, q, alpha * v);
260  ccrba(model, data_ref, q_plus, v);
261  SE3 oMc_ref_plus(SE3::Identity());
262  oMc_ref_plus.translation() = data_ref.com[0];
263  const Data::Matrix6x Ag_plus_ref = oMc_ref_plus.toDualActionMatrix() * data_ref.Ag;
264  pinocchio::crba(model, data_ref, q_plus, pinocchio::Convention::LOCAL);
265  const Data::Matrix6x Ag_plus_ref_from_M =
266  data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>();
267  const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref) / alpha;
268  const Data::Matrix6x dAg_ref_from_M = (Ag_plus_ref_from_M - Ag_ref_from_M) / alpha;
269 
270  dccrba(model, data, q, v);
271  SE3 oMc(SE3::Identity());
272  oMc.translation() = data.com[0];
273  Data::Matrix6x dAg = oMc.toDualActionMatrix() * data.dAg;
274  BOOST_CHECK(oMc.isApprox(oMc_ref));
275  BOOST_CHECK(dAg_ref.isApprox(dAg_ref_from_M, sqrt(alpha)));
276  }
277 
278  // Check for dAg/dt
279  {
280  Data data(model);
281  ccrba(model, data_ref, q, v);
282  const Data::Matrix6x Ag_ref = data_ref.Ag;
283 
284  const double alpha = 1e-8;
285  Eigen::VectorXd q_plus(model.nq);
286  q_plus = integrate(model, q, alpha * v);
287  ccrba(model, data_ref, q_plus, v);
288  const Data::Matrix6x Ag_plus_ref = data_ref.Ag;
289  const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref) / alpha;
290 
291  dccrba(model, data, q, v);
292  BOOST_CHECK(data.dAg.isApprox(dAg_ref, sqrt(alpha)));
293  }
294 
295  // Compute tensor dAg/dq
296  {
297  std::vector<Data::Matrix6x> dAgdq((size_t)model.nv, Data::Matrix6x::Zero(6, model.nv));
298  Data data(model), data_fd(model);
299  Eigen::VectorXd v_fd(Eigen::VectorXd::Zero(model.nv));
300  ccrba(model, data_fd, q, v);
301  SE3 oMc_ref(SE3::Identity());
302  oMc_ref.translation() = data_fd.com[0];
303 
304  Data::Matrix6x Ag0 = oMc_ref.toDualActionMatrix() * data_fd.Ag;
305  const Force hg0 = oMc_ref.act(data_fd.hg);
306 
307  Data::Matrix6x Ag_fd(6, model.nv);
308  Force hg_fd;
309  const double alpha = 1e-8;
310  Eigen::VectorXd q_plus(model.nq);
311  Data::Matrix6x dhdq(6, model.nv);
312  for (int k = 0; k < model.nv; ++k)
313  {
314  v_fd[k] = alpha;
315  q_plus = integrate(model, q, v_fd);
316  ccrba(model, data_fd, q_plus, v);
317  SE3 oMc_fd(SE3::Identity());
318  oMc_fd.translation() = data_fd.com[0];
319  Ag_fd = oMc_fd.toDualActionMatrix() * data_fd.Ag;
320  hg_fd = oMc_fd.act(data_fd.hg);
321  dAgdq[(size_t)k] = (Ag_fd - Ag0) / alpha;
322  dhdq.col(k) = (hg_fd - hg0).toVector() / alpha;
323  v_fd[k] = 0.;
324  }
325 
326  Data::Matrix6x dAg_ref(6, model.nv);
327  dAg_ref.setZero();
328  for (int k = 0; k < model.nv; ++k)
329  {
330  dAg_ref.col(k) = dAgdq[(size_t)k] * v;
331  }
332 
333  BOOST_CHECK(dhdq.isApprox(dAg_ref, sqrt(alpha)));
334  BOOST_CHECK((dAg_ref * v).isApprox(dhdq * v, sqrt(alpha)));
335  }
336 }
337 
338 BOOST_AUTO_TEST_CASE(test_centroidal_mapping_time_derivative)
339 {
340  pinocchio::Model model;
341  pinocchio::buildModels::humanoidRandom(model);
342  pinocchio::Data data(model), data_ref(model);
343 
344  model.lowerPositionLimit.head<3>().fill(-1.);
345  model.upperPositionLimit.head<3>().fill(1.);
346  Eigen::VectorXd q = randomConfiguration(model);
347  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
348 
349  computeCentroidalMapTimeVariation(model, data, q, v);
350  dccrba(model, data_ref, q, v);
351 
352  BOOST_CHECK(data.J.isApprox(data_ref.J));
353  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
354  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
355  BOOST_CHECK(data.dAg.isApprox(data_ref.dAg));
356 
357  computeJointJacobiansTimeVariation(model, data_ref, q, v);
358 
359  BOOST_CHECK(data.J.isApprox(data_ref.J));
360  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
361 }
362 
363 BOOST_AUTO_TEST_CASE(test_computeCentroidalMomentum_computeCentroidalMomentumTimeVariation)
364 {
365  using namespace pinocchio;
366  Model model;
367  buildModels::humanoidRandom(model);
368  addJointAndBody(model, JointModelSpherical(), "larm6_joint", "larm7");
369  Data data(model), data_fk1(model), data_fk2(model), data_ref(model);
370 
371  model.lowerPositionLimit.head<7>().fill(-1.);
372  model.upperPositionLimit.head<7>().fill(1.);
373 
374  Eigen::VectorXd q =
375  randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit);
376  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
377  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
378 
379  ccrba(model, data_ref, q, v);
380  forwardKinematics(model, data_ref, q, v);
381  centerOfMass(model, data_ref, q, v, false);
382  computeCentroidalMomentum(model, data, q, v);
383 
384  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
385  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
386  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
387  for (size_t k = 1; k < (size_t)model.njoints; ++k)
388  {
389  BOOST_CHECK(data.mass[k] == data_ref.mass[k]);
390  BOOST_CHECK(data.com[k].isApprox(data_ref.com[k]));
391  BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
392  }
393 
394  // Check other signature
395  forwardKinematics(model, data_fk1, q, v);
396  computeCentroidalMomentum(model, data_fk1);
397 
398  BOOST_CHECK(data_fk1.hg.isApprox(data.hg));
399 
400  computeCentroidalMomentumTimeVariation(model, data, q, v, a);
401  model.gravity.setZero();
402  rnea(model, data_ref, q, v, a);
403  dccrba(model, data_ref, q, v);
404  const Force hgdot(data_ref.Ag * a + data_ref.dAg * v);
405 
406  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
407  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
408  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
409  BOOST_CHECK(data.dhg.isApprox(hgdot));
410  for (size_t k = 1; k < (size_t)model.njoints; ++k)
411  {
412  BOOST_CHECK(data.mass[k] == data_ref.mass[k]);
413  BOOST_CHECK(data.com[k].isApprox(data_ref.com[k]));
414  BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
415  BOOST_CHECK(data.a[k].isApprox(data_ref.a_gf[k]));
416  BOOST_CHECK(data.f[k].isApprox(data_ref.f[k]));
417  }
418 
419  // Check other signature
420  forwardKinematics(model, data_fk2, q, v, a);
421  computeCentroidalMomentumTimeVariation(model, data_fk2);
422 
423  BOOST_CHECK(data_fk2.hg.isApprox(data.hg));
424  BOOST_CHECK(data_fk2.dhg.isApprox(data.dhg));
425 
426  // Check against finite differences
427  Data data_fd(model);
428  const double eps = 1e-8;
429  Eigen::VectorXd v_plus = v + eps * a;
430  Eigen::VectorXd q_plus = integrate(model, q, eps * v);
431 
432  const Force hg = computeCentroidalMomentum(model, data_fd, q, v);
433  const SE3::Vector3 com = data_fd.com[0];
434  const Force hg_plus = computeCentroidalMomentum(model, data_fd, q_plus, v_plus);
435  const SE3::Vector3 com_plus = data_fd.com[0];
436 
437  SE3 transform(SE3::Identity());
438  transform.translation() = com_plus - com;
439  Force dhg_ref = (transform.act(hg_plus) - hg) / eps;
440 
441  BOOST_CHECK(data.dhg.isApprox(dhg_ref, sqrt(eps)));
442 }
443 
444 BOOST_AUTO_TEST_SUITE_END()
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: multibody/data.hpp:199
display-shapes-meshcat.placement
placement
Definition: display-shapes-meshcat.py:24
pinocchio::addJointAndBody
void addJointAndBody(Model &model, const JointModelBase< D > &jmodel, const Model::JointIndex parent_id, const SE3 &joint_placement, const std::string &name, const Inertia &Y)
Definition: model-generator.hpp:15
transform
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::DataTpl::dAg
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
Definition: multibody/data.hpp:290
pinocchio::DataTpl::mass
std::vector< Scalar > mass
Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint ....
Definition: multibody/data.hpp:458
pinocchio::MimicTestCases::toFull
void toFull(const Eigen::VectorXd &q, Eigen::VectorXd &q_full) const
Definition: model-generator.hpp:241
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::JointModelBase
Definition: joint-model-base.hpp:78
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: multibody/data.hpp:377
pinocchio::SE3
context::SE3 SE3
Definition: spatial/fwd.hpp:59
pinocchio::Convention::WORLD
@ WORLD
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
pinocchio::name
std::string name(const LieGroupGenericTpl< LieGroupCollection > &lg)
Visit a LieGroupVariant to get the name of it.
addJointAndBody
static void addJointAndBody(pinocchio::Model &model, const pinocchio::JointModelBase< JointModel > &joint, const std::string &parent_name, const std::string &name, const pinocchio::SE3 placement=pinocchio::SE3::Random(), bool setRandomLimits=true)
Definition: unittest/centroidal.cpp:24
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::JointModelSpherical
JointModelSphericalTpl< context::Scalar > JointModelSpherical
Definition: multibody/joint/fwd.hpp:73
mimic_dynamics.a_full
a_full
Definition: mimic_dynamics.py:49
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, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
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::MimicTestCases::G
Eigen::MatrixXd G
Definition: model-generator.hpp:134
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::MimicTestCases::N_CASES
static const int N_CASES
Definition: model-generator.hpp:128
pinocchio::dccrba
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & dccrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration...
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true, bool mimic=false)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
model-generator.hpp
rnea.hpp
pinocchio::computeCentroidalMapTimeVariation
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeCentroidalMapTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Centroidal Momentum Matrix time derivative.
timer.hpp
mimic_dynamics.model_full
model_full
Definition: mimic_dynamics.py:9
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl< Scalar >::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
center-of-mass.hpp
joint-configuration.hpp
pinocchio::Force
context::Force Force
Definition: spatial/fwd.hpp:63
reachable-workspace-with-collisions.alpha
float alpha
Definition: reachable-workspace-with-collisions.py:162
pinocchio::placement
const MotionDense< Motion2 > const SE3Tpl< SE3Scalar, SE3Options > & placement
Definition: spatial/classic-acceleration.hpp:122
pinocchio::computeCentroidalMap
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeCentroidalMap(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the Centroidal Momentum Matrix.
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
Definition: joint-configuration.hpp:70
mimic_dynamics.v_full
v_full
Definition: mimic_dynamics.py:48
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: multibody/data.hpp:173
cartpole.v
v
Definition: cartpole.py:153
pinocchio::ccrba
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & ccrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal m...
pinocchio::MimicTestCases::model_mimic
pinocchio::Model model_mimic
Definition: model-generator.hpp:130
pinocchio::Inertia
context::Inertia Inertia
Definition: spatial/fwd.hpp:64
mimic_dynamics.model_mimic
model_mimic
Definition: mimic_dynamics.py:19
pinocchio::computeCentroidalMomentumTimeVariation
const DataTpl< Scalar, Options, JointCollectionTpl >::Force & computeCentroidalMomentumTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the Centroidal momemtum and its time derivatives, a.k.a. the total momenta of the system and...
pinocchio::DataTpl::Ag
Matrix6x Ag
Centroidal Momentum Matrix.
Definition: multibody/data.hpp:284
setup.name
name
Definition: cmake/cython/setup.in.py:179
q
q
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
a
Vec3f a
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
mimic_dynamics.q_full
q_full
Definition: mimic_dynamics.py:46
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:374
fill
fill
centroidal.hpp
pinocchio::rnea
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(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)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
mimic_dynamics.G
G
Definition: mimic_dynamics.py:27
pinocchio::MimicTestCases
Definition: model-generator.hpp:125
pinocchio::DataTpl::hg
Force hg
Centroidal momentum quantity.
Definition: multibody/data.hpp:297
pinocchio::Convention::LOCAL
@ LOCAL
dcrba.eps
int eps
Definition: dcrba.py:458
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
mimic_dynamics.data_full
data_full
Definition: mimic_dynamics.py:53
sample-models.hpp
pinocchio::computeCentroidalMomentum
const DataTpl< Scalar, Options, JointCollectionTpl >::Force & computeCentroidalMomentum(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Computes the Centroidal momentum, a.k.a. the total momenta of the system expressed around the center ...
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_ccrba)
Definition: unittest/centroidal.cpp:54
jacobian.hpp
pinocchio::ModelTpl< context::Scalar, context::Options >
pinocchio::MimicTestCases::model_full
pinocchio::Model model_full
Definition: model-generator.hpp:131
crba.hpp
robot-wrapper-viewer.com
com
Definition: robot-wrapper-viewer.py:45
mimic_dynamics.data_mimic
data_mimic
Definition: mimic_dynamics.py:52
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:33
isApprox
bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol)
pinocchio::centerOfMass
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 & centerOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
Computes the center of mass position of a given model according to a particular joint configuration....

pinocchio
Author(s):
autogenerated on Wed Apr 16 2025 02:41:44