GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/data.hpp Lines: 1 2 50.0 %
Date: 2024-01-23 21:41:47 Branches: 56 112 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2022 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/math/tensor.hpp"
10
11
#include "pinocchio/spatial/fwd.hpp"
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
20
#include "pinocchio/serialization/serializable.hpp"
21
22
#include <iostream>
23
#include <Eigen/Cholesky>
24
25
namespace pinocchio
26
{
27
28
  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
29
  struct DataTpl
30
  : serialization::Serializable< DataTpl<_Scalar,_Options,JointCollectionTpl> >
31
  {
32
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
33
34
    typedef _Scalar Scalar;
35
    enum { Options = _Options };
36
37
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
38
39
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
40
41
    typedef SE3Tpl<Scalar,Options> SE3;
42
    typedef MotionTpl<Scalar,Options> Motion;
43
    typedef ForceTpl<Scalar,Options> Force;
44
    typedef InertiaTpl<Scalar,Options> Inertia;
45
    typedef FrameTpl<Scalar,Options> Frame;
46
47
    typedef pinocchio::Index Index;
48
    typedef pinocchio::JointIndex JointIndex;
49
    typedef pinocchio::GeomIndex GeomIndex;
50
    typedef pinocchio::FrameIndex FrameIndex;
51
    typedef std::vector<Index> IndexVector;
52
53
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
54
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;
55
56
    typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
57
    typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
58
59
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
60
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
61
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
62
    typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6c;
63
    typedef Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor | Options> Vector6r;
64
65
    /// \brief Dense vectorized version of a joint configuration vector.
66
    typedef VectorXs ConfigVectorType;
67
68
    /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc).
69
    ///        It also handles the notion of co-tangent vector (e.g. torque, etc).
70
    typedef VectorXs TangentVectorType;
71
72
    /// \brief The 6d jacobian type (temporary)
73
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
74
    /// \brief The 3d jacobian type (temporary)
75
    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
76
77
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
78
    typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
79
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
80
81
    /// \brief The type of the body regressor
82
    typedef Eigen::Matrix<Scalar,6,10,Options> BodyRegressorType;
83
84
    /// \brief The type of Tensor for Kinematics and Dynamics second order derivatives
85
    typedef Tensor<Scalar,3,Options> Tensor3x;
86
87
    /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model,
88
    /// encapsulated in JointDataAccessor.
89
    JointDataVector joints;
90
91
    /// \brief Vector of joint accelerations expressed at the centers of the joints frames.
92
    PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a;
93
94
    /// \brief Vector of joint accelerations expressed at the origin of the world.
95
    PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa;
96
97
    /// \brief Vector of joint accelerations due to the gravity field.
98
    PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_gf;
99
100
    /// \brief Vector of joint accelerations expressed at the origin of the world including gravity contribution.
101
    PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_gf;
102
103
    /// \brief Vector of joint velocities expressed at the centers of the joints.
104
    PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v;
105
106
    /// \brief Vector of joint velocities expressed at the origin.
107
    PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov;
108
109
    /// \brief Vector of body forces expressed in the local frame of the joint. For each body, the force represents the sum of
110
    ///        all external forces acting on the body.
111
    PINOCCHIO_ALIGNED_STD_VECTOR(Force) f;
112
113
    /// \brief Vector of body forces expressed in the world frame. For each body, the force represents the sum of
114
    ///        all external forces acting on the body.
115
    PINOCCHIO_ALIGNED_STD_VECTOR(Force) of;
116
117
    /// \brief Vector of spatial momenta expressed in the local frame of the joint.
118
    PINOCCHIO_ALIGNED_STD_VECTOR(Force) h;
119
120
    /// \brief Vector of spatial momenta expressed in the world frame.
121
    PINOCCHIO_ALIGNED_STD_VECTOR(Force) oh;
122
123
    /// \brief Vector of absolute joint placements (wrt the world).
124
    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMi;
125
126
    /// \brief Vector of relative joint placements (wrt the body parent).
127
    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) liMi;
128
129
    /// \brief Vector of joint torques (dim model.nv).
130
    TangentVectorType tau;
131
132
    /// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis, centrifugal and gravitational effects.
133
    /// \note  In the multibody dynamics equation \f$ M\ddot{q} + b(q, \dot{q}) = \tau \f$,
134
    ///        the non linear effects are associated to the term \f$b\f$.
135
    VectorXs nle;
136
137
    /// \brief Vector of generalized gravity (dim model.nv).
138
    /// \note  In the multibody dynamics equation \f$ M\ddot{q} + c(q, \dot{q}) + g(q) = \tau \f$,
139
    ///        the gravity effect is associated to the \f$g\f$ term.
140
    VectorXs g;
141
142
    /// \brief Vector of absolute operationnel frame placements (wrt the world).
143
    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMf;
144
145
    /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint
146
    ///        and expressed in the local frame of the joint..
147
    PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) Ycrb;
148
149
    /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
150
    PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb; // TODO: change with dense symmetric matrix6
151
152
    /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
153
    MatrixXs M;
