GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/data.hpp
Date: 2025-02-12 21:03:38
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 Joint space intertia matrix square root (upper trianglular part) computed with a
328 /// Cholesky Decomposition.
329 MatrixXs U;
330
331 /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
332 VectorXs D;
333
334 /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky
335 /// Decomposition.
336 VectorXs Dinv;
337
338 /// \brief Temporary of size NV used in Cholesky Decomposition.
339 VectorXs tmp;
340
341 /// \brief First previous non-zero row in M (used in Cholesky Decomposition).
342 std::vector<int> parents_fromRow;
343
344 /// \brief Each element of this vector corresponds to the ordered list of indexes belonging to
345 /// the supporting tree of the
346 /// given index at the row level. It may be helpful to retrieve the sparsity pattern
347 /// through it.
348 std::vector<std::vector<int>> supports_fromRow;
349
350 /// \brief Subtree of the current row index (used in Cholesky Decomposition).
351 std::vector<int> nvSubtree_fromRow;
352
353 /// \brief Jacobian of joint placements.
354 /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and
355 /// expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i}
356 /// \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J =
357 /// \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}}
358 /// S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian
359 /// of a precise joint, you need to call pinocchio::getJointJacobian
360 Matrix6x J;
361
362 /// \brief Derivative of the Jacobian with respect to the time.
363 Matrix6x dJ;
364
365 /// \brief Second derivative of the Jacobian with respect to the time.
366 Matrix6x ddJ;
367
368 /// \brief psidot Derivative of Jacobian w.r.t to the parent body moving
369 /// v(p(j)) x Sj
370 Matrix6x psid;
371
372 /// \brief psiddot Second Derivative of Jacobian w.r.t to the parent body
373 /// moving a(p(j)) x Sj + v(p(j)) x psidj
374 Matrix6x psidd;
375
376 /// \brief Variation of the spatial velocity set with respect to the joint configuration.
377 Matrix6x dVdq;
378
379 /// \brief Variation of the spatial acceleration set with respect to the joint configuration.
380 Matrix6x dAdq;
381
382 /// \brief Variation of the spatial acceleration set with respect to the joint velocity.
383 Matrix6x dAdv;
384
385 /// \brief Partial derivative of the joint torque vector with respect to the joint
386 /// configuration.
387 RowMatrixXs dtau_dq;
388
389 /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
390 RowMatrixXs dtau_dv;
391
392 /// \brief Partial derivative of the joint acceleration vector with respect to the joint
393 /// configuration.
394 RowMatrixXs ddq_dq;
395
396 /// \brief Partial derivative of the joint acceleration vector with respect to the joint
397 /// velocity.
398 RowMatrixXs ddq_dv;
399
400 /// \brief Partial derivative of the joint acceleration vector with respect to the joint
401 /// torques.
402 RowMatrixXs ddq_dtau;
403
404 /// \brief Stack of partial derivative of the contact frame acceleration with respect to the
405 /// joint parameters.
406 MatrixXs dvc_dq;
407 MatrixXs dac_dq;
408 MatrixXs dac_dv;
409 MatrixXs dac_da;
410
411 /// \brief Operational space inertia matrix;
412 MatrixXs osim;
413
414 /// \brief Partial derivatives of the constraints forces with respect to the joint
415 /// configuration, velocity and torque;
416 MatrixXs dlambda_dq;
417 MatrixXs dlambda_dv;
418 MatrixXs dlambda_dtau;
419 MatrixXs dlambda_dx_prox, drhs_prox;
420
421 /// \brief Vector of joint placements wrt to algorithm end effector.
422 PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf;
423
424 /// \brief Vector of subtree center of mass positions expressed in the root joint of the
425 /// subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$ j
426 /// \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center
427 /// of mass position of the whole model and expressed in the global frame.
428 PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com;
429
430 /// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the
431 /// subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by
432 /// joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds
433 /// to the velocity of the CoM of the whole model expressed in the global frame.
434 PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
435
436 /// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of
437 /// the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported
438 /// by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0]
439 /// corresponds to the acceleration of the CoM of the whole model expressed in the global frame.
440 PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
441
442 /// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported
443 /// by joint \f$ j \f$. The element mass[0] corresponds to the total mass of the model.
444 std::vector<Scalar> mass;
445
446 /// \brief Jacobian of center of mass.
447 /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass,
448 /// expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}}
449 /// \dot{q}\f$.
450 Matrix3x Jcom;
451
452 /// \brief Kinetic energy of the system.
453 Scalar kinetic_energy;
454
455 /// \brief Potential energy of the system.
456 Scalar potential_energy;
457
458 /// \brief Mechanical energy of the system.
459 Scalar mechanical_energy;
460
461 // Temporary variables used in forward dynamics
462
463 /// \brief Inverse of the operational-space inertia matrix
464 MatrixXs JMinvJt;
465
466 /// \brief Cholesky decompostion of \f$JMinvJt\f$.
467 Eigen::LLT<MatrixXs> llt_JMinvJt;
468
469 /// \brief Lagrange Multipliers corresponding to the contact forces in
470 /// pinocchio::forwardDynamics.
471 VectorXs lambda_c;
472
473 /// \brief Proximal Lagrange Multipliers used in the computation of the Forward Dynamics
474 /// computations.
475 VectorXs lambda_c_prox;
476
477 /// \brief Difference between two consecutive iterations of the proxy algorithm.
478 VectorXs diff_lambda_c;
479
480 /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
481 MatrixXs sDUiJt;
482
483 /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
484 VectorXs torque_residual;
485
486 /// \brief Generalized velocity after impact.
487 TangentVectorType dq_after;
488
489 /// \brief Lagrange Multipliers corresponding to the contact impulses in
490 /// pinocchio::impulseDynamics.
491 VectorXs impulse_c;
492
493 /// \brief Matrix related to static regressor
494 Matrix3x staticRegressor;
495
496 /// \brief Body regressor
497 BodyRegressorType bodyRegressor;
498
499 /// \brief Matrix related to joint torque regressor
500 MatrixXs jointTorqueRegressor;
501
502 /// \brief Matrix related to kinetic energy regressor
503 RowVectorXs kineticEnergyRegressor;
504
505 /// \brief Matrix related to potential energy regressor
506 RowVectorXs potentialEnergyRegressor;
507
508 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) KA;
509 PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) LA;
510 PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lA;
511 PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lambdaA;
512 PINOCCHIO_ALIGNED_STD_VECTOR(int) par_cons_ind;
513 PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_bias;
514 PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) KAS;
515 PINOCCHIO_ALIGNED_STD_VECTOR(int) constraint_ind;
516 Eigen::LLT<MatrixXs> osim_llt;
517
518 #if defined(_MSC_VER)
519 // Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check
520 // operator precedence for possible error
521 #pragma warning(disable : 4554)
522 #endif
523
524 /// \brief Tensor containing the kinematic Hessian of all the joints.
525 Tensor3x kinematic_hessians;
526
527 #if defined(_MSC_VER)
528 #pragma warning(default : 4554) // C4554 enabled after tensor definition
529 #endif
530
531 /// \brief Cholesky decomposition of the KKT contact matrix
532 ContactCholeskyDecomposition contact_chol;
533
534 /// \brief RHS vector when solving the contact dynamics KKT problem
535 VectorXs primal_dual_contact_solution;
536
537 /// \brief Primal RHS in contact dynamic equations
538 VectorXs primal_rhs_contact;
539
540 #if defined(_MSC_VER)
541 // Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check
542 // operator precedence for possible error
543 #pragma warning(disable : 4554)
544 #endif
545
546 /// \brief SO Partial derivative of the joint torque vector with respect to
547 /// the joint configuration.
548 Tensor3x d2tau_dqdq;
549
550 /// \brief SO Partial derivative of the joint torque vector with respect to
551 /// the joint velocity.
552 Tensor3x d2tau_dvdv;
553
554 /// \brief SO Cross-Partial derivative of the joint torque vector with
555 /// respect to the joint configuration/velocity.
556 Tensor3x d2tau_dqdv;
557
558 /// \brief SO Cross-Partial derivative of the joint torque vector with
559 /// respect to the joint acceleration/configuration. This also equals to the
560 /// first-order partial derivative of the Mass Matrix w.r.t joint
561 /// configuration.
562 Tensor3x d2tau_dadq;
563
564 #if defined(_MSC_VER)
565 #pragma warning(default : 4554) // C4554 enabled after tensor definition
566 #endif
567
568 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6)
569 extended_motion_propagator; // Stores force propagator to the base link
570 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) extended_motion_propagator2;
571 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) spatial_inv_inertia; // Stores spatial inverse inertia
572 PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_descendant;
573 PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_ancestor;
574 PINOCCHIO_ALIGNED_STD_VECTOR(int) constraints_supported_dim;
575 PINOCCHIO_ALIGNED_STD_VECTOR(std::set<size_t>) constraints_supported;
576 PINOCCHIO_ALIGNED_STD_VECTOR(size_t) joints_supporting_constraints;
577 PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_joints;
578 PINOCCHIO_ALIGNED_STD_VECTOR(std::vector<size_t>) constraints_on_joint;
579
580 ///
581 /// \brief Default constructor of pinocchio::Data from a pinocchio::Model.
582 ///
583 /// \param[in] model The model structure of the rigid body system.
584 ///
585 explicit DataTpl(const Model & model);
586
587 ///
588 /// \brief Default constructor
589 ///
590 8 DataTpl()
591
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 168 taken 8 times.
✗ Branch 169 not taken.
✓ Branch 171 taken 8 times.
✗ Branch 172 not taken.
✓ Branch 174 taken 8 times.
✗ Branch 175 not taken.
✓ Branch 177 taken 8 times.
✗ Branch 178 not taken.
✓ Branch 183 taken 8 times.
✗ Branch 184 not taken.
✓ Branch 186 taken 8 times.
✗ Branch 187 not taken.
✓ Branch 189 taken 8 times.
✗ Branch 190 not taken.
✓ Branch 192 taken 8 times.
✗ Branch 193 not taken.
✓ Branch 195 taken 8 times.
✗ Branch 196 not taken.
✓ Branch 198 taken 8 times.
✗ Branch 199 not taken.
✓ Branch 201 taken 8 times.
✗ Branch 202 not taken.
✓ Branch 204 taken 8 times.
✗ Branch 205 not taken.
✓ Branch 207 taken 8 times.
✗ Branch 208 not taken.
✓ Branch 210 taken 8 times.
✗ Branch 211 not taken.
✓ Branch 213 taken 8 times.
✗ Branch 214 not taken.
✓ Branch 216 taken 8 times.
✗ Branch 217 not taken.
✓ Branch 219 taken 8 times.
✗ Branch 220 not taken.
✓ Branch 222 taken 8 times.
✗ Branch 223 not taken.
✓ Branch 225 taken 8 times.
✗ Branch 226 not taken.
✓ Branch 228 taken 8 times.
✗ Branch 229 not taken.
✓ Branch 231 taken 8 times.
✗ Branch 232 not taken.
✓ Branch 234 taken 8 times.
✗ Branch 235 not taken.
✓ Branch 237 taken 8 times.
✗ Branch 238 not taken.
✓ Branch 240 taken 8 times.
✗ Branch 241 not taken.
✓ Branch 243 taken 8 times.
✗ Branch 244 not taken.
✓ Branch 246 taken 8 times.
✗ Branch 247 not taken.
✓ Branch 249 taken 8 times.
✗ Branch 250 not taken.
✓ Branch 252 taken 8 times.
✗ Branch 253 not taken.
✓ Branch 255 taken 8 times.
✗ Branch 256 not taken.
✓ Branch 258 taken 8 times.
✗ Branch 259 not taken.
✓ Branch 261 taken 8 times.
✗ Branch 262 not taken.
✓ Branch 265 taken 8 times.
✗ Branch 266 not taken.
✓ Branch 268 taken 8 times.
✗ Branch 269 not taken.
✓ Branch 271 taken 8 times.
✗ Branch 272 not taken.
✓ Branch 274 taken 8 times.
✗ Branch 275 not taken.
✓ Branch 277 taken 8 times.
✗ Branch 278 not taken.
✓ Branch 280 taken 8 times.
✗ Branch 281 not taken.
✓ Branch 283 taken 8 times.
✗ Branch 284 not taken.
✓ Branch 286 taken 8 times.
✗ Branch 287 not taken.
✓ Branch 289 taken 8 times.
✗ Branch 290 not taken.
✓ Branch 292 taken 8 times.
✗ Branch 293 not taken.
✓ Branch 295 taken 8 times.
✗ Branch 296 not taken.
✓ Branch 298 taken 8 times.
✗ Branch 299 not taken.
✓ Branch 301 taken 8 times.
✗ Branch 302 not taken.
✓ Branch 304 taken 8 times.
✗ Branch 305 not taken.
✓ Branch 307 taken 8 times.
✗ Branch 308 not taken.
✓ Branch 310 taken 8 times.
✗ Branch 311 not taken.
✓ Branch 313 taken 8 times.
✗ Branch 314 not taken.
✓ Branch 316 taken 8 times.
✗ Branch 317 not taken.
✓ Branch 319 taken 8 times.
✗ Branch 320 not taken.
✓ Branch 322 taken 8 times.
✗ Branch 323 not taken.
✓ Branch 325 taken 8 times.
✗ Branch 326 not taken.
✓ Branch 328 taken 8 times.
✗ Branch 329 not taken.
✓ Branch 331 taken 8 times.
✗ Branch 332 not taken.
✓ Branch 334 taken 8 times.
✗ Branch 335 not taken.
✓ Branch 337 taken 8 times.
✗ Branch 338 not taken.
✓ Branch 340 taken 8 times.
✗ Branch 341 not taken.
✓ Branch 343 taken 8 times.
✗ Branch 344 not taken.
✓ Branch 346 taken 8 times.
✗ Branch 347 not taken.
✓ Branch 349 taken 8 times.
✗ Branch 350 not taken.
✓ Branch 352 taken 8 times.
✗ Branch 353 not taken.
✓ Branch 355 taken 8 times.
✗ Branch 356 not taken.
✓ Branch 358 taken 8 times.
✗ Branch 359 not taken.
✓ Branch 361 taken 8 times.
✗ Branch 362 not taken.
✓ Branch 364 taken 8 times.
✗ Branch 365 not taken.
✓ Branch 367 taken 8 times.
✗ Branch 368 not taken.
✓ Branch 370 taken 8 times.
✗ Branch 371 not taken.
✓ Branch 373 taken 8 times.
✗ Branch 374 not taken.
✓ Branch 376 taken 8 times.
✗ Branch 377 not taken.
✓ Branch 379 taken 8 times.
✗ Branch 380 not taken.
✓ Branch 382 taken 8 times.
✗ Branch 383 not taken.
✓ Branch 385 taken 8 times.
✗ Branch 386 not taken.
✓ Branch 388 taken 8 times.
✗ Branch 389 not taken.
✗ Branch 391 not taken.
✗ Branch 392 not taken.
✗ Branch 394 not taken.
✗ Branch 395 not taken.
✗ Branch 397 not taken.
✗ Branch 398 not taken.
 8 {
592 8 }
593
594 private:
595 void computeLastChild(const Model & model);
596 void computeParents_fromRow(const Model & model);
597 void computeSupports_fromRow(const Model & model);
598 };
599
600 } // namespace pinocchio
601
602 /* --- Details -------------------------------------------------------------- */
603 /* --- Details -------------------------------------------------------------- */
604 /* --- Details -------------------------------------------------------------- */
605 #include "pinocchio/multibody/data.hxx"
606
607 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
608 #include "pinocchio/multibody/data.txx"
609 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
610
611 #endif // ifndef __pinocchio_multibody_data_hpp__
612