data.hpp 15.2 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
28
29
30
31
32
33
34
35
36
//
// 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"
#include "pinocchio/multibody/joint/joint.hpp"
#include "pinocchio/deprecated.hh"
#include "pinocchio/container/aligned-vector.hpp"

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

namespace se3
{
37
38
39
 
  template<typename JointCollection>
  struct DataTpl
jcarpent's avatar
jcarpent committed
40
41
42
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
    enum { Options = JointCollection::Options };
    
    typedef ModelTpl<JointCollection> Model;
    
    typedef typename JointCollection::Scalar Scalar;
    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;
    
    typedef JointModelTpl<JointCollection> JointModel;
    typedef JointDataTpl<JointCollection> JointData;
    
    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;
    
70
71
72
73
74
75
76
    /// \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
77
    /// \brief The 6d jacobian type (temporary)
78
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
jcarpent's avatar
jcarpent committed
79
    /// \brief The 3d jacobian type (temporary)
80
    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
jcarpent's avatar
jcarpent committed
81
    
82
83
    typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXd;
jcarpent's avatar
jcarpent committed
84
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
    
    /// \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).
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<typename Inertia::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
151
152
153
    
    /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
    RowMatrixXd Minv;
    
    /// \brief The Coriolis matrix (a square matrix of dim model.nv).
154
    MatrixXs C;
jcarpent's avatar
jcarpent committed
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
    
    /// \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
175
    container::aligned_vector<typename Inertia::Matrix6> vxI;
jcarpent's avatar
jcarpent committed
176
177
    
    /// \brief Left variation of the inertia matrix
178
    container::aligned_vector<typename Inertia::Matrix6> Ivx;
jcarpent's avatar
jcarpent committed
179
180
181
182
183
    
    /// \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
184
    container::aligned_vector<typename Inertia::Matrix6> doYcrb;
jcarpent's avatar
jcarpent committed
185
186
    
    /// \brief Temporary for derivative algorithms
187
    typename Inertia::Matrix6 Itmp;
jcarpent's avatar
jcarpent committed
188
189
190
191
192
    
    /// \brief Temporary for derivative algorithms
    RowMatrix6 M6tmpR;
    
    /// \brief The joint accelerations computed from ABA
193
    TangentVectorType ddq;
jcarpent's avatar
jcarpent committed
194
195
196
    
    // ABA internal data
    /// \brief Inertia matrix of the subtree expressed as dense matrix [ABA]
197
    container::aligned_vector<typename Inertia::Matrix6> Yaba;  // TODO: change with dense symmetric matrix6
jcarpent's avatar
jcarpent committed
198
199
    
    /// \brief Intermediate quantity corresponding to apparent torque [ABA]
200
    TangentVectorType u;                  // Joint Inertia
jcarpent's avatar
jcarpent committed
201
202
203
204
205
206
207
208
    
    // 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
209
    /// \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
210
211
212
213
    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).
214
    /// \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
215
216
217
    ///
    Force hg;
    
218
219
220
221
222
223
    /// \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
224
225
226
227
228
229
230
231
232
233
234
235
236
    /// \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.
237
    MatrixXs U;
jcarpent's avatar
jcarpent committed
238
239
    
    /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
240
    VectorXs D;
jcarpent's avatar
jcarpent committed
241
242
    
    /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
243
    VectorXs Dinv;
jcarpent's avatar
jcarpent committed
244
245
    
    /// \brief Temporary of size NV used in Cholesky Decomposition.
246
    VectorXs tmp;
jcarpent's avatar
jcarpent committed
247
248
249
250
251
252
253
254
    
    /// \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.
255
    /// \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
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
    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.
271
    MatrixXs dtau_dq;
jcarpent's avatar
jcarpent committed
272
273
    
    /// \brief Partial derivative of the joint torque vector with respect to the joint velocity.
274
    MatrixXs dtau_dv;
jcarpent's avatar
jcarpent committed
275
276
    
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration.
277
    MatrixXs ddq_dq;
jcarpent's avatar
jcarpent committed
278
279
    
    /// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity.
280
    MatrixXs ddq_dv;
jcarpent's avatar
jcarpent committed
281
282
283
284
285
    
    /// \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.
286
    container::aligned_vector<Vector3> com;
jcarpent's avatar
jcarpent committed
287
288
    
    /// \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.
289
    container::aligned_vector<Vector3> vcom;
jcarpent's avatar
jcarpent committed
290
291
    
    /// \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.
292
    container::aligned_vector<Vector3> acom;
jcarpent's avatar
jcarpent committed
293
294
    
    /// \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.
295
    std::vector<Scalar> mass;
jcarpent's avatar
jcarpent committed
296
297
298
299
300
301
302
    
    /// \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.
303
    Scalar kinetic_energy;
jcarpent's avatar
jcarpent committed
304
305
    
    /// \brief Potential energy of the model.
306
    Scalar potential_energy;
jcarpent's avatar
jcarpent committed
307
308
309
310
    
    // Temporary variables used in forward dynamics
    
    /// \brief Inverse of the operational-space inertia matrix
311
    MatrixXs JMinvJt;
jcarpent's avatar
jcarpent committed
312
    
Guilhem Saurel's avatar
Guilhem Saurel committed
313
    /// \brief Cholesky decompostion of \f$\JMinvJt\f$.
314
    Eigen::LLT<MatrixXs> llt_JMinvJt;
jcarpent's avatar
jcarpent committed
315
316
    
    /// \brief Lagrange Multipliers corresponding to the contact forces in se3::forwardDynamics.
317
    VectorXs lambda_c;
jcarpent's avatar
jcarpent committed
318
319
    
    /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$.
320
    MatrixXs sDUiJt;
jcarpent's avatar
jcarpent committed
321
322
    
    /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$.
323
    VectorXs torque_residual;
jcarpent's avatar
jcarpent committed
324
325
    
    /// \brief Generalized velocity after impact.
326
    TangentVectorType dq_after;
jcarpent's avatar
jcarpent committed
327
328
    
    /// \brief Lagrange Multipliers corresponding to the contact impulses in se3::impulseDynamics.
329
    VectorXs impulse_c;
jcarpent's avatar
jcarpent committed
330
331
332
333
334
335
336
337
338
    
    // 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.
    ///
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
348
349
350
351
352
353
354
355

  };

} // namespace se3

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

#endif // ifndef __se3_data_hpp__