timings-contact-dynamics.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2024 INRIA
3 //
4 
5 #include "pinocchio/algorithm/joint-configuration.hpp"
6 #include "pinocchio/algorithm/kinematics.hpp"
7 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
8 #include "pinocchio/algorithm/rnea-derivatives.hpp"
9 #include "pinocchio/algorithm/aba-derivatives.hpp"
10 #include "pinocchio/algorithm/aba.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 #include "pinocchio/algorithm/crba.hpp"
13 #include "pinocchio/algorithm/contact-dynamics.hpp"
14 #include "pinocchio/algorithm/constrained-dynamics.hpp"
15 #include "pinocchio/algorithm/cholesky.hpp"
16 #include "pinocchio/parsers/urdf.hpp"
17 #include "pinocchio/multibody/sample-models.hpp"
18 #include "pinocchio/algorithm/pv.hpp"
19 
20 #include <iostream>
21 
22 #include "pinocchio/utils/timer.hpp"
23 
24 int main(int argc, const char ** argv)
25 {
26  using namespace Eigen;
27  using namespace pinocchio;
28 
29  PinocchioTicToc timer(PinocchioTicToc::US);
30 #ifdef NDEBUG
31  const int NBT = 1000 * 100;
32 #else
33  const int NBT = 1;
34  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
35 #endif
36 
37  // Build model
38  Model model;
39 
40  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
41  if (argc > 1)
42  filename = argv[1];
43  bool with_ff = true;
44 
45  if (argc > 2)
46  {
47  const std::string ff_option = argv[2];
48  if (ff_option == "-no-ff")
49  with_ff = false;
50  }
51 
52  if (filename == "HS")
53  buildModels::humanoidRandom(model, true);
54  else if (with_ff)
55  pinocchio::urdf::buildModel(filename, JointModelFreeFlyer(), model);
56  // pinocchio::urdf::buildModel(filename,JointModelRX(),model);
57  else
58  pinocchio::urdf::buildModel(filename, model);
59 
60  const std::string RA = "RARM_LINK6";
61  const JointIndex RA_id = model.frames[model.getFrameId(RA)].parent;
62  const std::string LA = "LARM_LINK6";
63  const JointIndex LA_id = model.frames[model.getFrameId(LA)].parent;
64  const std::string RF = "RLEG_LINK6";
65  const JointIndex RF_id = model.frames[model.getFrameId(RF)].parent;
66  const std::string LF = "LLEG_LINK6";
67  const JointIndex LF_id = model.frames[model.getFrameId(LF)].parent;
68 
69  RigidConstraintModel ci_RF_6D(CONTACT_6D, model, RF_id, LOCAL);
70  RigidConstraintModel ci_RF_3D(CONTACT_3D, model, RF_id, LOCAL);
71 
72  RigidConstraintModel ci_LF_6D(CONTACT_6D, model, LF_id, LOCAL);
73  RigidConstraintModel ci_LF_3D(CONTACT_3D, model, LF_id, LOCAL);
74 
75  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, RA_id, LOCAL);
76  RigidConstraintModel ci_LA_3D(CONTACT_3D, model, LA_id, LOCAL);
77 
78  // Define contact infos structure
79  static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
80  static PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_empty;
81  ContactCholeskyDecomposition contact_chol_empty(model, contact_models_empty);
82 
83  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D;
84  contact_models_6D.push_back(ci_RF_6D);
85  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D;
86  contact_data_6D.push_back(RigidConstraintData(ci_RF_6D));
87  ContactCholeskyDecomposition contact_chol_6D(model, contact_models_6D);
88 
89  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D;
90  contact_models_6D6D.push_back(ci_RF_6D);
91  contact_models_6D6D.push_back(ci_LF_6D);
92  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D6D;
93  contact_data_6D6D.push_back(RigidConstraintData(ci_RF_6D));
94  contact_data_6D6D.push_back(RigidConstraintData(ci_LF_6D));
95  ContactCholeskyDecomposition contact_chol_6D6D(model, contact_models_6D6D);
96 
97  ProximalSettings prox_settings;
98  prox_settings.max_iter = 10;
99  prox_settings.mu = 1e8;
100 
101  std::cout << "nq = " << model.nq << std::endl;
102  std::cout << "nv = " << model.nv << std::endl;
103  std::cout << "--" << std::endl;
104 
105  Data data(model);
106  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
107 
108  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qs(NBT);
109  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT);
110  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT);
111  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT);
112 
113  static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
114 
115  for (size_t i = 0; i < NBT; ++i)
116  {
117  qs[i] = randomConfiguration(model, -qmax, qmax);
118  qdots[i] = Eigen::VectorXd::Random(model.nv);
119  qddots[i] = Eigen::VectorXd::Random(model.nv);
120  taus[i] = Eigen::VectorXd::Random(model.nv);
121  }
122 
123  timer.tic();
124  SMOOTH(NBT)
125  {
126  aba(model, data, qs[_smooth], qdots[_smooth], taus[_smooth], Convention::WORLD);
127  }
128  std::cout << "ABA = \t\t";
129  timer.toc(std::cout, NBT);
130 
131  timer.tic();
132  SMOOTH(NBT)
133  {
134  contactABA(
135  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_empty,
136  contact_data_empty);
137  }
138  std::cout << "contact ABA = \t\t";
139  timer.toc(std::cout, NBT);
140 
141  initPvSolver(model, data, contact_models_empty);
142  timer.tic();
143  SMOOTH(NBT)
144  {
145  pv(
146  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_empty,
147  contact_data_empty, prox_settings);
148  }
149  std::cout << "PV = \t\t";
150  timer.toc(std::cout, NBT);
151 
152  timer.tic();
153  SMOOTH(NBT)
154  {
155  constrainedABA(
156  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_empty,
157  contact_data_empty, prox_settings);
158  }
159  std::cout << "constrainedABA = \t\t";
160  timer.toc(std::cout, NBT);
161 
162  double total_time = 0;
163  SMOOTH(NBT)
164  {
165  crba(model, data, qs[_smooth], Convention::WORLD);
166  timer.tic();
167  cholesky::decompose(model, data);
168  total_time += timer.toc(timer.DEFAULT_UNIT);
169  }
170  std::cout << "Sparse Cholesky = \t\t" << (total_time / NBT) << " "
171  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
172 
173  total_time = 0;
174  SMOOTH(NBT)
175  {
176  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
177  timer.tic();
178  contact_chol_empty.compute(model, data, contact_models_empty, contact_data_empty);
179  total_time += timer.toc(timer.DEFAULT_UNIT);
180  }
181  std::cout << "contactCholesky {} = \t\t" << (total_time / NBT) << " "
182  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
183 
184  total_time = 0;
185  MatrixXd H_inverse(contact_chol_empty.size(), contact_chol_empty.size());
186  SMOOTH(NBT)
187  {
188  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
189  contact_chol_empty.compute(model, data, contact_models_empty, contact_data_empty);
190  timer.tic();
191  contact_chol_empty.inverse(H_inverse);
192  total_time += timer.toc(timer.DEFAULT_UNIT);
193  }
194  std::cout << "contactCholeskyInverse {} = \t\t" << (total_time / NBT) << " "
195  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
196 
197  initConstraintDynamics(model, data, contact_models_empty);
198  timer.tic();
199  SMOOTH(NBT)
200  {
201  constraintDynamics(
202  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_empty,
203  contact_data_empty);
204  }
205  std::cout << "constraintDynamics {} = \t\t";
206  timer.toc(std::cout, NBT);
207 
208  std::cout << "--" << std::endl;
209  total_time = 0;
210  SMOOTH(NBT)
211  {
212  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
213  timer.tic();
214  contact_chol_6D.compute(model, data, contact_models_6D, contact_data_6D);
215  total_time += timer.toc(timer.DEFAULT_UNIT);
216  }
217  std::cout << "contactCholesky {6D} = \t\t" << (total_time / NBT) << " "
218  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
219 
220  total_time = 0;
221  H_inverse.resize(contact_chol_6D.size(), contact_chol_6D.size());
222  SMOOTH(NBT)
223  {
224  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
225  contact_chol_6D.compute(model, data, contact_models_6D, contact_data_6D);
226  timer.tic();
227  contact_chol_6D.inverse(H_inverse);
228  total_time += timer.toc(timer.DEFAULT_UNIT);
229  }
230  std::cout << "contactCholeskyInverse {6D} = \t\t" << (total_time / NBT) << " "
231  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
232 
233  MatrixXd J(contact_chol_6D.constraintDim(), model.nv);
234  J.setZero();
235  MatrixXd MJtJ_inv(
236  model.nv + contact_chol_6D.constraintDim(), model.nv + contact_chol_6D.constraintDim());
237  MJtJ_inv.setZero();
238 
239  VectorXd gamma(contact_chol_6D.constraintDim());
240  gamma.setZero();
241 
242  total_time = 0;
243  SMOOTH(NBT)
244  {
245  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
246  timer.tic();
247  getJointJacobian(model, data, ci_RF_6D.joint1_id, ci_RF_6D.reference_frame, J.middleRows<6>(0));
248  total_time += timer.toc(timer.DEFAULT_UNIT);
249 
250  forwardDynamics(model, data, qs[_smooth], qdots[_smooth], taus[_smooth], J, gamma);
251 
252  timer.tic();
253  cholesky::decompose(model, data);
254  getKKTContactDynamicMatrixInverse(model, data, J, MJtJ_inv);
255  total_time += timer.toc(timer.DEFAULT_UNIT);
256  }
257  std::cout << "KKTContactDynamicMatrixInverse {6D} = \t\t" << (total_time / NBT) << " "
258  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
259 
260  initConstraintDynamics(model, data, contact_models_6D);
261  timer.tic();
262  SMOOTH(NBT)
263  {
264  constraintDynamics(
265  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D, contact_data_6D);
266  }
267  std::cout << "constraintDynamics {6D} = \t\t";
268  timer.toc(std::cout, NBT);
269  timer.tic();
270  SMOOTH(NBT)
271  {
272  contactABA(
273  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D, contact_data_6D,
274  prox_settings);
275  }
276  std::cout << "contact ABA {6D} = \t\t";
277  timer.toc(std::cout, NBT);
278 
279  initPvSolver(model, data, contact_models_6D);
280  timer.tic();
281  SMOOTH(NBT)
282  {
283  pv(
284  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D, contact_data_6D,
285  prox_settings);
286  }
287  std::cout << "PV = \t\t";
288  timer.toc(std::cout, NBT);
289 
290  timer.tic();
291  SMOOTH(NBT)
292  {
293  constrainedABA(
294  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D, contact_data_6D,
295  prox_settings);
296  }
297  std::cout << "constrainedABA = \t\t";
298  timer.toc(std::cout, NBT);
299  std::cout << "--" << std::endl;
300 
301  total_time = 0;
302  SMOOTH(NBT)
303  {
304  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
305  timer.tic();
306  contact_chol_6D6D.compute(model, data, contact_models_6D6D, contact_data_6D6D);
307  total_time += timer.toc(timer.DEFAULT_UNIT);
308  }
309  std::cout << "contactCholesky {6D,6D} = \t\t" << (total_time / NBT) << " "
310  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
311 
312  total_time = 0;
313  H_inverse.resize(contact_chol_6D6D.size(), contact_chol_6D6D.size());
314  SMOOTH(NBT)
315  {
316  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
317  contact_chol_6D6D.compute(model, data, contact_models_6D6D, contact_data_6D6D);
318  timer.tic();
319  contact_chol_6D6D.inverse(H_inverse);
320  total_time += timer.toc(timer.DEFAULT_UNIT);
321  }
322  std::cout << "contactCholeskyInverse {6D,6D} = \t\t" << (total_time / NBT) << " "
323  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
324 
325  J.resize(contact_chol_6D6D.constraintDim(), model.nv);
326  J.setZero();
327  MJtJ_inv.resize(
328  model.nv + contact_chol_6D6D.constraintDim(), model.nv + contact_chol_6D6D.constraintDim());
329  MJtJ_inv.setZero();
330 
331  gamma.resize(contact_chol_6D6D.constraintDim());
332  gamma.setZero();
333 
334  total_time = 0;
335  SMOOTH(NBT)
336  {
337  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
338  getJointJacobian(model, data, ci_RF_6D.joint1_id, ci_RF_6D.reference_frame, J.middleRows<6>(0));
339  getJointJacobian(model, data, ci_LF_6D.joint1_id, ci_LF_6D.reference_frame, J.middleRows<6>(6));
340 
341  forwardDynamics(model, data, qs[_smooth], qdots[_smooth], taus[_smooth], J, gamma);
342 
343  timer.tic();
344  cholesky::decompose(model, data);
345  getKKTContactDynamicMatrixInverse(model, data, J, MJtJ_inv);
346  total_time += timer.toc(timer.DEFAULT_UNIT);
347  }
348  std::cout << "KKTContactDynamicMatrixInverse {6D,6D} = \t\t" << (total_time / NBT) << " "
349  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
350 
351  initConstraintDynamics(model, data, contact_models_6D6D);
352  timer.tic();
353  SMOOTH(NBT)
354  {
355  constraintDynamics(
356  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D6D,
357  contact_data_6D6D);
358  }
359  std::cout << "constraintDynamics {6D,6D} = \t\t";
360  timer.toc(std::cout, NBT);
361  timer.tic();
362  SMOOTH(NBT)
363  {
364  contactABA(
365  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D6D,
366  contact_data_6D6D, prox_settings);
367  }
368  std::cout << "contact ABA {6D,6D} = \t\t";
369  timer.toc(std::cout, NBT);
370 
371  initPvSolver(model, data, contact_models_6D6D);
372  timer.tic();
373  SMOOTH(NBT)
374  {
375  pv(
376  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D6D,
377  contact_data_6D6D, prox_settings);
378  }
379  std::cout << "PV = \t\t";
380  timer.toc(std::cout, NBT);
381 
382  timer.tic();
383  SMOOTH(NBT)
384  {
385  constrainedABA(
386  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D6D,
387  contact_data_6D6D, prox_settings);
388  }
389  std::cout << "constrainedABA = \t\t";
390  timer.toc(std::cout, NBT);
391 
392  J.setZero();
393  timer.tic();
394  SMOOTH(NBT)
395  {
396  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
397  getJointJacobian(model, data, ci_RF_6D.joint1_id, ci_RF_6D.reference_frame, J.middleRows<6>(0));
398  getJointJacobian(model, data, ci_LF_6D.joint1_id, ci_LF_6D.reference_frame, J.middleRows<6>(6));
399  forwardDynamics(model, data, taus[_smooth], J, gamma);
400  }
401  std::cout << "forwardDynamics {6D,6D} = \t\t";
402  timer.toc(std::cout, NBT);
403 
404  std::cout << "--" << std::endl;
405 
406  return 0;
407 }
pinocchio::RigidConstraintData
RigidConstraintDataTpl< context::Scalar, context::Options > RigidConstraintData
Definition: algorithm/fwd.hpp:27
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
main
int main(int argc, const char **argv)
Definition: timings-contact-dynamics.cpp:24
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:88
pinocchio::getKKTContactDynamicMatrixInverse
PINOCCHIO_DEPRECATED void getKKTContactDynamicMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv)
Computes the inverse of the KKT matrix for dynamics with contact constraints.
Eigen
pinocchio::constrainedABA
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & constrainedABA(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 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ContactModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ContactDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
The constrained Articulated Body Algorithm (constrainedABA). It computes constrained forward dynamics...
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::pv
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & pv(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 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ContactModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ContactDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
The Popov-Vereshchagin algorithm. It computes constrained forward dynamics, aka the joint acceleratio...
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
kinematics.hpp
rnea-derivatives.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
inverse-kinematics-3d.J
J
Definition: inverse-kinematics-3d.py:28
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::context::JointModelFreeFlyer
JointModelFreeFlyerTpl< Scalar > JointModelFreeFlyer
Definition: bindings/python/context/generic.hpp:126
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::aba
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(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 > &tau, const Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
PinocchioTicToc::unitName
static std::string unitName(Unit u)
Definition: timer.hpp:58
PinocchioTicToc::DEFAULT_UNIT
Unit DEFAULT_UNIT
Definition: timer.hpp:56
pinocchio::initPvSolver
void initPvSolver(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the data according to the contact information contained in contact_models.
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
pinocchio::CONTACT_6D
@ CONTACT_6D
Point contact model.
Definition: algorithm/contact-info.hpp:22
rnea.hpp
aba-derivatives.hpp
timer.hpp
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
joint-configuration.hpp
pinocchio::ProximalSettingsTpl
Structure containing all the settings parameters for the proximal algorithms.
Definition: algorithm/fwd.hpp:13
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
filename
filename
urdf.hpp
pinocchio::forwardDynamics
const PINOCCHIO_DEPRECATED DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & forwardDynamics(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 > &tau, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< DriftVectorType > &gamma, const Scalar inv_damping=0.)
Compute the forward dynamics with contact constraints. Internally, pinocchio::computeAllTerms is call...
pinocchio::RigidConstraintDataTpl
Definition: algorithm/constraints/fwd.hpp:16
pinocchio::initConstraintDynamics
void initConstraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the forward dynamics data according to the contact information contained in contact_models.
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, const std::string &rootJointName, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
constraintDynamics
VectorXd constraintDynamics(const Model &model, Data &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Model::JointIndex id)
Definition: unittest/constrained-dynamics-derivatives.cpp:185
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
Definition: container/aligned-vector.hpp:13
PinocchioTicToc::US
@ US
Definition: timer.hpp:53
cholesky.hpp
pinocchio::cholesky::decompose
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & decompose(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the Cholesky decomposition of the joint space inertia matrix M contained in data.
pinocchio::contactABA
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & contactABA(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 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, DataAllocator > &contact_data)
Definition: constrained-dynamics.hpp:172
kinematics-derivatives.hpp
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
contact-cholesky.contact_models
list contact_models
Definition: contact-cholesky.py:22
pinocchio::ContactCholeskyDecompositionTpl< context::Scalar, context::Options >
contact-dynamics.hpp
sample-models.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
constrained-dynamics.hpp
simulation-closed-kinematic-chains.prox_settings
prox_settings
Definition: simulation-closed-kinematic-chains.py:163
crba.hpp
pinocchio::CONTACT_3D
@ CONTACT_3D
Definition: algorithm/contact-info.hpp:21
pinocchio::computeAllTerms
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the...
PinocchioTicToc
Definition: timer.hpp:47
meshcat-viewer.qs
qs
Definition: meshcat-viewer.py:128
pv.hpp
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11
PinocchioTicToc::tic
void tic()
Definition: timer.hpp:82
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:12