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

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