data.hpp 16.2 KB
Newer Older
jcarpent's avatar
jcarpent committed
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
jcarpent's avatar
jcarpent committed
3
4
5
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

6
7
#ifndef __pinocchio_data_hpp__
#define __pinocchio_data_hpp__
jcarpent's avatar
jcarpent committed
8

9
10
#include "pinocchio/math/tensor.hpp"

jcarpent's avatar
jcarpent committed
11
12
13
14
15
16
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"
17
#include "pinocchio/multibody/joint/joint-generic.hpp"
jcarpent's avatar
jcarpent committed
18
19
20
21
22
#include "pinocchio/container/aligned-vector.hpp"

#include <iostream>
#include <Eigen/Cholesky>

23
namespace pinocchio
jcarpent's avatar
jcarpent committed
24
{
25
 
26
  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
27
  struct DataTpl
jcarpent's avatar
jcarpent committed
28
29
30
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
31
32
    typedef _Scalar Scalar;
    enum { Options = _Options };
33
    
34
35
36
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
37
38
39
40
41
42
43
    
    typedef SE3Tpl<Scalar,Options> SE3;
    typedef MotionTpl<Scalar,Options> Motion;
    typedef ForceTpl<Scalar,Options> Force;
    typedef InertiaTpl<Scalar,Options> Inertia;
    typedef FrameTpl<Scalar,Options> Frame;
    
44
45
46
47
    typedef pinocchio::Index Index;
    typedef pinocchio::JointIndex JointIndex;
    typedef pinocchio::GeomIndex GeomIndex;
    typedef pinocchio::FrameIndex FrameIndex;
48
49
    typedef std::vector<Index> IndexVector;
    
50
51
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;
52
53
54
55
56
57
58
59
    
    typedef container::aligned_vector<JointModel> JointModelVector;
    typedef container::aligned_vector<JointData> JointDataVector;
    
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    
60
61
62
63
64
65
66
    /// \brief Dense vectorized version of a joint configuration vector.
    typedef VectorXs ConfigVectorType;
    
    /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc).
    ///        It also handles the notion of co-tangent vector (e.g. torque, etc).
    typedef VectorXs TangentVectorType;
    
jcarpent's avatar
jcarpent committed
67
    /// \brief The 6d jacobian type (temporary)
68
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
jcarpent's avatar
jcarpent committed
69
    /// \brief The 3d jacobian type (temporary)
70
    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
jcarpent's avatar
jcarpent committed
71
    
72
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
73
    typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
74
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
75
76
77

    /// \brief The type of the body regressor
    typedef Eigen::Matrix<Scalar,6,10,Options> BodyRegressorType;
78
79
    
    /// \brief The type of Tensor for Kinematics and Dynamics second order derivatives
80
    typedef Tensor<Scalar,3,Options> Tensor3x;
81

82
    /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, 
jcarpent's avatar
jcarpent committed
83
84
85
    /// encapsulated in JointDataAccessor.
    JointDataVector joints;
    
86
    /// \brief Vector of joint accelerations expressed at the centers of the joints frames.
jcarpent's avatar
jcarpent committed
87
88
    container::aligned_vector<Motion> a;
    
89
    /// \brief Vector of joint accelerations expressed at the origin of the world.
jcarpent's avatar
jcarpent committed
90
91
92
93
94
    container::aligned_vector<Motion> oa;
    
    /// \brief Vector of joint accelerations due to the gravity field.
    container::aligned_vector<Motion> a_gf;
    
95
96
97
    /// \brief Vector of joint accelerations expressed at the origin of the world including gravity contribution.
    container::aligned_vector<Motion> oa_gf;
    
