pinocchio  2.5.6
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 
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 
161 
164 
167 
170 
173 
176 
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 
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
217 
218  // dCCRBA return quantities
222 
228 
234 
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 
279 
282 
285 
288 
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 
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 
348 
350  VectorXs impulse_c;
351 
354 
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 
pinocchio::DataTpl::DataTpl
DataTpl()
Default constructor.
Definition: data.hpp:374
pinocchio::DataTpl::lambda_c
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition: data.hpp:338
pinocchio::DataTpl::tmp
VectorXs tmp
Temporary of size NV used in Cholesky Decomposition.
Definition: data.hpp:264
pinocchio::DataTpl::bodyRegressor
BodyRegressorType bodyRegressor
Body regressor.
Definition: data.hpp:356
pinocchio::DataTpl
Definition: data.hpp:29
pinocchio::DataTpl::Ag
Matrix6x Ag
Centroidal Momentum Matrix.
Definition: data.hpp:216
pinocchio::DataTpl::supports_fromRow
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
pinocchio::DataTpl::Tensor3x
Tensor< Scalar, 3, Options > Tensor3x
 
Definition: data.hpp:83
pinocchio::DataTpl::Jcom
Matrix3x Jcom
Jacobian of center of mass.
Definition: data.hpp:321
pinocchio::DataTpl::dAg
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
Definition: data.hpp:221
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: data.hpp:128
pinocchio::DataTpl::jointTorqueRegressor
MatrixXs jointTorqueRegressor
Matrix related to joint torque regressor.
Definition: data.hpp:359
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::DataTpl::u
TangentVectorType u
Intermediate quantity corresponding to apparent torque [ABA].
Definition: data.hpp:211
pinocchio::DataTpl::torque_residual
VectorXs torque_residual
Temporary corresponding to the residual torque .
Definition: data.hpp:344
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: data.hpp:281
pinocchio::DataTpl::dAdv
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
Definition: data.hpp:290
pinocchio::JointDataTpl
Definition: fwd.hpp:97
pinocchio::DataTpl::nvSubtree_fromRow
std::vector< int > nvSubtree_fromRow
Subtree of the current row index (used in Cholesky Decomposition).
Definition: data.hpp:274
pinocchio::DataTpl::SDinv
Matrix6x SDinv
Used in computeMinverse.
Definition: data.hpp:172
pinocchio::DataTpl::dq_after
TangentVectorType dq_after
Generalized velocity after impact.
Definition: data.hpp:347
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: data.hpp:71
pinocchio::DataTpl::C
MatrixXs C
The Coriolis matrix (a square matrix of dim model.nv).
Definition: data.hpp:157
pinocchio::DataTpl::kinematic_hessians
Tensor3x kinematic_hessians
Tensor containing the kinematic Hessian of all the joints.
Definition: data.hpp:362
pinocchio::DataTpl::BodyRegressorType
Eigen::Matrix< Scalar, 6, 10, Options > BodyRegressorType
The type of the body regressor.
Definition: data.hpp:80
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: data.hpp:73
pinocchio::DataTpl::Ig
Inertia Ig
Centroidal Composite Rigid Body Inertia.
Definition: data.hpp:237
pinocchio::DataTpl::staticRegressor
Matrix3x staticRegressor
Matrix related to static regressor.
Definition: data.hpp:353
pinocchio::DataTpl::nle
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis,...
Definition: data.hpp:133
pinocchio::FrameTpl
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: frame.hpp:31
pinocchio::DataTpl::M6tmp
Matrix6 M6tmp
Temporary for derivative algorithms.
Definition: data.hpp:199
pinocchio::DataTpl::ddq
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition: data.hpp:204
pinocchio::DataTpl::potential_energy
Scalar potential_energy
Potential energy of the model.
Definition: data.hpp:327
pinocchio::DataTpl::kinetic_energy
Scalar kinetic_energy
Kinetic energy of the model.
Definition: data.hpp:324
pinocchio::DataTpl::g
VectorXs g
Vector of generalized gravity (dim model.nv).
Definition: data.hpp:138
pinocchio::DataTpl::D
VectorXs D
Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
Definition: data.hpp:258
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:151
pinocchio::DataTpl::ddq_dv
MatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Definition: data.hpp:302
pinocchio::DataTpl::TangentVectorType
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Definition: data.hpp:68
pinocchio::DataTpl::Itmp
Matrix6 Itmp
Temporary for derivative algorithms.
Definition: data.hpp:196
pinocchio::JointModelTpl
Definition: fwd.hpp:93
pinocchio::DataTpl::lastChild
std::vector< int > lastChild
Index of the last child (for CRBA)
Definition: data.hpp:243
pinocchio::ForceTpl< Scalar, Options >
pinocchio::DataTpl::Minv
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:154
pinocchio::DataTpl::end_idx_v_fromRow
std::vector< int > end_idx_v_fromRow
End index of the Joint motion subspace.
Definition: data.hpp:252
pinocchio::DataTpl::joints
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model,...
Definition: data.hpp:87
pinocchio::DataTpl::IS
Matrix6x IS
Used in computeMinverse.
Definition: data.hpp:178
pinocchio::DataTpl::sDUiJt
MatrixXs sDUiJt
Temporary corresponding to .
Definition: data.hpp:341
pinocchio::DataTpl::dFda
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
Definition: data.hpp:169
pinocchio::DataTpl::dtau_dv
MatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity.
Definition: data.hpp:296
pinocchio::DataTpl::dtau_dq
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: data.hpp:293
pinocchio::DataTpl::JMinvJt
MatrixXs JMinvJt
Inverse of the operational-space inertia matrix.
Definition: data.hpp:332
pinocchio::DataTpl::U
MatrixXs U
Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decompositi...
Definition: data.hpp:255
pinocchio::DataTpl::Dinv
VectorXs Dinv
Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Definition: data.hpp:261
pinocchio::DataTpl::dFdq
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Definition: data.hpp:163
pinocchio::DataTpl::UDinv
Matrix6x UDinv
Used in computeMinverse.
Definition: data.hpp:175
pinocchio::DataTpl::impulse_c
VectorXs impulse_c
Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
Definition: data.hpp:350
pinocchio::DataTpl::mass
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
pinocchio::DataTpl::parents_fromRow
std::vector< int > parents_fromRow
First previous non-zero row in M (used in Cholesky Decomposition).
Definition: data.hpp:267
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: data.hpp:278
pinocchio::DataTpl::dFdv
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Definition: data.hpp:166
pinocchio::DataTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: data.hpp:64
pinocchio::DataTpl::dHdq
Matrix6x dHdq
Variation of the spatial momenta with respect to the joint configuration.
Definition: data.hpp:160
pinocchio::InertiaTpl< Scalar, Options >
pinocchio::DataTpl::nvSubtree
std::vector< int > nvSubtree
Dimension of the subtree motion space (for CRBA)
Definition: data.hpp:246
pinocchio::DataTpl::start_idx_v_fromRow
std::vector< int > start_idx_v_fromRow
Starting index of the Joint motion subspace.
Definition: data.hpp:249
pinocchio::Tensor< Scalar, 3, Options >
pinocchio::DataTpl::dAdq
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Definition: data.hpp:287
pinocchio::DataTpl::dVdq
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Definition: data.hpp:284
pinocchio::MotionTpl< Scalar, Options >
pinocchio::DataTpl::dhg
Force dhg
Centroidal momentum time derivative.
Definition: data.hpp:233
pinocchio::ModelTpl
Definition: fwd.hpp:23
pinocchio::DataTpl::hg
Force hg
Centroidal momentum quantity.
Definition: data.hpp:227
pinocchio::DataTpl::ddq_dq
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Definition: data.hpp:299
pinocchio::DataTpl::llt_JMinvJt
Eigen::LLT< MatrixXs > llt_JMinvJt
Cholesky decompostion of .
Definition: data.hpp:335
pinocchio::serialization::Serializable
Definition: serializable.hpp:16
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24