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 template<typename JointModel>
22 static void addJointAndBody(
23  pinocchio::Model & model,
24  const pinocchio::JointModelBase<JointModel> & joint,
25  const std::string & parent_name,
26  const std::string & name,
27  const pinocchio::SE3 placement = pinocchio::SE3::Random(),
28  bool setRandomLimits = true)
29 {
30  using namespace pinocchio;
31  typedef typename JointModel::ConfigVector_t CV;
32  typedef typename JointModel::TangentVector_t TV;
33 
34  Model::JointIndex idx;
35 
36  if (setRandomLimits)
37  idx = model.addJoint(
38  model.getJointId(parent_name), joint, SE3::Random(), name + "_joint",
39  TV::Random() + TV::Constant(1), TV::Random() + TV::Constant(1),
40  CV::Random() - CV::Constant(1), CV::Random() + CV::Constant(1));
41  else
42  idx = model.addJoint(model.getJointId(parent_name), joint, placement, name + "_joint");
43 
44  model.addJointFrame(idx);
45 
46  model.appendBodyToJoint(idx, Inertia::Random(), SE3::Identity());
47  model.addBodyFrame(name + "_body", idx);
48 }
49 
50 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
51 
52 BOOST_AUTO_TEST_CASE(test_ccrba)
53 {
54  pinocchio::Model model;
55  pinocchio::buildModels::humanoidRandom(model);
56  pinocchio::Data data(model), data_ref(model);
57 
58  model.lowerPositionLimit.head<3>().fill(-1.);
59  model.upperPositionLimit.head<3>().fill(1.);
60  Eigen::VectorXd q = randomConfiguration(model);
61  Eigen::VectorXd v = Eigen::VectorXd::Ones(model.nv);
62 
63  pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL);
64  data_ref.M.triangularView<Eigen::StrictlyLower>() =
65  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
66 
67  data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]);
68 
69  pinocchio::Data data_ref_other(model);
70  pinocchio::crba(model, data_ref_other, q, pinocchio::Convention::WORLD);
71  data_ref_other.M.triangularView<Eigen::StrictlyLower>() =
72  data_ref_other.M.transpose().triangularView<Eigen::StrictlyLower>();
73  BOOST_CHECK(data_ref_other.M.isApprox(data_ref.M));
74 
75  pinocchio::SE3 cMo(pinocchio::SE3::Matrix3::Identity(), -data_ref.Ycrb[0].lever());
76  BOOST_CHECK(data_ref.Ycrb[0].isApprox(data_ref_other.oYcrb[0]));
77 
78  ccrba(model, data, q, v);
79  BOOST_CHECK(data.com[0].isApprox(-cMo.translation(), 1e-12));
80  BOOST_CHECK(data.oYcrb[0].matrix().isApprox(data_ref.Ycrb[0].matrix(), 1e-12));
81 
82  pinocchio::Inertia Ig_ref(cMo.act(data.oYcrb[0]));
83  BOOST_CHECK(data.Ig.matrix().isApprox(Ig_ref.matrix(), 1e-12));
84 
85  pinocchio::SE3 oM1(data_ref.liMi[1]);
86  pinocchio::SE3 cM1(cMo * oM1);
87 
88  pinocchio::Data::Matrix6x Ag_ref(
89  cM1.inverse().toActionMatrix().transpose() * data_ref.M.topRows<6>());
90  BOOST_CHECK(data.Ag.isApprox(Ag_ref, 1e-12));
91 }
92 
93 BOOST_AUTO_TEST_CASE(test_centroidal_mapping)
94 {
95  pinocchio::Model model;
96  pinocchio::buildModels::humanoidRandom(model);
97  pinocchio::Data data(model), data_ref(model);
98 
99  model.lowerPositionLimit.head<3>().fill(-1.);
100  model.upperPositionLimit.head<3>().fill(1.);
101  Eigen::VectorXd q = randomConfiguration(model);
102  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
103 
104  computeCentroidalMap(model, data, q);
105  ccrba(model, data_ref, q, v);
106 
107  BOOST_CHECK(data.J.isApprox(data_ref.J));
108  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
109 
110  computeJointJacobians(model, data_ref, q);
111 
112  BOOST_CHECK(data.J.isApprox(data_ref.J));
113 }
114 
115 BOOST_AUTO_TEST_CASE(test_dccrb)
116 {
117  using namespace pinocchio;
118  Model model;
119  buildModels::humanoidRandom(model);
120  addJointAndBody(model, JointModelSpherical(), "larm6_joint", "larm7");
121  Data data(model), data_ref(model);
122 
123  model.lowerPositionLimit.head<7>().fill(-1.);
124  model.upperPositionLimit.head<7>().fill(1.);
125 
126  Eigen::VectorXd q =
127  randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit);
128  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
129  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
130 
131  const Eigen::VectorXd g = rnea(model, data_ref, q, 0 * v, 0 * a);
132  rnea(model, data_ref, q, v, a);
133 
134  pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL);
135  data_ref.M.triangularView<Eigen::StrictlyLower>() =
136  data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
137 
138  SE3 cMo(SE3::Identity());
139  data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]);
140  cMo.translation() = -data_ref.Ycrb[0].lever();
141 
142  SE3 oM1(data_ref.liMi[1]);
143  SE3 cM1(cMo * oM1);
144  Data::Matrix6x Ag_ref(cM1.toDualActionMatrix() * data_ref.M.topRows<6>());
145 
146  Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>())));
147 
148  ccrba(model, data_ref, q, v);
149  dccrba(model, data, q, v);
150  BOOST_CHECK(data.Ag.isApprox(Ag_ref));
151  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
152  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
153 
154  centerOfMass(model, data_ref, q, v, a);
155  BOOST_CHECK(data_ref.com[0].isApprox(data_ref.Ycrb[0].lever()));
156  BOOST_CHECK(data_ref.vcom[0].isApprox(data.hg.linear() / data_ref.M(0, 0)));
157  BOOST_CHECK(data_ref.vcom[0].isApprox(data.vcom[0]));
158  BOOST_CHECK(data_ref.acom[0].isApprox(hdot_ref.linear() / data_ref.M(0, 0)));
159 
160  Force hdot(data.Ag * a + data.dAg * v);
161  BOOST_CHECK(hdot.isApprox(hdot_ref));
162 
163  dccrba(model, data, q, 0 * v);
164  BOOST_CHECK(data.dAg.isZero());
165 
166  // Check that dYcrb is equal to doYcrb
167  {
168  // Compute dYcrb
169  Data data_ref(model), data_ref_plus(model), data(model);
170 
171  const double alpha = 1e-8;
172  Eigen::VectorXd q_plus(model.nq);
173  q_plus = integrate(model, q, alpha * v);
174 
175  forwardKinematics(model, data_ref, q);
176  pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL);
177  pinocchio::crba(model, data_ref_plus, q_plus, pinocchio::Convention::LOCAL);
178  forwardKinematics(model, data_ref_plus, q_plus);
179  dccrba(model, data, q, v);
180 
181  for (size_t i = 1; i < (size_t)model.njoints; ++i)
182  {
183  Inertia::Matrix6 dYcrb = (data_ref_plus.oMi[i].act(data_ref_plus.Ycrb[i]).matrix()
184  - data_ref.oMi[i].act(data_ref.Ycrb[i]).matrix())
185  / alpha;
186  BOOST_CHECK(data.doYcrb[i].isApprox(dYcrb, sqrt(alpha)));
187  }
188  }
189 
190  {
191  Data data(model);
192  ccrba(model, data_ref, q, v);
193  SE3 oMc_ref(SE3::Identity());
194  oMc_ref.translation() = data_ref.com[0];
195  const Data::Matrix6x Ag_ref = oMc_ref.toDualActionMatrix() * data_ref.Ag;
196  pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL);
197  const Data::Matrix6x Ag_ref_from_M =
198  data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>();
199 
200  const double alpha = 1e-8;
201  Eigen::VectorXd q_plus(model.nq);
202  q_plus = integrate(model, q, alpha * v);
203  ccrba(model, data_ref, q_plus, v);
204  SE3 oMc_ref_plus(SE3::Identity());
205  oMc_ref_plus.translation() = data_ref.com[0];
206  const Data::Matrix6x Ag_plus_ref = oMc_ref_plus.toDualActionMatrix() * data_ref.Ag;
207  pinocchio::crba(model, data_ref, q_plus, pinocchio::Convention::LOCAL);
208  const Data::Matrix6x Ag_plus_ref_from_M =
209  data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>();
210  const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref) / alpha;
211  const Data::Matrix6x dAg_ref_from_M = (Ag_plus_ref_from_M - Ag_ref_from_M) / alpha;
212 
213  dccrba(model, data, q, v);
214  SE3 oMc(SE3::Identity());
215  oMc.translation() = data.com[0];
216  Data::Matrix6x dAg = oMc.toDualActionMatrix() * data.dAg;
217  BOOST_CHECK(oMc.isApprox(oMc_ref));
218  BOOST_CHECK(dAg_ref.isApprox(dAg_ref_from_M, sqrt(alpha)));
219  }
220 
221  // Check for dAg/dt
222  {
223  Data data(model);
224  ccrba(model, data_ref, q, v);
225  const Data::Matrix6x Ag_ref = data_ref.Ag;
226 
227  const double alpha = 1e-8;
228  Eigen::VectorXd q_plus(model.nq);
229  q_plus = integrate(model, q, alpha * v);
230  ccrba(model, data_ref, q_plus, v);
231  const Data::Matrix6x Ag_plus_ref = data_ref.Ag;
232  const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref) / alpha;
233 
234  dccrba(model, data, q, v);
235  BOOST_CHECK(data.dAg.isApprox(dAg_ref, sqrt(alpha)));
236  }
237 
238  // Compute tensor dAg/dq
239  {
240  std::vector<Data::Matrix6x> dAgdq((size_t)model.nv, Data::Matrix6x::Zero(6, model.nv));
241  Data data(model), data_fd(model);
242  Eigen::VectorXd v_fd(Eigen::VectorXd::Zero(model.nv));
243  ccrba(model, data_fd, q, v);
244  SE3 oMc_ref(SE3::Identity());
245  oMc_ref.translation() = data_fd.com[0];
246 
247  Data::Matrix6x Ag0 = oMc_ref.toDualActionMatrix() * data_fd.Ag;
248  const Force hg0 = oMc_ref.act(data_fd.hg);
249 
250  Data::Matrix6x Ag_fd(6, model.nv);
251  Force hg_fd;
252  const double alpha = 1e-8;
253  Eigen::VectorXd q_plus(model.nq);
254  Data::Matrix6x dhdq(6, model.nv);
255  for (int k = 0; k < model.nv; ++k)
256  {
257  v_fd[k] = alpha;
258  q_plus = integrate(model, q, v_fd);
259  ccrba(model, data_fd, q_plus, v);
260  SE3 oMc_fd(SE3::Identity());
261  oMc_fd.translation() = data_fd.com[0];
262  Ag_fd = oMc_fd.toDualActionMatrix() * data_fd.Ag;
263  hg_fd = oMc_fd.act(data_fd.hg);
264  dAgdq[(size_t)k] = (Ag_fd - Ag0) / alpha;
265  dhdq.col(k) = (hg_fd - hg0).toVector() / alpha;
266  v_fd[k] = 0.;
267  }
268 
269  Data::Matrix6x dAg_ref(6, model.nv);
270  dAg_ref.setZero();
271  for (int k = 0; k < model.nv; ++k)
272  {
273  dAg_ref.col(k) = dAgdq[(size_t)k] * v;
274  }
275 
276  BOOST_CHECK(dhdq.isApprox(dAg_ref, sqrt(alpha)));
277  BOOST_CHECK((dAg_ref * v).isApprox(dhdq * v, sqrt(alpha)));
278  }
279 }
280 
281 BOOST_AUTO_TEST_CASE(test_centroidal_mapping_time_derivative)
282 {
283  pinocchio::Model model;
284  pinocchio::buildModels::humanoidRandom(model);
285  pinocchio::Data data(model), data_ref(model);
286 
287  model.lowerPositionLimit.head<3>().fill(-1.);
288  model.upperPositionLimit.head<3>().fill(1.);
289  Eigen::VectorXd q = randomConfiguration(model);
290  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
291 
292  computeCentroidalMapTimeVariation(model, data, q, v);
293  dccrba(model, data_ref, q, v);
294 
295  BOOST_CHECK(data.J.isApprox(data_ref.J));
296  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
297  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
298  BOOST_CHECK(data.dAg.isApprox(data_ref.dAg));
299 
300  computeJointJacobiansTimeVariation(model, data_ref, q, v);
301 
302  BOOST_CHECK(data.J.isApprox(data_ref.J));
303  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
304 }
305 
306 BOOST_AUTO_TEST_CASE(test_computeCentroidalMomentum_computeCentroidalMomentumTimeVariation)
307 {
308  using namespace pinocchio;
309  Model model;
310  buildModels::humanoidRandom(model);
311  addJointAndBody(model, JointModelSpherical(), "larm6_joint", "larm7");
312  Data data(model), data_fk1(model), data_fk2(model), data_ref(model);
313 
314  model.lowerPositionLimit.head<7>().fill(-1.);
315  model.upperPositionLimit.head<7>().fill(1.);
316 
317  Eigen::VectorXd q =
318  randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit);
319  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
320  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);
321 
322  ccrba(model, data_ref, q, v);
323  forwardKinematics(model, data_ref, q, v);
324  centerOfMass(model, data_ref, q, v, false);
325  computeCentroidalMomentum(model, data, q, v);
326 
327  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
328  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
329  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
330  for (size_t k = 1; k < (size_t)model.njoints; ++k)
331  {
332  BOOST_CHECK(data.mass[k] == data_ref.mass[k]);
333  BOOST_CHECK(data.com[k].isApprox(data_ref.com[k]));
334  BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
335  }
336 
337  // Check other signature
338  forwardKinematics(model, data_fk1, q, v);
339  computeCentroidalMomentum(model, data_fk1);
340 
341  BOOST_CHECK(data_fk1.hg.isApprox(data.hg));
342 
343  computeCentroidalMomentumTimeVariation(model, data, q, v, a);
344  model.gravity.setZero();
345  rnea(model, data_ref, q, v, a);
346  dccrba(model, data_ref, q, v);
347  const Force hgdot(data_ref.Ag * a + data_ref.dAg * v);
348 
349  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
350  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
351  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
352  BOOST_CHECK(data.dhg.isApprox(hgdot));
353  for (size_t k = 1; k < (size_t)model.njoints; ++k)
354  {
355  BOOST_CHECK(data.mass[k] == data_ref.mass[k]);
356  BOOST_CHECK(data.com[k].isApprox(data_ref.com[k]));
357  BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
358  BOOST_CHECK(data.a[k].isApprox(data_ref.a_gf[k]));
359  BOOST_CHECK(data.f[k].isApprox(data_ref.f[k]));
360  }
361 
362  // Check other signature
363  forwardKinematics(model, data_fk2, q, v, a);
364  computeCentroidalMomentumTimeVariation(model, data_fk2);
365 
366  BOOST_CHECK(data_fk2.hg.isApprox(data.hg));
367  BOOST_CHECK(data_fk2.dhg.isApprox(data.dhg));
368 
369  // Check against finite differences
370  Data data_fd(model);
371  const double eps = 1e-8;
372  Eigen::VectorXd v_plus = v + eps * a;
373  Eigen::VectorXd q_plus = integrate(model, q, eps * v);
374 
375  const Force hg = computeCentroidalMomentum(model, data_fd, q, v);
376  const SE3::Vector3 com = data_fd.com[0];
377  const Force hg_plus = computeCentroidalMomentum(model, data_fd, q_plus, v_plus);
378  const SE3::Vector3 com_plus = data_fd.com[0];
379 
380  SE3 transform(SE3::Identity());
381  transform.translation() = com_plus - com;
382  Force dhg_ref = (transform.act(hg_plus) - hg) / eps;
383 
384  BOOST_CHECK(data.dhg.isApprox(dhg_ref, sqrt(eps)));
385 }
386 
387 BOOST_AUTO_TEST_SUITE_END()
pinocchio::InertiaTpl< context::Scalar, context::Options >
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:11
pinocchio::SE3Tpl< context::Scalar, context::Options >::Vector3
traits< SE3Tpl >::Vector3 Vector3
Definition: spatial/se3-tpl.hpp:55
transform
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::SE3Tpl::inverse
SE3Tpl inverse() const
aXb = bXa.inverse()
Definition: spatial/se3-tpl.hpp:149
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:444
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:75
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: multibody/data.hpp:363
pinocchio::SE3Tpl
Definition: context/casadi.hpp:29
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:22
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::JointModelSpherical
JointModelSphericalTpl< context::Scalar > JointModelSpherical
Definition: multibody/joint/fwd.hpp:73
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::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::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::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::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
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
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::SE3Tpl::translation
ConstLinearRef translation() const
Definition: se3-base.hpp:52
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
center-of-mass.hpp
joint-configuration.hpp
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
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::ForceTpl
Definition: context/casadi.hpp:25
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::InertiaTpl< context::Scalar, context::Options >::Random
static InertiaTpl Random()
Definition: spatial/inertia.hpp:361
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
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: multibody/data.hpp:360
fill
fill
pinocchio::SE3Tpl::Random
static SE3Tpl Random()
Definition: spatial/se3-tpl.hpp:154
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...
pinocchio::DataTpl::hg
Force hg
Centroidal momentum quantity.
Definition: multibody/data.hpp:297
pinocchio::Convention::LOCAL
@ LOCAL
pinocchio::SE3Tpl::Identity
static SE3Tpl Identity()
Definition: spatial/se3-tpl.hpp:136
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
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:52
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
crba.hpp
robot-wrapper-viewer.com
com
Definition: robot-wrapper-viewer.py:45
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
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 Thu Dec 19 2024 03:41:26