154
155
    /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
156
    RowMatrixXs Minv;
157
158
    /// \brief The Coriolis matrix (a square matrix of dim model.nv).
159
    MatrixXs C;
160
161
    /// \brief Variation of the spatial momenta with respect to the joint configuration.
162
    Matrix6x dHdq;
163
164
    /// \brief Variation of the forceset with respect to the joint configuration.
165
    Matrix6x dFdq;
166
167
    /// \brief Variation of the forceset with respect to the joint velocity.
168
    Matrix6x dFdv;
169
170
    /// \brief Variation of the forceset with respect to the joint acceleration.
171
    Matrix6x dFda;
172
173
    /// \brief Used in computeMinverse
174
    Matrix6x SDinv;
175
176
    /// \brief Used in computeMinverse
177
    Matrix6x UDinv;
178
179
    /// \brief Used in computeMinverse
180
    Matrix6x IS;
181
182
    /// \brief Right variation of the inertia matrix
183
    PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) vxI;
184
185
    /// \brief Left variation of the inertia matrix
186
    PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx;
187
188
    /// \brief Combined variations of the inertia matrix \f$ B_i =  \frac{1}{2} [(v_i\times∗)I_i + (I_i v_i)\times^{∗} − I_i(v_i\times)] \f$  consistent with Christoffel symbols.
189
    PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) B;
190
191
    /// \brief Rigid Body Inertia supported by the joint expressed in the world frame
192
    PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias;
193
194
    /// \brief Composite Rigid Body Inertia expressed in the world frame
195
    PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb;
196
197
    /// \brief Time variation of Composite Rigid Body Inertia expressed in the world frame
198
    PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
199
200
    /// \brief Temporary for derivative algorithms
201
    Matrix6 Itmp;
202
203
    /// \brief Temporary for derivative algorithms
204
    Matrix6 M6tmp;
205
    RowMatrix6 M6tmpR;
206
    RowMatrix6 M6tmpR2;
207
208
    /// \brief The joint accelerations computed from ABA
209
    TangentVectorType ddq;
210
211
    // ABA internal data
212
    /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
213
    PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba;  // TODO: change with dense symmetric matrix6
214
215
    /// \brief Intermediate quantity corresponding to apparent torque [ABA]
216
    TangentVectorType u;                  // Joint Inertia
217
218
    // CCRBA return quantities
219
    /// \brief Centroidal Momentum Matrix
220
    /// \note \f$ hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
221
    Matrix6x Ag;
222
223
    // dCCRBA return quantities
224
    /// \brief Centroidal Momentum Matrix Time Variation
225
    /// \note \f$ \dot{h_g} = A_g \ddot{q}\ + \dot{A_g}\dot{q}\f$ maps the joint velocity and acceleration vectors to the time variation of the centroidal momentum.
226
    Matrix6x dAg;
227
228
    /// \brief Centroidal momentum quantity.
229
    /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
230
    /// \note \f$ h_g = \left( m\dot{c}, L_{g} \right); \f$. \f$ h_g \f$ is the stack of the linear momentum and the angular momemtum vectors.
231
    ///
232
    Force hg;
233
234
    /// \brief Centroidal momentum time derivative.
235
    /// \note The centroidal momentum time derivative is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
236
    /// \note \f$ \dot{h_g} = \left( m\ddot{c}, \dot{L}_{g} \right); \f$. \f$ \dot{h_g} \f$ is the stack of the linear momentum variation and the angular momemtum variation.
237
    ///
238
    Force dhg;
239
240
    /// \brief Centroidal Composite Rigid Body Inertia.
241
    /// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroidal momentum quantity.
242
    Inertia Ig;
243
244
    /// \brief Spatial forces set, used in CRBA and CCRBA
245
    PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb;
246
247
    /// \brief Index of the last child (for CRBA)
248
    std::vector<int> lastChild;
249
250
    /// \brief Dimension of the subtree motion space (for CRBA)
251
    std::vector<int> nvSubtree;
252
253
    /// \brief Starting index of the Joint motion subspace
254
    std::vector<int> start_idx_v_fromRow;
255
256
    /// \brief End index of the Joint motion subspace
257
    std::vector<int> end_idx_v_fromRow;
258
259
    /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
260
    MatrixXs U;
261
262
    /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
263
    VectorXs D;
264
265
    /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
266
    VectorXs Dinv;
267
268
    /// \brief Temporary of size NV used in Cholesky Decomposition.
269
    VectorXs tmp;
270
271
    /// \brief First previous non-zero row in M (used in Cholesky Decomposition).
272
    std::vector<int> parents_fromRow;
273
274
    /// \brief Each element of this vector corresponds to the ordered list of indexes belonging to the supporting tree of the
275
    ///        given index at the row level. It may be helpful to retrieve the sparsity pattern through it.
276
    std::vector< std::vector<int> > supports_fromRow;
277
278
    /// \brief Subtree of the current row index (used in Cholesky Decomposition).
279
    std::vector<int> nvSubtree_fromRow;
