pinocchio  2.3.1-dirty
multibody/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) oYcrb;
188 
190  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
191 
193  Matrix6 Itmp;
194 
196  Matrix6 M6tmp;
197  RowMatrix6 M6tmpR;
198  RowMatrix6 M6tmpR2;
199 
201  TangentVectorType ddq;
202 
203  // ABA internal data
205  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6
206 
208  TangentVectorType u; // Joint Inertia
209 
210  // CCRBA return quantities
213  Matrix6x Ag;
214 
215  // dCCRBA return quantities
218  Matrix6x dAg;
219 
224  Force hg;
225 
230  Force dhg;
231 
234  Inertia Ig;
235 
237  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb;
238 
240  std::vector<int> lastChild;
241 
243  std::vector<int> nvSubtree;
244 
246  std::vector<int> start_idx_v_fromRow;
247 
249  std::vector<int> end_idx_v_fromRow;
250 
252  MatrixXs U;
253 
255  VectorXs D;
256 
258  VectorXs Dinv;
259 
261  VectorXs tmp;
262 
264  std::vector<int> parents_fromRow;
265 
268  std::vector< std::vector<int> > supports_fromRow;
269 
271  std::vector<int> nvSubtree_fromRow;
272 
275  Matrix6x J;
276 
278  Matrix6x dJ;
279 
281  Matrix6x dVdq;
282 
284  Matrix6x dAdq;
285 
287  Matrix6x dAdv;
288 
290  MatrixXs dtau_dq;
291 
293  MatrixXs dtau_dv;
294 
296  MatrixXs ddq_dq;
297 
299  MatrixXs ddq_dv;
300 
302  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf;
303 
305  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com;
306 
308  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
309 
311  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
312 
314  std::vector<Scalar> mass;
315 
318  Matrix3x Jcom;
319 
322 
325 
326  // Temporary variables used in forward dynamics
327 
329  MatrixXs JMinvJt;
330 
332  Eigen::LLT<MatrixXs> llt_JMinvJt;
333 
335  VectorXs lambda_c;
336 
338  MatrixXs sDUiJt;
339 
341  VectorXs torque_residual;
342 
344  TangentVectorType dq_after;
345 
347  VectorXs impulse_c;
348 
350  Matrix3x staticRegressor;
351 
353  BodyRegressorType bodyRegressor;
354 
357 
360 
366  explicit DataTpl(const Model & model);
367 
371  DataTpl() {}
372 
373  private:
374  void computeLastChild(const Model & model);
375  void computeParents_fromRow(const Model & model);
376  void computeSupports_fromRow(const Model & model);
377 
378  };
379 
380 } // namespace pinocchio
381 
382 /* --- Details -------------------------------------------------------------- */
383 /* --- Details -------------------------------------------------------------- */
384 /* --- Details -------------------------------------------------------------- */
385 #include "pinocchio/multibody/data.hxx"
386 
387 #endif // ifndef __pinocchio_multibody_data_hpp__
388 
MatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, encapsulated in JointDataAccessor.
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Eigen::LLT< MatrixXs > llt_JMinvJt
Cholesky decompostion of .
Matrix3x Jcom
Jacobian of center of mass.
std::vector< int > lastChild
Index of the last child (for CRBA)
Matrix6x UDinv
Used in computeMinverse.
MatrixXs jointTorqueRegressor
Matrix related to joint torque regressor.
VectorXs Dinv
Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Tensor< Scalar, 3, Options > Tensor3x
 
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
MatrixXs JMinvJt
Inverse of the operational-space inertia matrix.
TangentVectorType dq_after
Generalized velocity after impact.
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
MatrixXs sDUiJt
Temporary corresponding to .
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Inertia Ig
Centroidal Composite Rigid Body Inertia.
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
Matrix6x Ag
Centroidal Momentum Matrix.
TangentVectorType u
Intermediate quantity corresponding to apparent torque [ABA].
VectorXs torque_residual
Temporary corresponding to the residual torque .
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
Matrix3x staticRegressor
Matrix related to static regressor.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
std::vector< int > nvSubtree_fromRow
Subtree of the current row index (used in Cholesky Decomposition).
Force dhg
Centroidal momentum time derivative.
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Matrix6x dHdq
Variation of the spatial momenta with respect to the joint configuration.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Tensor3x kinematic_hessians
Tensor containing the kinematic Hessian of all the joints.
VectorXs g
Vector of generalized gravity (dim model.nv).
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis...
MatrixXs C
The Coriolis matrix (a square matrix of dim model.nv).
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
TangentVectorType ddq
The joint accelerations computed from ABA.
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).
Matrix6 Itmp
Temporary for derivative algorithms.
BodyRegressorType bodyRegressor
Body regressor.
std::vector< Scalar > mass
Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint ...
MatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity. ...
VectorXs D
Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition. ...
DataTpl()
Default constructor.
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
std::vector< int > end_idx_v_fromRow
End index of the Joint motion subspace.
MatrixXs U
Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decompositi...
std::vector< std::vector< int > > supports_fromRow
Each element of this vector corresponds to the ordered list of indexes belonging to the supporting tr...
Matrix6x J
Jacobian of joint placements.
VectorXs tmp
Temporary of size NV used in Cholesky Decomposition.
VectorXs impulse_c
Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
Main pinocchio namespace.
Definition: treeview.dox:24
TangentVectorType tau
Vector of joint torques (dim model.nv).
std::vector< int > parents_fromRow
First previous non-zero row in M (used in Cholesky Decomposition).
std::vector< int > start_idx_v_fromRow
Starting index of the Joint motion subspace.
Matrix6x IS
Used in computeMinverse.
Matrix6x SDinv
Used in computeMinverse.
std::vector< int > nvSubtree
Dimension of the subtree motion space (for CRBA)
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Scalar kinetic_energy
Kinetic energy of the model.
Matrix6 M6tmp
Temporary for derivative algorithms.
Eigen::Matrix< Scalar, 6, 10, Options > BodyRegressorType
The type of the body regressor.
Force hg
Centroidal momentum quantity.
Scalar potential_energy
Potential energy of the model.