jcarpent's avatar
jcarpent committed
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
    /// \brief Vector of joint velocities expressed at the centers of the joints.
    container::aligned_vector<Motion> v;
    
    /// \brief Vector of joint velocities expressed at the origin.
    container::aligned_vector<Motion> ov;
    
    /// \brief Vector of body forces expressed in the local frame of the joint. For each body, the force represents the sum of
    ///        all external forces acting on the body.
    container::aligned_vector<Force> f;
    
    /// \brief Vector of body forces expressed in the world frame. For each body, the force represents the sum of
    ///        all external forces acting on the body.
    container::aligned_vector<Force> of;
    
    /// \brief Vector of spatial momenta expressed in the local frame of the joint.
    container::aligned_vector<Force> h;
    
    /// \brief Vector of spatial momenta expressed in the world frame.
    container::aligned_vector<Force> oh;
    
    /// \brief Vector of absolute joint placements (wrt the world).
    container::aligned_vector<SE3> oMi;

    /// \brief Vector of relative joint placements (wrt the body parent).
    container::aligned_vector<SE3> liMi;
    
    /// \brief Vector of joint torques (dim model.nv).
125
    TangentVectorType tau;
jcarpent's avatar
jcarpent committed
126
127
128
129
    
    /// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis, centrifugal and gravitational effects.
    /// \note  In the multibody dynamics equation \f$ M\ddot{q} + b(q, \dot{q}) = \tau \f$,
    ///        the non linear effects are associated to the term \f$b\f$.
130
    VectorXs nle;
jcarpent's avatar
jcarpent committed
131
132
133
134
    
    /// \brief Vector of generalized gravity (dim model.nv).
    /// \note  In the multibody dynamics equation \f$ M\ddot{q} + c(q, \dot{q}) + g(q) = \tau \f$,
    ///        the gravity effect is associated to the \f$g\f$ term.
135
    VectorXs g;
jcarpent's avatar
jcarpent committed
136
137
138
139
140
141
142
143

    /// \brief Vector of absolute operationnel frame placements (wrt the world).
    container::aligned_vector<SE3> oMf;

    /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint
    ///        and expressed in the local frame of the joint..
    container::aligned_vector<Inertia> Ycrb;
    
Guilhem Saurel's avatar
Guilhem Saurel committed
144
    /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
145
    container::aligned_vector<Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
jcarpent's avatar
jcarpent committed
146
147
    
    /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
148
    MatrixXs M;
jcarpent's avatar
jcarpent committed
149
150
    
    /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
151
    RowMatrixXs Minv;
jcarpent's avatar
jcarpent committed
152
153
    
    /// \brief The Coriolis matrix (a square matrix of dim model.nv).
154
    MatrixXs C;
155
156
157

    /// \brief Variation of the spatial momenta with respect to the joint configuration.
    Matrix6x dHdq;
jcarpent's avatar
jcarpent committed
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
    
    /// \brief Variation of the forceset with respect to the joint configuration.
    Matrix6x dFdq;
    
    /// \brief Variation of the forceset with respect to the joint velocity.
    Matrix6x dFdv;
    
    /// \brief Variation of the forceset with respect to the joint acceleration.
    Matrix6x dFda;

    /// \brief Used in computeMinverse
    Matrix6x SDinv;

    /// \brief Used in computeMinverse
    Matrix6x UDinv;

    /// \brief Used in computeMinverse
    Matrix6x IS;

    /// \brief Right variation of the inertia matrix
178
    container::aligned_vector<Matrix6> vxI;
jcarpent's avatar
jcarpent committed
179
180
    
    /// \brief Left variation of the inertia matrix
181
    container::aligned_vector<Matrix6> Ivx;
jcarpent's avatar
jcarpent committed
182
183
184
185
186
    
    /// \brief Inertia quantities expressed in the world frame
    container::aligned_vector<Inertia> oYcrb;
    
    /// \brief Time variation of the inertia quantities expressed in the world frame
187
    container::aligned_vector<Matrix6> doYcrb;
jcarpent's avatar
jcarpent committed
188
189
    
    /// \brief Temporary for derivative algorithms
190
    Matrix6 Itmp;
jcarpent's avatar
jcarpent committed
191
192
    
    /// \brief Temporary for derivative algorithms
193
    Matrix6 M6tmp;
jcarpent's avatar
jcarpent committed
194
    RowMatrix6 M6tmpR;
195
    RowMatrix6 M6tmpR2;
jcarpent's avatar
jcarpent committed
196
197
    
    /// \brief The joint accelerations computed from ABA