280
281
    /// \brief Jacobian of joint placements.
282
    /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian of a precise joint, you need to call pinocchio::getJointJacobian
283
    Matrix6x J;
284
285
    /// \brief Derivative of the Jacobian with respect to the time.
286
    Matrix6x dJ;
287
288
      /// \brief Second derivative of the Jacobian with respect to the time.
289
    Matrix6x ddJ;
290
291
    /// \brief psidot Derivative of Jacobian w.r.t to the parent body moving
292
    /// v(p(j)) x Sj
293
    Matrix6x psid;
294
295
    /// \brief psiddot Second Derivative of Jacobian w.r.t to the parent body
296
    /// moving a(p(j)) x Sj + v(p(j)) x psidj
297
    Matrix6x psidd;
298
299
    /// \brief Variation of the spatial velocity set with respect to the joint configuration.
300
    Matrix6x dVdq;
301
302
    /// \brief Variation of the spatial acceleration set with respect to the joint configuration.
303
    Matrix6x dAdq;
304
305
    /// \brief Variation of the spatial acceleration set with respect to the joint velocity.
306
    Matrix6x dAdv;
307
308
    /// \brief Partial derivative of the joint torque vector with respect to the joint configuration.
309
    MatrixXs dtau_dq;
310
311
    /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
312
    MatrixXs dtau_dv;
313
314
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.
315
    MatrixXs ddq_dq;
316
317
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.
318
    MatrixXs ddq_dv;
319
320
    /// \brief Vector of joint placements wrt to algorithm end effector.
321
    PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf;
322
323
    /// \brief Vector of subtree center of mass positions expressed in the root joint of the subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center of mass position of the whole model and expressed in the global frame.
324
    PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com;
325
326
    /// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds to the velocity of the CoM of the whole model expressed in the global frame.
327
    PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
328
329
    /// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame.
330
    PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
331
332
    /// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$ j \f$. The element mass[0] corresponds to the total mass of the model.
333
    std::vector<Scalar> mass;
334
335
    /// \brief Jacobian of center of mass.
336
    /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$.
337
    Matrix3x Jcom;
338
339
    /// \brief Kinetic energy of the model.
340
    Scalar kinetic_energy;
341
342
    /// \brief Potential energy of the model.
343
    Scalar potential_energy;
344
345
    // Temporary variables used in forward dynamics
346
347
    /// \brief Inverse of the operational-space inertia matrix
348
    MatrixXs JMinvJt;
349
350
    /// \brief Cholesky decompostion of \f$JMinvJt\f$.
351
    Eigen::LLT<MatrixXs> llt_JMinvJt;
352
353
    /// \brief Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
354
    VectorXs lambda_c;
355
356
    /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
357
    MatrixXs sDUiJt;
358
359
    /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
360
    VectorXs torque_residual;
361
362
    /// \brief Generalized velocity after impact.
363
    TangentVectorType dq_after;
364
365
    /// \brief Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
366
    VectorXs impulse_c;
367
368
    /// \brief Matrix related to static regressor
369
    Matrix3x staticRegressor;
370
371
    /// \brief Body regressor
372
    BodyRegressorType bodyRegressor;
373
374
    /// \brief Matrix related to joint torque regressor
375
    MatrixXs jointTorqueRegressor;
376
377
    /// \brief Tensor containing the kinematic Hessian of all the joints.
378
    Tensor3x kinematic_hessians;
379
380
    /// \brief SO Partial derivative of the joint torque vector with respect to
381
    /// the joint configuration.
382
    Tensor3x d2tau_dqdq;
383
384
    /// \brief SO Partial derivative of the joint torque vector with respect to
385
    /// the joint velocity.
386
    Tensor3x d2tau_dvdv;
387
388
    /// \brief SO Cross-Partial derivative of the joint torque vector with
389
    /// respect to the joint configuration/velocity.
390
    Tensor3x d2tau_dqdv;
391
392
    /// \brief SO Cross-Partial derivative of the joint torque vector with
393
    /// respect to the joint acceleration/configuration. This also equals to the
394
    /// first-order partial derivative of the Mass Matrix w.r.t joint
395
    /// configuration.
396
    Tensor3x d2tau_dadq;
397
398
    ///
399
    /// \brief Default constructor of pinocchio::Data from a pinocchio::Model.
400
    ///
401
    /// \param[in] model The model structure of the rigid body system.
402
    ///
403
    explicit DataTpl(const Model & model);
404
405
    ///
406
    /// \brief Default constructor
407
    ///
408




























7
    DataTpl() {}
409
410
  private:
411
    void computeLastChild(const Model & model);
412
    void computeParents_fromRow(const Model & model);
413
    void computeSupports_fromRow(const Model & model);
414
415
  };
416
417
} // namespace pinocchio
418
419
/* --- Details -------------------------------------------------------------- */
420
/* --- Details -------------------------------------------------------------- */
421
/* --- Details -------------------------------------------------------------- */
422
#include "pinocchio/multibody/data.hxx"
423
424
#endif // ifndef __pinocchio_multibody_data_hpp__
425