data.hpp 15 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,Eigen::RowMajor | Options> RowMatrix6;
71
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
jcarpent's avatar
jcarpent committed
72
    
73
    /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, 
jcarpent's avatar
jcarpent committed
74
75
76
    /// encapsulated in JointDataAccessor.
    JointDataVector joints;
    
77
    /// \brief Vector of joint accelerations expressed at the centers of the joints frames.
jcarpent's avatar
jcarpent committed
78
79
    container::aligned_vector<Motion> a;
    
80
    /// \brief Vector of joint accelerations expressed at the origin of the world.
jcarpent's avatar
jcarpent committed
81
82
83
84
85
    container::aligned_vector<Motion> oa;
    
    /// \brief Vector of joint accelerations due to the gravity field.
    container::aligned_vector<Motion> a_gf;
    
86
87
88
    /// \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
89
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
    /// \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).
116
    TangentVectorType tau;
jcarpent's avatar
jcarpent committed
117
118
119
120
    
    /// \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$.
121
    VectorXs nle;
jcarpent's avatar
jcarpent committed
122
123
124
125
    
    /// \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.
126
    VectorXs g;
jcarpent's avatar
jcarpent committed
127
128
129
130
131
132
133
134

    /// \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
135
    /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details.
136
    container::aligned_vector<typename Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
jcarpent's avatar
jcarpent committed
137
138
    
    /// \brief The joint space inertia matrix (a square matrix of dim model.nv).
139
    MatrixXs M;
jcarpent's avatar
jcarpent committed
140
141
    
    /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
142
    RowMatrixXs Minv;
jcarpent's avatar
jcarpent committed
143
144
    
    /// \brief The Coriolis matrix (a square matrix of dim model.nv).
145
    MatrixXs C;
jcarpent's avatar
jcarpent committed
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
    
    /// \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
166
    container::aligned_vector<typename Inertia::Matrix6> vxI;
jcarpent's avatar
jcarpent committed
167
168
    
    /// \brief Left variation of the inertia matrix
169
    container::aligned_vector<typename Inertia::Matrix6> Ivx;
jcarpent's avatar
jcarpent committed
170
171
172
173
174
    
    /// \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
175
    container::aligned_vector<typename Inertia::Matrix6> doYcrb;
jcarpent's avatar
jcarpent committed
176
177
    
    /// \brief Temporary for derivative algorithms
178
    typename Inertia::Matrix6 Itmp;
jcarpent's avatar
jcarpent committed
179
180
181
    
    /// \brief Temporary for derivative algorithms
    RowMatrix6 M6tmpR;
182
    RowMatrix6 M6tmpR2;
jcarpent's avatar
jcarpent committed
183
184
    
    /// \brief The joint accelerations computed from ABA
185
    TangentVectorType ddq;
jcarpent's avatar
jcarpent committed
186
187
188
    
    // ABA internal data
    /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
189
    container::aligned_vector<typename Inertia::Matrix6> Yaba;  // TODO: change with dense symmetric matrix6
jcarpent's avatar
jcarpent committed
190
191
    
    /// \brief Intermediate quantity corresponding to apparent torque [ABA]
192
    TangentVectorType u;                  // Joint Inertia
jcarpent's avatar
jcarpent committed
193
194
195
196
197
198
199
200
    
    // 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
201
    /// \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
202
203
204
205
    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).
206
    /// \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
207
208
209
    ///
    Force hg;
    
210
211
212
213
214
215
    /// \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
216
217
218
219
220
221
222
223
224
225
226
227
228
    /// \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.
229
    MatrixXs U;
jcarpent's avatar
jcarpent committed
230
231
    
    /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
232
    VectorXs D;
jcarpent's avatar
jcarpent committed
233
234
    
    /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
235
    VectorXs Dinv;
jcarpent's avatar
jcarpent committed
236
237
    
    /// \brief Temporary of size NV used in Cholesky Decomposition.
238
    VectorXs tmp;
jcarpent's avatar
jcarpent committed
239
240
241
242
243
244
245
246
    
    /// \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.
247
    /// \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
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
    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.
263
    MatrixXs dtau_dq;
jcarpent's avatar
jcarpent committed
264
265
    
    /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
266
    MatrixXs dtau_dv;
jcarpent's avatar
jcarpent committed
267
268
    
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.
269
    MatrixXs ddq_dq;
jcarpent's avatar
jcarpent committed
270
271
    
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.
272
    MatrixXs ddq_dv;
jcarpent's avatar
jcarpent committed
273
274
275
276
277
    
    /// \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.
278
    container::aligned_vector<Vector3> com;
jcarpent's avatar
jcarpent committed
279
280
    
    /// \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.
281
    container::aligned_vector<Vector3> vcom;
jcarpent's avatar
jcarpent committed
282
283
    
    /// \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.
284
    container::aligned_vector<Vector3> acom;
jcarpent's avatar
jcarpent committed
285
    
286
    /// \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.
287
    std::vector<Scalar> mass;
jcarpent's avatar
jcarpent committed
288
289
290
291
292
293
294
    
    /// \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.
295
    Scalar kinetic_energy;
jcarpent's avatar
jcarpent committed
296
297
    
    /// \brief Potential energy of the model.
298
    Scalar potential_energy;
jcarpent's avatar
jcarpent committed
299
300
301
302
    
    // Temporary variables used in forward dynamics
    
    /// \brief Inverse of the operational-space inertia matrix
303
    MatrixXs JMinvJt;
jcarpent's avatar
jcarpent committed
304
    
Guilhem Saurel's avatar
Guilhem Saurel committed
305
    /// \brief Cholesky decompostion of \f$\JMinvJt\f$.
306
    Eigen::LLT<MatrixXs> llt_JMinvJt;
jcarpent's avatar
jcarpent committed
307
    
308
    /// \brief Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
309
    VectorXs lambda_c;
jcarpent's avatar
jcarpent committed
310
311
    
    /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
312
    MatrixXs sDUiJt;
jcarpent's avatar
jcarpent committed
313
314
    
    /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
315
    VectorXs torque_residual;
jcarpent's avatar
jcarpent committed
316
317
    
    /// \brief Generalized velocity after impact.
318
    TangentVectorType dq_after;
jcarpent's avatar
jcarpent committed
319
    
320
    /// \brief Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
321
    VectorXs impulse_c;
jcarpent's avatar
jcarpent committed
322
323
324
325
326
    
    // data related to regressor
    Matrix3x staticRegressor;
    
    ///
327
    /// \brief Default constructor of pinocchio::Data from a pinocchio::Model.
jcarpent's avatar
jcarpent committed
328
329
330
    ///
    /// \param[in] model The model structure of the rigid body system.
    ///
331
    explicit DataTpl(const Model & model);
jcarpent's avatar
jcarpent committed
332
333

  private:
334
335
    void computeLastChild(const Model & model);
    void computeParents_fromRow(const Model & model);
jcarpent's avatar
jcarpent committed
336
337
338

  };

339
} // namespace pinocchio
jcarpent's avatar
jcarpent committed
340
341
342
343
344
345

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

346
#endif // ifndef __pinocchio_data_hpp__
jcarpent's avatar
jcarpent committed
347