pinocchio  2.6.3
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
data.hpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_multibody_data_hpp__
7 #define __pinocchio_multibody_data_hpp__
8 
9 #include "pinocchio/math/tensor.hpp"
10 
11 #include "pinocchio/spatial/fwd.hpp"
12 #include "pinocchio/spatial/se3.hpp"
13 #include "pinocchio/spatial/force.hpp"
14 #include "pinocchio/spatial/motion.hpp"
15 #include "pinocchio/spatial/inertia.hpp"
16 #include "pinocchio/multibody/fwd.hpp"
17 #include "pinocchio/multibody/joint/joint-generic.hpp"
18 #include "pinocchio/container/aligned-vector.hpp"
19 
20 #include "pinocchio/serialization/serializable.hpp"
21 
22 #include <iostream>
23 #include <Eigen/Cholesky>
24 
25 namespace pinocchio
26 {
27 
28  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
29  struct DataTpl
30  : serialization::Serializable< DataTpl<_Scalar,_Options,JointCollectionTpl> >
31  {
32  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
33 
34  typedef _Scalar Scalar;
35  enum { Options = _Options };
36 
37  typedef JointCollectionTpl<Scalar,Options> JointCollection;
38 
40 
46 
47  typedef pinocchio::Index Index;
48  typedef pinocchio::JointIndex JointIndex;
49  typedef pinocchio::GeomIndex GeomIndex;
50  typedef pinocchio::FrameIndex FrameIndex;
51  typedef std::vector<Index> IndexVector;
52 
55 
56  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
57  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
58 
59  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
60  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
61  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
62 
64  typedef VectorXs ConfigVectorType;
65 
68  typedef VectorXs TangentVectorType;
69 
71  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
73  typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
74 
75  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
76  typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
77  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
78 
80  typedef Eigen::Matrix<Scalar,6,10,Options> BodyRegressorType;
81 
84 
87  JointDataVector joints;
88 
90  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a;
91 
93  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa;
94 
96  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_gf;
97 
99  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_gf;
100 
102  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v;
103 
105  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov;
106 
109  PINOCCHIO_ALIGNED_STD_VECTOR(Force) f;
110 
113  PINOCCHIO_ALIGNED_STD_VECTOR(Force) of;
114 
116  PINOCCHIO_ALIGNED_STD_VECTOR(Force) h;
117 
119  PINOCCHIO_ALIGNED_STD_VECTOR(Force) oh;
120 
122  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMi;
123 
125  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) liMi;
126 
128  TangentVectorType tau;
129 
133  VectorXs nle;
134 
138  VectorXs g;
139 
141  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMf;
142 
145  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) Ycrb;
146 
148  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb; // TODO: change with dense symmetric matrix6
149 
151  MatrixXs M;
152 
154  RowMatrixXs Minv;
155 
157  MatrixXs C;
158 
160  Matrix6x dHdq;
161 
163  Matrix6x dFdq;
164 
166  Matrix6x dFdv;
167 
169  Matrix6x dFda;
170 
172  Matrix6x SDinv;
173 
175  Matrix6x UDinv;
176 
178  Matrix6x IS;
179 
181  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) vxI;
182 
184  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx;
185 
187  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias;
188 
190  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb;
191 
193  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
194 
196  Matrix6 Itmp;
197 
199  Matrix6 M6tmp;
200  RowMatrix6 M6tmpR;
201  RowMatrix6 M6tmpR2;
202 
204  TangentVectorType ddq;
205 
206  // ABA internal data
208  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6
209 
211  TangentVectorType u; // Joint Inertia
212 
213  // CCRBA return quantities
216  Matrix6x Ag;
217 
218  // dCCRBA return quantities
221  Matrix6x dAg;
222 
227  Force hg;
228 
233  Force dhg;
234 
237  Inertia Ig;
238 
240  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb;
241 
243  std::vector<int> lastChild;
244 
246  std::vector<int> nvSubtree;
247 
249  std::vector<int> start_idx_v_fromRow;
250 
252  std::vector<int> end_idx_v_fromRow;
253 
255  MatrixXs U;
256 
258  VectorXs D;
259 
261  VectorXs Dinv;
262 
264  VectorXs tmp;
265 
267  std::vector<int> parents_fromRow;
268 
271  std::vector< std::vector<int> > supports_fromRow;
272 
274  std::vector<int> nvSubtree_fromRow;
275 
278  Matrix6x J;
279 
281  Matrix6x dJ;
282 
284  Matrix6x dVdq;
285 
287  Matrix6x dAdq;
288 
290  Matrix6x dAdv;
291 
293  MatrixXs dtau_dq;
294 
296  MatrixXs dtau_dv;
297 
299  MatrixXs ddq_dq;
300 
302  MatrixXs ddq_dv;
303 
305  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf;
306 
308  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com;
309 
311  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
312 
314  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
315 
317  std::vector<Scalar> mass;
318 
321  Matrix3x Jcom;
322 
325 
328 
329  // Temporary variables used in forward dynamics
330 
332  MatrixXs JMinvJt;
333 
335  Eigen::LLT<MatrixXs> llt_JMinvJt;
336 
338  VectorXs lambda_c;
339 
341  MatrixXs sDUiJt;
342 
344  VectorXs torque_residual;
345 
347  TangentVectorType dq_after;
348 
350  VectorXs impulse_c;
351 
353  Matrix3x staticRegressor;
354 
356  BodyRegressorType bodyRegressor;
357 
360 
363 
369  explicit DataTpl(const Model & model);
370 
374  DataTpl() {}
375 
376  private:
377  void computeLastChild(const Model & model);
378  void computeParents_fromRow(const Model & model);
379  void computeSupports_fromRow(const Model & model);
380 
381  };
382 
383 } // namespace pinocchio
384 
385 /* --- Details -------------------------------------------------------------- */
386 /* --- Details -------------------------------------------------------------- */
387 /* --- Details -------------------------------------------------------------- */
388 #include "pinocchio/multibody/data.hxx"
389 
390 #endif // ifndef __pinocchio_multibody_data_hpp__
391 
MatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Definition: data.hpp:302
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, encapsulated in JointDataAccessor.
Definition: data.hpp:87
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
Definition: data.hpp:169
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:154
Eigen::LLT< MatrixXs > llt_JMinvJt
Cholesky decompostion of .
Definition: data.hpp:335
Matrix3x Jcom
Jacobian of center of mass.
Definition: data.hpp:321
std::vector< int > lastChild
Index of the last child (for CRBA)
Definition: data.hpp:243
Matrix6x UDinv
Used in computeMinverse.
Definition: data.hpp:175
MatrixXs jointTorqueRegressor
Matrix related to joint torque regressor.
Definition: data.hpp:359
VectorXs Dinv
Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Definition: data.hpp:261
Tensor< Scalar, 3, Options > Tensor3x
 .