198
    TangentVectorType ddq;
jcarpent's avatar
jcarpent committed
199
200
201
    
    // ABA internal data
    /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
202
    container::aligned_vector<Matrix6> Yaba;  // TODO: change with dense symmetric matrix6
jcarpent's avatar
jcarpent committed
203
204
    
    /// \brief Intermediate quantity corresponding to apparent torque [ABA]
205
    TangentVectorType u;                  // Joint Inertia
jcarpent's avatar
jcarpent committed
206
207
208
209
210
211
212
213
    
    // CCRBA return quantities
    /// \brief Centroidal Momentum Matrix
    /// \note \f$ hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum.
    Matrix6x Ag;
    
    // dCCRBA return quantities
    /// \brief Centroidal Momentum Matrix Time Variation
Guilhem Saurel's avatar
Guilhem Saurel committed
214
    /// \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.
jcarpent's avatar
jcarpent committed
215
216
217
218
    Matrix6x dAg;
    
    /// \brief Centroidal momentum quantity.
    /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame).
219
    /// \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.
jcarpent's avatar
jcarpent committed
220
221
222
    ///
    Force hg;
    
223
224
225
226
227
228
    /// \brief Centroidal momentum time derivative.
    /// \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).
    /// \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.
    ///
    Force dhg;
    
jcarpent's avatar
jcarpent committed
229
230
231
232
233
234
235
236
237
    /// \brief Centroidal Composite Rigid Body Inertia.
    /// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroil momentum quantity.
    Inertia Ig;

    /// \brief Spatial forces set, used in CRBA and CCRBA
    container::aligned_vector<Matrix6x> Fcrb;

    /// \brief Index of the last child (for CRBA)
    std::vector<int> lastChild;
238
    
jcarpent's avatar
jcarpent committed
239
240
    /// \brief Dimension of the subtree motion space (for CRBA)
    std::vector<int> nvSubtree;
241
242
    
    /// \brief Starting index of the Joint motion subspace
Justin Carpentier's avatar
Justin Carpentier committed
243
    std::vector<int> start_idx_v_fromRow;
244
245
246
    
    /// \brief End index of the Joint motion subspace
    std::vector<int> end_idx_v_fromRow;
jcarpent's avatar
jcarpent committed
247
248

    /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
249
    MatrixXs U;
jcarpent's avatar
jcarpent committed
250
251
    
    /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
252
    VectorXs D;
jcarpent's avatar
jcarpent committed
253
254
    
    /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
255
    VectorXs Dinv;
jcarpent's avatar
jcarpent committed
256
257
    
    /// \brief Temporary of size NV used in Cholesky Decomposition.
258
    VectorXs tmp;
jcarpent's avatar
jcarpent committed
259
260
261
262
    
    /// \brief First previous non-zero row in M (used in Cholesky Decomposition).
    std::vector<int> parents_fromRow;
    
263
264
265
266
    /// \brief Each element of this vector corresponds to the ordered list of indexes belonging to the supporting tree of the
    ///        given index at the row level. It may be helpful to retrieve the sparsity pattern through it.
    std::vector< std::vector<int> > supports_fromRow;
    
jcarpent's avatar
jcarpent committed
267
268
269
270
    /// \brief Subtree of the current row index (used in Cholesky Decomposition).
    std::vector<int> nvSubtree_fromRow;
    
    /// \brief Jacobian of joint placements.
271
    /// \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
jcarpent's avatar
jcarpent committed
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
    Matrix6x J;
    
    /// \brief Derivative of the Jacobian with respect to the time.
    Matrix6x dJ;
    
    /// \brief Variation of the spatial velocity set with respect to the joint configuration.
    Matrix6x dVdq;
    
    /// \brief Variation of the spatial acceleration set with respect to the joint configuration.
    Matrix6x dAdq;
    
    /// \brief Variation of the spatial acceleration set with respect to the joint velocity.
    Matrix6x dAdv;
    
    /// \brief Partial derivative of the joint torque vector with respect to the joint configuration.
287
    MatrixXs dtau_dq;
jcarpent's avatar
jcarpent committed
288
289
    
    /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
