GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/data.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 3 3 100.0%
Branches: 127 260 48.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2024 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #ifndef __pinocchio_multibody_data_hpp__
7 #define __pinocchio_multibody_data_hpp__
8
9 #include "pinocchio/spatial/fwd.hpp"
10 #include "pinocchio/math/tensor.hpp"
11
12 #include "pinocchio/spatial/se3.hpp"
13 #include "pinocchio/spatial/force.hpp"
14 #include "pinocchio/spatial/motion.hpp"
15 #include "pinocchio/spatial/inertia.hpp"
16 #include "pinocchio/multibody/fwd.hpp"
17 #include "pinocchio/multibody/joint/joint-generic.hpp"
18 #include "pinocchio/container/aligned-vector.hpp"
19 #include "pinocchio/algorithm/contact-cholesky.hpp"
20
21 #include "pinocchio/serialization/serializable.hpp"
22
23 #include <Eigen/Cholesky>
24 #include <Eigen/StdVector>
25 #include <Eigen/src/Core/util/Constants.h>
26
27 #include <cstddef>
28 #include <set>
29
30 namespace pinocchio
31 {
32
33 template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
34 struct traits<DataTpl<_Scalar, _Options, JointCollectionTpl>>
35 {
36 typedef _Scalar Scalar;
37 };
38
39 template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
40 struct DataTpl
41 : serialization::Serializable<DataTpl<_Scalar, _Options, JointCollectionTpl>>
42 , NumericalBase<DataTpl<_Scalar, _Options, JointCollectionTpl>>
43 {
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45
46 typedef _Scalar Scalar;
47 enum
48 {
49 Options = _Options
50 };
51
52 typedef JointCollectionTpl<Scalar, Options> JointCollection;
53
54 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
55
56 typedef SE3Tpl<Scalar, Options> SE3;
57 typedef MotionTpl<Scalar, Options> Motion;
58 typedef ForceTpl<Scalar, Options> Force;
59 typedef InertiaTpl<Scalar, Options> Inertia;
60 typedef FrameTpl<Scalar, Options> Frame;
61
62 typedef pinocchio::Index Index;
63 typedef pinocchio::JointIndex JointIndex;
64 typedef pinocchio::GeomIndex GeomIndex;
65 typedef pinocchio::FrameIndex FrameIndex;
66 typedef std::vector<Index> IndexVector;
67
68 typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel;
69 typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointData;
70
71 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
72 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
73
74 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
75 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
76 typedef Eigen::Matrix<Scalar, 1, Eigen::Dynamic, Options | Eigen::RowMajor> RowVectorXs;
77 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
78 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
79
80 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6c;
81 typedef Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor | Options> Vector6r;
82
83 /// \brief Dense vectorized version of a joint configuration vector.
84 typedef VectorXs ConfigVectorType;
85
86 /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration,
87 /// etc).
88 /// It also handles the notion of co-tangent vector (e.g. torque, etc).
89 typedef VectorXs TangentVectorType;
90
91 /// \brief The 6d jacobian type (temporary)
92 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
93 /// \brief The 3d jacobian type (temporary)
94 typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic, Options> Matrix3x;
95
96 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
97 typedef Eigen::Matrix<Scalar, 6, 6, Eigen::RowMajor | Options> RowMatrix6;
98 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Options>
99 RowMatrixXs;
100
101 /// \brief The type of the body regressor
102 typedef Eigen::Matrix<Scalar, 6, 10, Options> BodyRegressorType;
103
104 ///  \brief The type of Tensor for Kinematics and Dynamics second order derivatives
105 typedef Tensor<Scalar, 3, Options> Tensor3x;
106
107 // TODO Remove when API is stabilized
108 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
109 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
110 typedef ContactCholeskyDecompositionTpl<Scalar, Options> ContactCholeskyDecomposition;
111 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
112
113 /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in
114 /// model, encapsulated in JointDataAccessor.
115 JointDataVector joints;
116
117 /// \brief Vector of joint accelerations expressed in the local frame of the joint.
118 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a;
119
120 /// \brief Vector of joint accelerations expressed at the origin of the world.
121 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa;
122
123 /// \brief Vector of joint accelerations expressed at the origin of the world.
124 /// These accelerations are used in the context of augmented Lagrangian algorithms.
125 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_drift;
126
127 /// \brief Vector of joint accelerations expressed at the origin of the world.
128 /// These accelerations are used in the context of augmented Lagrangian algorithms.
129 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_augmented;
130
131 /// \brief Vector of joint accelerations due to the gravity field.
132 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_gf;
133
134 /// \brief Vector of joint accelerations expressed at the origin of the world including the
135 /// gravity contribution.
136 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_gf;
137
138 /// \brief Vector of joint velocities expressed in the local frame of the joint.
139 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v;
140
141 /// \brief Vector of joint velocities expressed at the origin of the world.
142 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov;
143
144 /// \brief Vector of body forces expressed in the local frame of the joint. For each body, the
145 /// force represents the sum of
146 /// all external forces acting on the body.
147 PINOCCHIO_ALIGNED_STD_VECTOR(Force) f;
148
149 /// \brief Vector of body forces expressed at the origin of the world. For each body, the force
150 /// represents the sum of
151 /// all external forces acting on the body.
152 PINOCCHIO_ALIGNED_STD_VECTOR(Force) of;
153
154 /// \brief Vector of body forces expressed in the world frame. For each body, the force
155 /// represents the sum of
156 /// all external forces acting on the body. These forces are used in the context of
157 /// augmented Lagrangian algorithms.
158 PINOCCHIO_ALIGNED_STD_VECTOR(Force) of_augmented;
159
160 /// \brief Vector of spatial momenta expressed in the local frame of the joint.
161 PINOCCHIO_ALIGNED_STD_VECTOR(Force) h;
162
163 /// \brief Vector of spatial momenta expressed at the origin of the world.
164 PINOCCHIO_ALIGNED_STD_VECTOR(Force) oh;
165
166 /// \brief Vector of absolute joint placements (wrt the world).
167 PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMi;
168
169 /// \brief Vector of relative joint placements (wrt the body parent).
170 PINOCCHIO_ALIGNED_STD_VECTOR(SE3) liMi;
171
172 /// \brief Vector of joint torques (dim model.nv).
173 TangentVectorType tau;
174
175 /// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the
176 /// Coriolis, centrifugal and gravitational effects. \note In the multibody dynamics equation
177 /// \f$ M\ddot{q} + b(q, \dot{q}) = \tau \f$,
178 /// the non linear effects are associated to the term \f$b\f$.
179 VectorXs nle;
180
181 /// \brief Vector of generalized gravity (dim model.nv).
182 /// \note In the multibody dynamics equation \f$ M\ddot{q} + c(q, \dot{q}) + g(q) = \tau \f$,
183 /// the gravity effect is associated to the \f$g\f$ term.
184 VectorXs g;
185
186 /// \brief Vector of absolute operationnel frame placements (wrt the world).
187 PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMf;
188
189 /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the
190 /// subtree supported by the joint
191 /// and expressed in the local frame of the joint..
192 PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) Ycrb;
193
194 /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$
195 /// \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
196 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb; // TODO: change with dense symmetric matrix6
197
198 /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
199 MatrixXs M;
200
201 /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
202 RowMatrixXs Minv;
203
204 /// \brief The Coriolis matrix (a square matrix of dim model.nv).
205 MatrixXs C;
206
207 /// \brief Variation of the spatial momenta set with respect to the joint configuration.
208 Matrix6x dHdq;
209
210 /// \brief Variation of the forceset with respect to the joint configuration.
211 Matrix6x dFdq;
212
213 /// \brief Variation of the forceset with respect to the joint velocity.
214 Matrix6x dFdv;
215
216 /// \brief Variation of the forceset with respect to the joint acceleration.
217 Matrix6x dFda;
218
219 /// \brief Used in computeMinverse
220 Matrix6x SDinv;
221
222 /// \brief Used in computeMinverse
223 Matrix6x UDinv;
224
225 /// \brief Used in computeMinverse
226 Matrix6x IS;
227
228 /// \brief Right variation of the inertia matrix
229 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) vxI;
230
231 /// \brief Left variation of the inertia matrix
232 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx;
233
234 /// \brief Combined variations of the inertia matrix \f$ B_i = \frac{1}{2} [(v_i\times∗)I_i +
235 /// (I_i v_i)\times^{∗} − I_i(v_i\times)] \f$ consistent with Christoffel symbols.
236 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) B;
237
238 /// \brief Rigid Body Inertia supported by the joint expressed in the world frame
239 PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias;
240
241 /// \brief Composite Rigid Body Inertia expressed in the world frame
242 PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb;
243
244 /// \brief Time variation of Composite Rigid Body Inertia expressed in the world frame
245 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
246
247 /// \brief Temporary for derivative algorithms
248 Matrix6 Itmp;
249
250 /// \brief Temporary for derivative algorithms
251 Matrix6 M6tmp;
252 RowMatrix6 M6tmpR;
253 RowMatrix6 M6tmpR2;
254
255 /// \brief The joint accelerations computed from ABA
256 TangentVectorType ddq;
257
258 // ABA internal data
259 /// \brief Articulated Body Inertia matrix of the subtree expressed in the LOCAL coordinate
260 /// frame of the joint
261 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6
262
263 /// \brief Articulated Body Inertia matrix of the subtree expressed in the WORLD coordinate
264 /// frame
265 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oYaba; // TODO: change with dense symmetric matrix6
266
267 /// \brief Articulated Body Inertia matrix with contact apparent inertia, of a given the subtree
268 /// and expressed in the WORLD coordinate frame
269 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6)
270 oYaba_contact; // TODO: change with dense symmetric matrix6
271
272 /// \brief Acceleration propagator
273 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oL; // TODO: change with dense symmetric matrix6
274
275 /// \brief Inverse articulated inertia
276 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oK; // TODO: change with dense symmetric matrix6
277
278 /// \brief Intermediate quantity corresponding to apparent torque [ABA]
279 TangentVectorType u; // Joint Inertia
280
281 // CCRBA return quantities
282 /// \brief Centroidal Momentum Matrix
283 /// \note \f$ hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
284 Matrix6x Ag;
285
286 // dCCRBA return quantities
287 /// \brief Centroidal Momentum Matrix Time Variation
288 /// \note \f$ \dot{h_g} = A_g \ddot{q}\ + \dot{A_g}\dot{q}\f$ maps the joint velocity and
289 /// acceleration vectors to the time variation of the centroidal momentum.
290 Matrix6x dAg;
291
292 /// \brief Centroidal momentum quantity.
293 /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with
294 /// the inertial frame (i.e. the world frame). \note \f$ h_g = \left( m\dot{c}, L_{g} \right);
295 /// \f$. \f$ h_g \f$ is the stack of the linear momentum and the angular momemtum vectors.
296 ///
297 Force hg;
298
299 /// \brief Centroidal momentum time derivative.
300 /// \note The centroidal momentum time derivative is expressed in the frame centered at the CoM
301 /// and aligned with the inertial frame (i.e. the world frame). \note \f$ \dot{h_g} = \left(
302 /// m\ddot{c}, \dot{L}_{g} \right); \f$. \f$ \dot{h_g} \f$ is the stack of the linear momentum
303 /// variation and the angular momemtum variation.
304 ///
305 Force dhg;
306
307 /// \brief Centroidal Composite Rigid Body Inertia.
308 /// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroidal momentum
309 /// quantity.
310 Inertia Ig;
311
312 /// \brief Spatial forces set, used in CRBA and CCRBA
313 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb;
314
315 /// \brief Index of the last child (for CRBA)
316 std::vector<int> lastChild;
317
318 /// \brief Dimension of the subtree motion space (for CRBA)
319 std::vector<int> nvSubtree;
320
321 /// \brief Starting index of the Joint motion subspace
322 std::vector<int> start_idx_v_fromRow;
323
324 /// \brief End index of the Joint motion subspace
325 std::vector<int> end_idx_v_fromRow;
326
327 /// \brief Extended model mapping of the joint rows
328 /// (idx_vExtended_to_idx_v_fromRow[idx_vExtended] = idx_v)
329 std::vector<int> idx_vExtended_to_idx_v_fromRow;
330
331 /// \brief Store the index of the first non mimic child of each mimic joint (for CRBA).
332 /// Store 0 if there is no non mimic child.
333 std::vector<JointIndex> mimic_subtree_joint;
334
335 /// \brief Joint space inertia matrix square root (upper triangular part) computed with a
336 /// Cholesky Decomposition.
337 MatrixXs U;
338
339 /// \brief Diagonal of the joint space inertia matrix obtained by a Cholesky Decomposition.
340 VectorXs D;
341
342 /// \brief Diagonal inverse of the joint space inertia matrix obtained by a Cholesky
343 /// Decomposition.
344 VectorXs Dinv;
345
346 /// \brief Temporary of size NV used in Cholesky Decomposition.
347 VectorXs tmp;
348
349 /// \brief First previous non-zero row in M (used in Cholesky Decomposition).
350 std::vector<int> parents_fromRow;
351
352 /// \brief First previous non-zero row belonging to a mimic joint in M (used in Jacobian).
353 std::vector<int> mimic_parents_fromRow;
354
355 /// \brief First previous non-zero row belonging to a non mimic joint in M (used in Jacobian).
356 std::vector<int> non_mimic_parents_fromRow;
357
358 /// \brief Each element of this vector corresponds to the ordered list of indexes
359 /// belonging to the supporting tree of the given index at the row level.
360 /// It may be helpful to retrieve the sparsity pattern through it.
361 std::vector<std::vector<int>> supports_fromRow;
362
363 /// \brief Subtree of the current row index (used in Cholesky Decomposition).
364 std::vector<int> nvSubtree_fromRow;
365
366 /// \brief Jacobian of joint placements.
367 /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and
368 /// expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i}
369 /// \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J =
370 /// \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots &
371 /// ^{0}X_{\text{nvExtended}} S_{\text{nvExtended}} \end{bmatrix} \f$. This Jacobian has no
372 /// special meaning. To get the jacobian of a precise joint, you need to call
373 /// pinocchio::getJointJacobian
374 Matrix6x J;
375
376 /// \brief Derivative of the Jacobian with respect to the time.
377 Matrix6x dJ;
378
379 /// \brief Second derivative of the Jacobian with respect to the time.
380 Matrix6x ddJ;
381
382 /// \brief psidot Derivative of Jacobian w.r.t to the parent body moving
383 /// v(p(j)) x Sj
384 Matrix6x psid;
385
386 /// \brief psiddot Second Derivative of Jacobian w.r.t to the parent body
387 /// moving a(p(j)) x Sj + v(p(j)) x psidj
388 Matrix6x psidd;
389
390 /// \brief Variation of the spatial velocity set with respect to the joint configuration.
391 Matrix6x dVdq;
392
393 /// \brief Variation of the spatial acceleration set with respect to the joint configuration.
394 Matrix6x dAdq;
395
396 /// \brief Variation of the spatial acceleration set with respect to the joint velocity.
397 Matrix6x dAdv;
398
399 /// \brief Partial derivative of the joint torque vector with respect to the joint
400 /// configuration.
401 RowMatrixXs dtau_dq;
402
403 /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
404 RowMatrixXs dtau_dv;
405
406 /// \brief Partial derivative of the joint acceleration vector with respect to the joint
407 /// configuration.
408 RowMatrixXs ddq_dq;
409
410 /// \brief Partial derivative of the joint acceleration vector with respect to the joint
411 /// velocity.
412 RowMatrixXs ddq_dv;
413
414 /// \brief Partial derivative of the joint acceleration vector with respect to the joint
415 /// torques.
416 RowMatrixXs ddq_dtau;
417
418 /// \brief Stack of partial derivative of the contact frame acceleration with respect to the
419 /// joint parameters.
420 MatrixXs dvc_dq;
421 MatrixXs dac_dq;
422 MatrixXs dac_dv;
423 MatrixXs dac_da;
424
425 /// \brief Operational space inertia matrix;
426 MatrixXs osim;
427
428 /// \brief Partial derivatives of the constraints forces with respect to the joint
429 /// configuration, velocity and torque;
430 MatrixXs dlambda_dq;
431 MatrixXs dlambda_dv;
432 MatrixXs dlambda_dtau;
433 MatrixXs dlambda_dx_prox, drhs_prox;
434
435 /// \brief Vector of joint placements wrt to algorithm end effector.
436 PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf;
437
438 /// \brief Vector of subtree center of mass positions expressed in the root joint of the
439 /// subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$ j
440 /// \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center
441 /// of mass position of the whole model and expressed in the global frame.
442 PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com;
443
444 /// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the
445 /// subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by
446 /// joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds
447 /// to the velocity of the CoM of the whole model expressed in the global frame.
448 PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
449
450 /// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of
451 /// the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported
452 /// by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0]
453 /// corresponds to the acceleration of the CoM of the whole model expressed in the global frame.
454 PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
455
456 /// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported
457 /// by joint \f$ j \f$. The element mass[0] corresponds to the total mass of the model.
458 std::vector<Scalar> mass;
459
460 /// \brief Jacobian of center of mass.
461 /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass,
462 /// expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}}
463 /// \dot{q}\f$.
464 Matrix3x Jcom;
465
466 /// \brief Kinetic energy of the system.
467 Scalar kinetic_energy;
468
469 /// \brief Potential energy of the system.
470 Scalar potential_energy;
471
472 /// \brief Mechanical energy of the system.
473 Scalar mechanical_energy;
474
475 // Temporary variables used in forward dynamics
476
477 /// \brief Inverse of the operational-space inertia matrix
478 MatrixXs JMinvJt;
479
480 /// \brief Cholesky decompostion of \f$JMinvJt\f$.
481 Eigen::LLT<MatrixXs> llt_JMinvJt;
482
483 /// \brief Lagrange Multipliers corresponding to the contact forces in
484 /// pinocchio::forwardDynamics.
485 VectorXs lambda_c;
486
487 /// \brief Proximal Lagrange Multipliers used in the computation of the Forward Dynamics
488 /// computations.
489 VectorXs lambda_c_prox;
490
491 /// \brief Difference between two consecutive iterations of the proxy algorithm.
492 VectorXs diff_lambda_c;
493
494 /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
495 MatrixXs sDUiJt;
496
497 /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
498 VectorXs torque_residual;
499
500 /// \brief Generalized velocity after impact.
501 TangentVectorType dq_after;
502
503 /// \brief Lagrange Multipliers corresponding to the contact impulses in
504 /// pinocchio::impulseDynamics.
505 VectorXs impulse_c;
506
507 /// \brief Matrix related to static regressor
508 Matrix3x staticRegressor;
509
510 /// \brief Body regressor
511 BodyRegressorType bodyRegressor;
512
513 /// \brief Matrix related to joint torque regressor
514 MatrixXs jointTorqueRegressor;
515
516 /// \brief Matrix related to kinetic energy regressor
517 RowVectorXs kineticEnergyRegressor;
518
519 /// \brief Matrix related to potential energy regressor
520 RowVectorXs potentialEnergyRegressor;
521
522 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) KA;
523 PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) LA;
524 PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lA;
525 PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lambdaA;
526 PINOCCHIO_ALIGNED_STD_VECTOR(int) par_cons_ind;
527 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_bias;
528 PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) KAS;
529 PINOCCHIO_ALIGNED_STD_VECTOR(int) constraint_ind;
530 Eigen::LLT<MatrixXs> osim_llt;
531
532 #if defined(_MSC_VER)
533 // Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check
534 // operator precedence for possible error
535 #pragma warning(disable : 4554)
536 #endif
537
538 /// \brief Tensor containing the kinematic Hessian of all the joints.
539 Tensor3x kinematic_hessians;
540
541 #if defined(_MSC_VER)
542 #pragma warning(default : 4554) // C4554 enabled after tensor definition
543 #endif
544
545 /// \brief Cholesky decomposition of the KKT contact matrix
546 ContactCholeskyDecomposition contact_chol;
547
548 /// \brief RHS vector when solving the contact dynamics KKT problem
549 VectorXs primal_dual_contact_solution;
550
551 /// \brief Primal RHS in contact dynamic equations
552 VectorXs primal_rhs_contact;
553
554 #if defined(_MSC_VER)
555 // Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check
556 // operator precedence for possible error
557 #pragma warning(disable : 4554)
558 #endif
559
560 /// \brief SO Partial derivative of the joint torque vector with respect to
561 /// the joint configuration.
562 Tensor3x d2tau_dqdq;
563
564 /// \brief SO Partial derivative of the joint torque vector with respect to
565 /// the joint velocity.
566 Tensor3x d2tau_dvdv;
567
568 /// \brief SO Cross-Partial derivative of the joint torque vector with
569 /// respect to the joint configuration/velocity.
570 Tensor3x d2tau_dqdv;
571
572 /// \brief SO Cross-Partial derivative of the joint torque vector with
573 /// respect to the joint acceleration/configuration. This also equals to the
574 /// first-order partial derivative of the Mass Matrix w.r.t joint
575 /// configuration.
576 Tensor3x d2tau_dadq;
577
578 #if defined(_MSC_VER)
579 #pragma warning(default : 4554) // C4554 enabled after tensor definition
580 #endif
581
582 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6)
583 extended_motion_propagator; // Stores force propagator to the base link
584 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) extended_motion_propagator2;
585 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) spatial_inv_inertia; // Stores spatial inverse inertia
586 PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_descendant;
587 PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_ancestor;
588 PINOCCHIO_ALIGNED_STD_VECTOR(int) constraints_supported_dim;
589 PINOCCHIO_ALIGNED_STD_VECTOR(std::set<size_t>) constraints_supported;
590 PINOCCHIO_ALIGNED_STD_VECTOR(size_t) joints_supporting_constraints;
591 PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_joints;
592 PINOCCHIO_ALIGNED_STD_VECTOR(std::vector<size_t>) constraints_on_joint;
593
594 ///
595 /// \brief Default constructor of pinocchio::Data from a pinocchio::Model.
596 ///
597 /// \param[in] model The model structure of the rigid body system.
598 ///
599 explicit DataTpl(const Model & model);
600
601 ///
602 /// \brief Default constructor
603 ///
604 8 DataTpl()
605
127/260
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 8 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 8 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 8 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 8 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 8 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 8 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 8 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 8 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 8 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 8 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 8 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 8 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 8 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 8 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 8 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 8 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 8 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 8 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 8 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 8 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 8 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 8 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 8 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 8 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 8 times.
✗ Branch 87 not taken.
✓ Branch 89 taken 8 times.
✗ Branch 90 not taken.
✓ Branch 92 taken 8 times.
✗ Branch 93 not taken.
✓ Branch 95 taken 8 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 8 times.
✗ Branch 99 not taken.
✓ Branch 101 taken 8 times.
✗ Branch 102 not taken.
✓ Branch 104 taken 8 times.
✗ Branch 105 not taken.
✓ Branch 107 taken 8 times.
✗ Branch 108 not taken.
✓ Branch 110 taken 8 times.
✗ Branch 111 not taken.
✓ Branch 113 taken 8 times.
✗ Branch 114 not taken.
✓ Branch 116 taken 8 times.
✗ Branch 117 not taken.
✓ Branch 119 taken 8 times.
✗ Branch 120 not taken.
✓ Branch 122 taken 8 times.
✗ Branch 123 not taken.
✓ Branch 125 taken 8 times.
✗ Branch 126 not taken.
✓ Branch 128 taken 8 times.
✗ Branch 129 not taken.
✓ Branch 131 taken 8 times.
✗ Branch 132 not taken.
✓ Branch 134 taken 8 times.
✗ Branch 135 not taken.
✓ Branch 137 taken 8 times.
✗ Branch 138 not taken.
✓ Branch 140 taken 8 times.
✗ Branch 141 not taken.
✓ Branch 143 taken 8 times.
✗ Branch 144 not taken.
✓ Branch 146 taken 8 times.
✗ Branch 147 not taken.
✓ Branch 149 taken 8 times.
✗ Branch 150 not taken.
✓ Branch 152 taken 8 times.
✗ Branch 153 not taken.
✓ Branch 155 taken 8 times.
✗ Branch 156 not taken.
✓ Branch 158 taken 8 times.
✗ Branch 159 not taken.
✓ Branch 161 taken 8 times.
✗ Branch 162 not taken.
✓ Branch 170 taken 8 times.
✗ Branch 171 not taken.
✓ Branch 173 taken 8 times.
✗ Branch 174 not taken.
✓ Branch 176 taken 8 times.
✗ Branch 177 not taken.
✓ Branch 179 taken 8 times.
✗ Branch 180 not taken.
✓ Branch 187 taken 8 times.
✗ Branch 188 not taken.
✓ Branch 190 taken 8 times.
✗ Branch 191 not taken.
✓ Branch 193 taken 8 times.
✗ Branch 194 not taken.
✓ Branch 196 taken 8 times.
✗ Branch 197 not taken.
✓ Branch 199 taken 8 times.
✗ Branch 200 not taken.
✓ Branch 202 taken 8 times.
✗ Branch 203 not taken.
✓ Branch 205 taken 8 times.
✗ Branch 206 not taken.
✓ Branch 208 taken 8 times.
✗ Branch 209 not taken.
✓ Branch 211 taken 8 times.
✗ Branch 212 not taken.
✓ Branch 214 taken 8 times.
✗ Branch 215 not taken.
✓ Branch 217 taken 8 times.
✗ Branch 218 not taken.
✓ Branch 220 taken 8 times.
✗ Branch 221 not taken.
✓ Branch 223 taken 8 times.
✗ Branch 224 not taken.
✓ Branch 226 taken 8 times.
✗ Branch 227 not taken.
✓ Branch 229 taken 8 times.
✗ Branch 230 not taken.
✓ Branch 232 taken 8 times.
✗ Branch 233 not taken.
✓ Branch 235 taken 8 times.
✗ Branch 236 not taken.
✓ Branch 238 taken 8 times.
✗ Branch 239 not taken.
✓ Branch 241 taken 8 times.
✗ Branch 242 not taken.
✓ Branch 244 taken 8 times.
✗ Branch 245 not taken.
✓ Branch 247 taken 8 times.
✗ Branch 248 not taken.
✓ Branch 250 taken 8 times.
✗ Branch 251 not taken.
✓ Branch 253 taken 8 times.
✗ Branch 254 not taken.
✓ Branch 256 taken 8 times.
✗ Branch 257 not taken.
✓ Branch 259 taken 8 times.
✗ Branch 260 not taken.
✓ Branch 262 taken 8 times.
✗ Branch 263 not taken.
✓ Branch 265 taken 8 times.
✗ Branch 266 not taken.
✓ Branch 269 taken 8 times.
✗ Branch 270 not taken.
✓ Branch 272 taken 8 times.
✗ Branch 273 not taken.
✓ Branch 275 taken 8 times.
✗ Branch 276 not taken.
✓ Branch 278 taken 8 times.
✗ Branch 279 not taken.
✓ Branch 281 taken 8 times.
✗ Branch 282 not taken.
✓ Branch 284 taken 8 times.
✗ Branch 285 not taken.
✓ Branch 287 taken 8 times.
✗ Branch 288 not taken.
✓ Branch 290 taken 8 times.
✗ Branch 291 not taken.
✓ Branch 293 taken 8 times.
✗ Branch 294 not taken.
✓ Branch 296 taken 8 times.
✗ Branch 297 not taken.
✓ Branch 299 taken 8 times.
✗ Branch 300 not taken.
✓ Branch 302 taken 8 times.
✗ Branch 303 not taken.
✓ Branch 305 taken 8 times.
✗ Branch 306 not taken.
✓ Branch 308 taken 8 times.
✗ Branch 309 not taken.
✓ Branch 311 taken 8 times.
✗ Branch 312 not taken.
✓ Branch 314 taken 8 times.
✗ Branch 315 not taken.
✓ Branch 317 taken 8 times.
✗ Branch 318 not taken.
✓ Branch 320 taken 8 times.
✗ Branch 321 not taken.
✓ Branch 323 taken 8 times.
✗ Branch 324 not taken.
✓ Branch 326 taken 8 times.
✗ Branch 327 not taken.
✓ Branch 329 taken 8 times.
✗ Branch 330 not taken.
✓ Branch 332 taken 8 times.
✗ Branch 333 not taken.
✓ Branch 335 taken 8 times.
✗ Branch 336 not taken.
✓ Branch 338 taken 8 times.
✗ Branch 339 not taken.
✓ Branch 341 taken 8 times.
✗ Branch 342 not taken.
✓ Branch 344 taken 8 times.
✗ Branch 345 not taken.
✓ Branch 347 taken 8 times.
✗ Branch 348 not taken.
✓ Branch 350 taken 8 times.
✗ Branch 351 not taken.
✓ Branch 353 taken 8 times.
✗ Branch 354 not taken.
✓ Branch 356 taken 8 times.
✗ Branch 357 not taken.
✓ Branch 359 taken 8 times.
✗ Branch 360 not taken.
✓ Branch 362 taken 8 times.
✗ Branch 363 not taken.
✓ Branch 365 taken 8 times.
✗ Branch 366 not taken.
✓ Branch 368 taken 8 times.
✗ Branch 369 not taken.
✓ Branch 371 taken 8 times.
✗ Branch 372 not taken.
✓ Branch 374 taken 8 times.
✗ Branch 375 not taken.
✓ Branch 377 taken 8 times.
✗ Branch 378 not taken.
✓ Branch 380 taken 8 times.
✗ Branch 381 not taken.
✓ Branch 383 taken 8 times.
✗ Branch 384 not taken.
✓ Branch 386 taken 8 times.
✗ Branch 387 not taken.
✓ Branch 389 taken 8 times.
✗ Branch 390 not taken.
✓ Branch 392 taken 8 times.
✗ Branch 393 not taken.
✗ Branch 395 not taken.
✗ Branch 396 not taken.
✗ Branch 398 not taken.
✗ Branch 399 not taken.
✗ Branch 401 not taken.
✗ Branch 402 not taken.
 8 {
606 8 }
607
608 private:
609 void computeLastChild(const Model & model);
610 void computeParents_fromRow(const Model & model);
611 void computeSupports_fromRow(const Model & model);
612 };
613
614 } // namespace pinocchio
615
616 /* --- Details -------------------------------------------------------------- */
617 /* --- Details -------------------------------------------------------------- */
618 /* --- Details -------------------------------------------------------------- */
619 #include "pinocchio/multibody/data.hxx"
620
621 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
622 #include "pinocchio/multibody/data.txx"
623 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
624
625 #endif // ifndef __pinocchio_multibody_data_hpp__
626