data.hpp 15.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
11
12
13
14

#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"
15
#include "pinocchio/multibody/joint/joint-generic.hpp"
jcarpent's avatar
jcarpent committed
16
17
18
19
20
#include "pinocchio/container/aligned-vector.hpp"

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

21
namespace pinocchio
jcarpent's avatar
jcarpent committed
22
{
23
 
24
  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
25
  struct DataTpl
jcarpent's avatar
jcarpent committed
26
27
28
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
29
30
    typedef _Scalar Scalar;
    enum { Options = _Options };
31
    
32
33
34
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
35
36
37
38
39
40
41
    
    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;
    
42
43
44
45
    typedef pinocchio::Index Index;
    typedef pinocchio::JointIndex JointIndex;
    typedef pinocchio::GeomIndex GeomIndex;
    typedef pinocchio::FrameIndex FrameIndex;
46
47
    typedef std::vector<Index> IndexVector;
    
48
49
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;
50
51
52
53
54
55
56
57
    
    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;
    
58
59
60
61
62
63
64
    /// \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
65
    /// \brief The 6d jacobian type (temporary)
66
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
jcarpent's avatar
jcarpent committed
67
    /// \brief The 3d jacobian type (temporary)
68
    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
jcarpent's avatar
jcarpent committed
69
    
70
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
71
    typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
72
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
jcarpent's avatar
jcarpent committed
73
    
74
    /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, 
jcarpent's avatar
jcarpent committed
75
76
77
    /// encapsulated in JointDataAccessor.
    JointDataVector joints;
    
78
    /// \brief Vector of joint accelerations expressed at the centers of the joints frames.
jcarpent's avatar
jcarpent committed
79
80
    container::aligned_vector<Motion> a;
    
81
    /// \brief Vector of joint accelerations expressed at the origin of the world.
jcarpent's avatar
jcarpent committed
82
83
84
85
86
    container::aligned_vector<Motion> oa;
    
    /// \brief Vector of joint accelerations due to the gravity field.
    container::aligned_vector<Motion> a_gf;
    
87
88
89
    /// \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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
    /// \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).
117
    TangentVectorType tau;
jcarpent's avatar
jcarpent committed
118
119
120
121
    
    /// \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$.
122
    VectorXs nle;
jcarpent's avatar
jcarpent committed
123
124
125
126
    
    /// \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.
127
    VectorXs g;
jcarpent's avatar
jcarpent committed
128
129
130
131
132
133
134
135

    /// \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
136
    /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
137
    container::aligned_vector<typename Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
jcarpent's avatar
jcarpent committed
138
139
    
    /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
140
    MatrixXs M;
jcarpent's avatar
jcarpent committed
141
142
    
    /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
143
    RowMatrixXs Minv;
jcarpent's avatar
jcarpent committed
144
145
    
    /// \brief The Coriolis matrix (a square matrix of dim model.nv).
146
    MatrixXs C;
147
148
149

    /// \brief Variation of the spatial momenta with respect to the joint configuration.
    Matrix6x dHdq;
jcarpent's avatar
jcarpent committed
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
    
    /// \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
170
    container::aligned_vector<typename Inertia::Matrix6> vxI;
jcarpent's avatar
jcarpent committed
171
172
    
    /// \brief Left variation of the inertia matrix
173
    container::aligned_vector<typename Inertia::Matrix6> Ivx;
jcarpent's avatar
jcarpent committed
174
175
176
177
178
    
    /// \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
179
    container::aligned_vector<typename Inertia::Matrix6> doYcrb;
jcarpent's avatar
jcarpent committed
180
181
    
    /// \brief Temporary for derivative algorithms
182
    typename Inertia::Matrix6 Itmp;
jcarpent's avatar
jcarpent committed
183
184
    
    /// \brief Temporary for derivative algorithms
185
    Matrix6 M6tmp;
jcarpent's avatar
jcarpent committed
186
    RowMatrix6 M6tmpR;
187
    RowMatrix6 M6tmpR2;
jcarpent's avatar
jcarpent committed
188
189
    
    /// \brief The joint accelerations computed from ABA
190
    TangentVectorType ddq;
jcarpent's avatar
jcarpent committed
191
192
193
    
    // ABA internal data
    /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
194
    container::aligned_vector<typename Inertia::Matrix6> Yaba;  // TODO: change with dense symmetric matrix6
jcarpent's avatar
jcarpent committed
195
196
    
    /// \brief Intermediate quantity corresponding to apparent torque [ABA]
197
    TangentVectorType u;                  // Joint Inertia
jcarpent's avatar
jcarpent committed
198
199
200
201
202
203
204
205
    
    // 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
206
    /// \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
207
208
209
210
    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).