Definition: data.hpp:83
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: data.hpp:293
MatrixXs JMinvJt
Inverse of the operational-space inertia matrix.
Definition: data.hpp:332
TangentVectorType dq_after
Generalized velocity after impact.
Definition: data.hpp:347
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: frame.hpp:32
MatrixXs sDUiJt
Temporary corresponding to .
Definition: data.hpp:341
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Definition: data.hpp:163
Inertia Ig
Centroidal Composite Rigid Body Inertia.
Definition: data.hpp:237
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
Definition: data.hpp:290
Matrix6x Ag
Centroidal Momentum Matrix.
Definition: data.hpp:216
TangentVectorType u
Intermediate quantity corresponding to apparent torque [ABA].
Definition: data.hpp:211
VectorXs torque_residual
Temporary corresponding to the residual torque .
Definition: data.hpp:344
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Definition: data.hpp:166
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
Definition: data.hpp:221
Matrix3x staticRegressor
Matrix related to static regressor.
Definition: data.hpp:353
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: data.hpp:64
std::vector< int > nvSubtree_fromRow
Subtree of the current row index (used in Cholesky Decomposition).
Definition: data.hpp:274
Force dhg
Centroidal momentum time derivative.
Definition: data.hpp:233
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Definition: data.hpp:287
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Definition: data.hpp:299
Matrix6x dHdq
Variation of the spatial momenta with respect to the joint configuration.
Definition: data.hpp:160
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: data.hpp:71
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:151
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: data.hpp:281
Tensor3x kinematic_hessians
Tensor containing the kinematic Hessian of all the joints.
Definition: data.hpp:362
VectorXs g
Vector of generalized gravity (dim model.nv).
Definition: data.hpp:138
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis...
Definition: data.hpp:133
MatrixXs C
The Coriolis matrix (a square matrix of dim model.nv).
Definition: data.hpp:157
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: data.hpp:73
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition: data.hpp:204
VectorXs TangentVectorType
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).
Definition: data.hpp:68
Matrix6 Itmp
Temporary for derivative algorithms.
Definition: data.hpp:196
BodyRegressorType bodyRegressor
Body regressor.
Definition: data.hpp:356
std::vector< Scalar > mass
Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint ...
Definition: data.hpp:317
MatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity. ...
Definition: data.hpp:296
VectorXs D
Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition. ...
Definition: data.hpp:258
DataTpl()
Default constructor.
Definition: data.hpp:374
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition: data.hpp:338
std::vector< int > end_idx_v_fromRow
End index of the Joint motion subspace.
Definition: data.hpp:252
MatrixXs U
Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decompositi...
Definition: data.hpp:255
std::vector< std::vector< int > > supports_fromRow
Each element of this vector corresponds to the ordered list of indexes belonging to the supporting tr...
Definition: data.hpp:271
Matrix6x J
Jacobian of joint placements.
Definition: data.hpp:278
VectorXs tmp
Temporary of size NV used in Cholesky Decomposition.
Definition: data.hpp:264
VectorXs impulse_c
Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
Definition: data.hpp:350
Main pinocchio namespace.
Definition: treeview.dox:24
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: data.hpp:128
std::vector< int > parents_fromRow
First previous non-zero row in M (used in Cholesky Decomposition).
Definition: data.hpp:267
std::vector< int > start_idx_v_fromRow
Starting index of the Joint motion subspace.
Definition: data.hpp:249
Matrix6x IS
Used in computeMinverse.
Definition: data.hpp:178
Matrix6x SDinv
Used in computeMinverse.
Definition: data.hpp:172
std::vector< int > nvSubtree
Dimension of the subtree motion space (for CRBA)
Definition: data.hpp:246
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Definition: data.hpp:284
Scalar kinetic_energy
Kinetic energy of the model.
Definition: data.hpp:324
Matrix6 M6tmp
Temporary for derivative algorithms.
Definition: data.hpp:199
Eigen::Matrix< Scalar, 6, 10, Options > BodyRegressorType
The type of the body regressor.
Definition: data.hpp:80
Force hg
Centroidal momentum quantity.
Definition: data.hpp:227
Scalar potential_energy
Potential energy of the model.
Definition: data.hpp:327