290
    MatrixXs dtau_dv;
jcarpent's avatar
jcarpent committed
291
292
    
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.
293
    MatrixXs ddq_dq;
jcarpent's avatar
jcarpent committed
294
295
    
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.
296
    MatrixXs ddq_dv;
jcarpent's avatar
jcarpent committed
297
298
299
300
301
    
    /// \brief Vector of joint placements wrt to algorithm end effector.
    container::aligned_vector<SE3> iMf;

    /// \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.
302
    container::aligned_vector<Vector3> com;
jcarpent's avatar
jcarpent committed
303
304
    
    /// \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.
305
    container::aligned_vector<Vector3> vcom;
jcarpent's avatar
jcarpent committed
306
307
    
    /// \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.
308
    container::aligned_vector<Vector3> acom;
jcarpent's avatar
jcarpent committed
309
    
310
    /// \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.
311
    std::vector<Scalar> mass;
jcarpent's avatar
jcarpent committed
312
    
Wolfgang Merkt's avatar
Wolfgang Merkt committed
313
    /// \brief Jacobian of center of mass.
jcarpent's avatar
jcarpent committed
314
315
316
317
    /// \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$.
    Matrix3x Jcom;

    /// \brief Kinetic energy of the model.
318
    Scalar kinetic_energy;
jcarpent's avatar
jcarpent committed
319
320
    
    /// \brief Potential energy of the model.
321
    Scalar potential_energy;
jcarpent's avatar
jcarpent committed
322
323
324
325
    
    // Temporary variables used in forward dynamics
    
    /// \brief Inverse of the operational-space inertia matrix
326
    MatrixXs JMinvJt;
jcarpent's avatar
jcarpent committed
327
    
Justin Carpentier's avatar
Justin Carpentier committed
328
    /// \brief Cholesky decompostion of \f$JMinvJt\f$.
329
    Eigen::LLT<MatrixXs> llt_JMinvJt;
jcarpent's avatar
jcarpent committed
330
    
331
    /// \brief Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
332
    VectorXs lambda_c;
jcarpent's avatar
jcarpent committed
333
334
    
    /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
335
    MatrixXs sDUiJt;
jcarpent's avatar
jcarpent committed
336
337
    
    /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
338
    VectorXs torque_residual;
jcarpent's avatar
jcarpent committed
339
340
    
    /// \brief Generalized velocity after impact.
341
    TangentVectorType dq_after;
jcarpent's avatar
jcarpent committed
342
    
343
    /// \brief Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
344
    VectorXs impulse_c;
jcarpent's avatar
jcarpent committed
345
    
346
    /// \brief Matrix related to static regressor
jcarpent's avatar
jcarpent committed
347
    Matrix3x staticRegressor;
348

349
    /// \brief Body regressor
350
351
    BodyRegressorType bodyRegressor;

352
    /// \brief Matrix related to joint torque regressor
353
    MatrixXs jointTorqueRegressor;
jcarpent's avatar
jcarpent committed
354
    
355
    /// \brief Tensor containing the kinematic Hessian of all the joints.
356
    Tensor3x kinematic_hessians;
357
    
jcarpent's avatar
jcarpent committed
358
    ///
359
    /// \brief Default constructor of pinocchio::Data from a pinocchio::Model.
jcarpent's avatar
jcarpent committed
360
361
362
    ///
    /// \param[in] model The model structure of the rigid body system.
    ///
363
    explicit DataTpl(const Model & model);
364
365
366
367
368
    
    ///
    /// \brief Default constructor
    ///
    DataTpl() {}
jcarpent's avatar
jcarpent committed
369
370

  private:
371
372
    void computeLastChild(const Model & model);
    void computeParents_fromRow(const Model & model);
373
    void computeSupports_fromRow(const Model & model);
jcarpent's avatar
jcarpent committed
374
375
376

  };

377
} // namespace pinocchio
jcarpent's avatar
jcarpent committed
378
379
380
381
382
383

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/data.hxx"

384
#endif // ifndef __pinocchio_data_hpp__
jcarpent's avatar
jcarpent committed
385