data.hpp 14.9 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;
    
jcarpent's avatar
jcarpent committed
70
    /// \brief The 6d jacobian type (temporary)
71
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
jcarpent's avatar
jcarpent committed
72
    /// \brief The 3d jacobian type (temporary)
73
    typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
jcarpent's avatar
jcarpent committed
74
    
75
76
    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
77
78
79
80
81
82
83
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
    
    /// \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).
118
    VectorXs tau;
jcarpent's avatar
jcarpent committed
119
120
121
122
    
    /// \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$.
123
    VectorXs nle;
jcarpent's avatar
jcarpent committed
124
125
126
127
    
    /// \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.
128
    VectorXs g;
jcarpent's avatar
jcarpent committed
129
130
131
132
133
134
135
136

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

  private:
335
336
    void computeLastChild(const Model & model);
    void computeParents_fromRow(const Model & model);
jcarpent's avatar
jcarpent committed
337
338
339
340
341
342
343
344
345
346
347
348

  };

} // namespace se3

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

#endif // ifndef __se3_data_hpp__