data.hpp 15.4 KB
Newer Older
jcarpent's avatar
jcarpent committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_data_hpp__
#define __se3_data_hpp__

#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"
28
#include "pinocchio/multibody/joint/joint-generic.hpp"
jcarpent's avatar
jcarpent committed
29
30
31
32
33
34
35
#include "pinocchio/container/aligned-vector.hpp"

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

namespace se3
{
36
 
37
  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
38
  struct DataTpl
jcarpent's avatar
jcarpent committed
39
40
41
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
42
43
    typedef _Scalar Scalar;
    enum { Options = _Options };
44
    
45
46
47
    typedef JointCollectionTpl<Scalar,Options> JointCollection;
    
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
48
49
50
51
52
53
54
55
56
57
58
59
60
    
    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;
    
    typedef se3::Index Index;
    typedef se3::JointIndex JointIndex;
    typedef se3::GeomIndex GeomIndex;
    typedef se3::FrameIndex FrameIndex;
    typedef std::vector<Index> IndexVector;
    
61
62
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel;
    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData;
63
64
65
66
67
68
69
70
    
    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;
    
71
72
73
74
75
76
77
    /// \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
78
    /// \brief The 6d jacobian type (temporary)
79
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
jcarpent's avatar
jcarpent committed
80
    /// \brief The 3d jacobian type (temporary)
81
    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
jcarpent's avatar
jcarpent committed
82
    
83
    typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
84
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
jcarpent's avatar
jcarpent committed
85
86
87
88
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
116
117
118
119
120
121
122
123
124
125
    
    /// \brief Vector of se3::JointData associated to the se3::JointModel stored in model, 
    /// encapsulated in JointDataAccessor.
    JointDataVector joints;
    
    /// \brief Vector of joint accelerations expressed at the centers of the joints.
    container::aligned_vector<Motion> a;
    
    /// \brief Vector of joint accelerations expressed at the origin.
    container::aligned_vector<Motion> oa;
    
    /// \brief Vector of joint accelerations due to the gravity field.
    container::aligned_vector<Motion> a_gf;
    
    /// \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).
126
    TangentVectorType tau;
jcarpent's avatar
jcarpent committed
127
128
129
130
    
    /// \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$.
131
    VectorXs nle;
jcarpent's avatar
jcarpent committed
132
133
134
135
    
    /// \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.
136
    VectorXs g;
jcarpent's avatar
jcarpent committed
137
138
139
140
141
142
143
144

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

  private:
344
345
    void computeLastChild(const Model & model);
    void computeParents_fromRow(const Model & model);
jcarpent's avatar
jcarpent committed
346
347
348
349
350
351
352
353
354
355
356
357

  };

} // namespace se3

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

#endif // ifndef __se3_data_hpp__