pinocchio  2.6.16
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
data.hpp
1 //
2 // Copyright (c) 2015-2022 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(Matrix6) B;
188 
190  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias;
191 
193  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb;
194 
196  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
197 
199  Matrix6 Itmp;
200 
202  Matrix6 M6tmp;
203  RowMatrix6 M6tmpR;
204  RowMatrix6 M6tmpR2;
205 
208 
209  // ABA internal data
211  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6
212 
214  TangentVectorType u; // Joint Inertia
215 
216  // CCRBA return quantities
220 
221  // dCCRBA return quantities
225 
231 
237 
241 
243  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb;
244 
246  std::vector<int> lastChild;
247 
249  std::vector<int> nvSubtree;
250 
252  std::vector<int> start_idx_v_fromRow;
253 
255  std::vector<int> end_idx_v_fromRow;
256 
258  MatrixXs U;
259 
261  VectorXs D;
262 
264  VectorXs Dinv;
265 
267  VectorXs tmp;
268 
270  std::vector<int> parents_fromRow;
271 
274  std::vector< std::vector<int> > supports_fromRow;
275 
277  std::vector<int> nvSubtree_fromRow;
278 
282 
285 
288 
291 
294 
296  MatrixXs dtau_dq;
297 
299  MatrixXs dtau_dv;
300 
302  MatrixXs ddq_dq;
303 
305  MatrixXs ddq_dv;
306 
308  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf;
309 
311  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com;
312 
314  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
315 
317  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
318 
320  std::vector<Scalar> mass;
321 
325 
328 
331 
332  // Temporary variables used in forward dynamics
333 
335  MatrixXs JMinvJt;
336 
338  Eigen::LLT<MatrixXs> llt_JMinvJt;
339 
341  VectorXs lambda_c;
342 
344  MatrixXs sDUiJt;
345 
347  VectorXs torque_residual;
348 
351 
353  VectorXs impulse_c;
354 
357 
360 
363 
366 
372  explicit DataTpl(const Model & model);
373 
377  DataTpl() {}
378 
379  private:
380  void computeLastChild(const Model & model);
381  void computeParents_fromRow(const Model & model);
382  void computeSupports_fromRow(const Model & model);
383 
384  };
385 
386 } // namespace pinocchio
387 
388 /* --- Details -------------------------------------------------------------- */
389 /* --- Details -------------------------------------------------------------- */
390 /* --- Details -------------------------------------------------------------- */
391 #include "pinocchio/multibody/data.hxx"
392 
393 #endif // ifndef __pinocchio_multibody_data_hpp__
394 
pinocchio::DataTpl::DataTpl
DataTpl()
Default constructor.
Definition: data.hpp:377
pinocchio::DataTpl::lambda_c
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition: data.hpp:341
pinocchio::DataTpl::tmp
VectorXs tmp
Temporary of size NV used in Cholesky Decomposition.
Definition: data.hpp:267
pinocchio::DataTpl::bodyRegressor
BodyRegressorType bodyRegressor
Body regressor.
Definition: data.hpp:359
pinocchio::DataTpl
Definition: data.hpp:29
pinocchio::DataTpl::Ag
Matrix6x Ag
Centroidal Momentum Matrix.
Definition: data.hpp:219
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:274
pinocchio::DataTpl::Tensor3x
Tensor< Scalar, 3, Options > Tensor3x
&#160;
Definition: data.hpp:83
pinocchio::DataTpl::Jcom
Matrix3x Jcom
Jacobian of center of mass.
Definition: data.hpp:324
pinocchio::DataTpl::dAg
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
Definition: data.hpp:224
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:362
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::DataTpl::u
TangentVectorType u
Intermediate quantity corresponding to apparent torque [ABA].
Definition: data.hpp:214
pinocchio::DataTpl::torque_residual
VectorXs torque_residual
Temporary corresponding to the residual torque .
Definition: data.hpp:347
pinocchio::DataTpl::dJ
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: data.hpp:284
pinocchio::DataTpl::dAdv
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
Definition: data.hpp:293
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:277
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:350
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:365
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:240
pinocchio::DataTpl::staticRegressor
Matrix3x staticRegressor
Matrix related to static regressor.
Definition: data.hpp:356
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:32
pinocchio::DataTpl::M6tmp
Matrix6 M6tmp
Temporary for derivative algorithms.
Definition: data.hpp:202
pinocchio::DataTpl::ddq
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition: data.hpp:207
pinocchio::DataTpl::potential_energy
Scalar potential_energy
Potential energy of the model.
Definition: data.hpp:330
pinocchio::DataTpl::kinetic_energy
Scalar kinetic_energy
Kinetic energy of the model.
Definition: data.hpp:327
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:261
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:305
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:199
pinocchio::JointModelTpl
Definition: fwd.hpp:93
pinocchio::DataTpl::lastChild
std::vector< int > lastChild
Index of the last child (for CRBA)
Definition: data.hpp:246
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:255
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:344
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:299
pinocchio::DataTpl::dtau_dq
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: data.hpp:296
pinocchio::DataTpl::JMinvJt
MatrixXs JMinvJt
Inverse of the operational-space inertia matrix.
Definition: data.hpp:335
pinocchio::DataTpl::U
MatrixXs U
Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decompositi...
Definition: data.hpp:258
pinocchio::DataTpl::Dinv
VectorXs Dinv
Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Definition: data.hpp:264
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:353
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:320
pinocchio::DataTpl::parents_fromRow
std::vector< int > parents_fromRow
First previous non-zero row in M (used in Cholesky Decomposition).
Definition: data.hpp:270
pinocchio::DataTpl::J
Matrix6x J
Jacobian of joint placements.
Definition: data.hpp:281
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:249
pinocchio::DataTpl::start_idx_v_fromRow
std::vector< int > start_idx_v_fromRow
Starting index of the Joint motion subspace.
Definition: data.hpp:252
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:290
pinocchio::DataTpl::dVdq
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Definition: data.hpp:287
pinocchio::MotionTpl< Scalar, Options >
pinocchio::DataTpl::dhg
Force dhg
Centroidal momentum time derivative.
Definition: data.hpp:236
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl >
pinocchio::DataTpl::hg
Force hg
Centroidal momentum quantity.
Definition: data.hpp:230
pinocchio::DataTpl::ddq_dq
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Definition: data.hpp:302
pinocchio::DataTpl::llt_JMinvJt
Eigen::LLT< MatrixXs > llt_JMinvJt
Cholesky decompostion of .
Definition: data.hpp:338
pinocchio::serialization::Serializable
Definition: serializable.hpp:16
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11