211
    /// \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
212
213
214
    ///
    Force hg;
    
215
216
217
218
219
220
    /// \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
221
222
223
224
225
226
227
228
229
230
231
232
233
    /// \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;
    /// \brief Dimension of the subtree motion space (for CRBA)
    std::vector<int> nvSubtree;

    /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition.
234
    MatrixXs U;
jcarpent's avatar
jcarpent committed
235
236
    
    /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
237
    VectorXs D;
jcarpent's avatar
jcarpent committed
238
239
    
    /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
240
    VectorXs Dinv;
jcarpent's avatar
jcarpent committed
241
242
    
    /// \brief Temporary of size NV used in Cholesky Decomposition.
243
    VectorXs tmp;
jcarpent's avatar
jcarpent committed
244
245
246
247
248
249
250
251
    
    /// \brief First previous non-zero row in M (used in Cholesky Decomposition).
    std::vector<int> parents_fromRow;
    
    /// \brief Subtree of the current row index (used in Cholesky Decomposition).
    std::vector<int> nvSubtree_fromRow;
    
    /// \brief Jacobian of joint placements.
252
    /// \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
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
    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.
268
    MatrixXs dtau_dq;
jcarpent's avatar
jcarpent committed
269
270
    
    /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
271
    MatrixXs dtau_dv;
jcarpent's avatar
jcarpent committed
272
273
    
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.
274
    MatrixXs ddq_dq;
jcarpent's avatar
jcarpent committed
275
276
    
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.
277
    MatrixXs ddq_dv;
jcarpent's avatar
jcarpent committed
278
279
280
281
282
    
    /// \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.
283
    container::aligned_vector<Vector3> com;
jcarpent's avatar
jcarpent committed
284
285
    
    /// \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.
286
    container::aligned_vector<Vector3> vcom;
jcarpent's avatar
jcarpent committed
287
288
    
    /// \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.
289
    container::aligned_vector<Vector3> acom;
jcarpent's avatar
jcarpent committed
290
    
291
    /// \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.
292
    std::vector<Scalar> mass;
jcarpent's avatar
jcarpent committed
293
294
295
296
297
298
299
    
    /// \brief Jacobien of center of mass.
    /// \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.
300
    Scalar kinetic_energy;
jcarpent's avatar
jcarpent committed
301
302
    
    /// \brief Potential energy of the model.
303
    Scalar potential_energy;
jcarpent's avatar
jcarpent committed
304
305
306
307
    
    // Temporary variables used in forward dynamics
    
    /// \brief Inverse of the operational-space inertia matrix
308
    MatrixXs JMinvJt;
jcarpent's avatar
jcarpent committed
309
    
Guilhem Saurel's avatar
Guilhem Saurel committed
310
    /// \brief Cholesky decompostion of \f$\JMinvJt\f$.
311
    Eigen::LLT<MatrixXs> llt_JMinvJt;
jcarpent's avatar
jcarpent committed
312
    
313
    /// \brief Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
314
    VectorXs lambda_c;
jcarpent's avatar
jcarpent committed
315
316
    
    /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
317
    MatrixXs sDUiJt;
jcarpent's avatar
jcarpent committed
318
319
    
    /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
320
    VectorXs torque_residual;
jcarpent's avatar
jcarpent committed
321
322
    
    /// \brief Generalized velocity after impact.
323
    TangentVectorType dq_after;
jcarpent's avatar
jcarpent committed
324
    
325
    /// \brief Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
326
    VectorXs impulse_c;
jcarpent's avatar
jcarpent committed
327
    
328
    // data related to static regressor
jcarpent's avatar
jcarpent committed
329
    Matrix3x staticRegressor;
330
331
332

    // data related to joint torque regressor
    MatrixXs jointTorqueRegressor;
jcarpent's avatar
jcarpent committed
333
334
    
    ///
335
    /// \brief Default constructor of pinocchio::Data from a pinocchio::Model.
jcarpent's avatar
jcarpent committed
336
337
338
    ///
    /// \param[in] model The model structure of the rigid body system.
    ///
339
    explicit DataTpl(const Model & model);
jcarpent's avatar
jcarpent committed
340
341

  private:
342
343
    void computeLastChild(const Model & model);
    void computeParents_fromRow(const Model & model);
jcarpent's avatar
jcarpent committed
344
345
346

  };

347
} // namespace pinocchio
jcarpent's avatar
jcarpent committed
348
349
350
351
352
353

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

354
#endif // ifndef __pinocchio_data_hpp__
jcarpent's avatar
jcarpent committed
355