src/multibody/data.hpp
Go to the documentation of this file.
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 
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  typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6c;
63  typedef Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor | Options> Vector6r;
64 
66  typedef VectorXs ConfigVectorType;
67 
70  typedef VectorXs TangentVectorType;
71 
73  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
75  typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
76 
77  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
78  typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
79  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
80 
82  typedef Eigen::Matrix<Scalar,6,10,Options> BodyRegressorType;
83 
86 
89  JointDataVector joints;
90 
93 
96 
98  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_gf;
99 
101  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_gf;
102 
105 
107  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov;
108 
112 
116 
119 
122 
125 
128 
130  TangentVectorType tau;
131 
135  VectorXs nle;
136 
140  VectorXs g;
141 
144 
147  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) Ycrb;
148 
150  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb; // TODO: change with dense symmetric matrix6
151 
153  MatrixXs M;
154 
156  RowMatrixXs Minv;
157 
159  MatrixXs C;
160 
162  Matrix6x dHdq;
163 
165  Matrix6x dFdq;
166 
168  Matrix6x dFdv;
169 
171  Matrix6x dFda;
172 
174  Matrix6x SDinv;
175 
177  Matrix6x UDinv;
178 
180  Matrix6x IS;
181 
183  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) vxI;
184 
186  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx;
187 
190 
192  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias;
193 
195  PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb;
196 
198  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
199 
201  Matrix6 Itmp;
202 
204  Matrix6 M6tmp;
205  RowMatrix6 M6tmpR;
206  RowMatrix6 M6tmpR2;
207 
209  TangentVectorType ddq;
210 
211  // ABA internal data
213  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6
214 
216  TangentVectorType u; // Joint Inertia
217 
218  // CCRBA return quantities
221  Matrix6x Ag;
222 
223  // dCCRBA return quantities
226  Matrix6x dAg;
227 
232  Force hg;
233 
238  Force dhg;
239 
242  Inertia Ig;
243 
245  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb;
246 
248  std::vector<int> lastChild;
249 
251  std::vector<int> nvSubtree;
252 
254  std::vector<int> start_idx_v_fromRow;
255 
257  std::vector<int> end_idx_v_fromRow;
258 
260  MatrixXs U;
261 
263  VectorXs D;
264 
266  VectorXs Dinv;
267 
269  VectorXs tmp;
270 
272  std::vector<int> parents_fromRow;
273 
276  std::vector< std::vector<int> > supports_fromRow;
277 
279  std::vector<int> nvSubtree_fromRow;
280 
283  Matrix6x J;
284 
286  Matrix6x dJ;
287 
289  Matrix6x ddJ;
290 
293  Matrix6x psid;
294 
297  Matrix6x psidd;
298 
300  Matrix6x dVdq;
301 
303  Matrix6x dAdq;
304 
306  Matrix6x dAdv;
307 
309  MatrixXs dtau_dq;
310 
312  MatrixXs dtau_dv;
313 
315  MatrixXs ddq_dq;
316 
318  MatrixXs ddq_dv;
319 
322 
325 
327  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
328 
330  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
331 
333  std::vector<Scalar> mass;
334 
337  Matrix3x Jcom;
338 
341 
344 
345  // Temporary variables used in forward dynamics
346 
348  MatrixXs JMinvJt;
349 
351  Eigen::LLT<MatrixXs> llt_JMinvJt;
352 
354  VectorXs lambda_c;
355 
357  MatrixXs sDUiJt;
358 
360  VectorXs torque_residual;
361 
363  TangentVectorType dq_after;
364 
366  VectorXs impulse_c;
367 
369  Matrix3x staticRegressor;
370 
372  BodyRegressorType bodyRegressor;
373 
376 
379 
382  Tensor3x d2tau_dqdq;
383 
386  Tensor3x d2tau_dvdv;
387 
390  Tensor3x d2tau_dqdv;
391 
396  Tensor3x d2tau_dadq;
397 
403  explicit DataTpl(const Model & model);
404 
408  DataTpl() {}
409 
410  private:
411  void computeLastChild(const Model & model);
412  void computeParents_fromRow(const Model & model);
413  void computeSupports_fromRow(const Model & model);
414 
415  };
416 
417 } // namespace pinocchio
418 
419 /* --- Details -------------------------------------------------------------- */
420 /* --- Details -------------------------------------------------------------- */
421 /* --- Details -------------------------------------------------------------- */
422 #include "pinocchio/multibody/data.hxx"
423 
424 #endif // ifndef __pinocchio_multibody_data_hpp__
425 
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Matrix6x psidd
psiddot Second Derivative of Jacobian w.r.t to the parent body moving a(p(j)) x Sj + v(p(j)) x psidj ...
TangentVectorType tau
Vector of joint torques (dim model.nv).
std::vector< int > nvSubtree_fromRow
Subtree of the current row index (used in Cholesky Decomposition).
FrameTpl< Scalar, Options > Frame
Matrix6x psid
psidot Derivative of Jacobian w.r.t to the parent body moving v(p(j)) x Sj
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
JointDataTpl< Scalar, Options, JointCollectionTpl > JointData
VectorXs g
Vector of generalized gravity (dim model.nv).
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor|Options > RowMatrixXs
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.
Matrix6x UDinv
Used in computeMinverse.
MatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity. ...
MatrixXs jointTorqueRegressor
Matrix related to joint torque regressor.
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
VectorXs tmp
Temporary of size NV used in Cholesky Decomposition.
Tensor3x kinematic_hessians
Tensor containing the kinematic Hessian of all the joints.
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
void computeLastChild(const Model &model)
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, encapsulated in JointDataAccessor.
BodyRegressorType bodyRegressor
Body regressor.
std::vector< int > end_idx_v_fromRow
End index of the Joint motion subspace.
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector
Eigen::Matrix< Scalar, 6, 6, Eigen::RowMajor|Options > RowMatrix6
Eigen::Matrix< Scalar, 6, 1, Options > Vector6c
MotionTpl< Scalar, Options > Motion
VectorXs torque_residual
Temporary corresponding to the residual torque .
Tensor< Scalar, 3, Options > Tensor3x
 .
Eigen::LLT< MatrixXs > llt_JMinvJt
Cholesky decompostion of .
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
VectorXs D
Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition. ...
TangentVectorType u
Intermediate quantity corresponding to apparent torque [ABA].
ModelTpl< Scalar, Options, JointCollectionTpl > Model
Matrix6x ddJ
Second derivative of the Jacobian with respect to the time.
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Matrix6 Itmp
Temporary for derivative algorithms.
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis...
ForceTpl< Scalar, Options > Force
Matrix6x IS
Used in computeMinverse.
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
pinocchio::Index Index
std::vector< Index > IndexVector
MatrixXs JMinvJt
Inverse of the operational-space inertia matrix.
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Matrix3x Jcom
Jacobian of center of mass.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Tensor3x d2tau_dqdq
SO Partial derivative of the joint torque vector with respect to the joint configuration.
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
MatrixXs C
The Coriolis matrix (a square matrix of dim model.nv).
JointCollectionTpl< Scalar, Options > JointCollection
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
InertiaTpl< Scalar, Options > Inertia
Tensor3x d2tau_dqdv
SO Cross-Partial derivative of the joint torque vector with respect to the joint configuration/veloci...
Vec3f a
TangentVectorType ddq
The joint accelerations computed from ABA.
std::vector< int > lastChild
Index of the last child (for CRBA)
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Force dhg
Centroidal momentum time derivative.
pinocchio::JointIndex JointIndex
MatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Scalar kinetic_energy
Kinetic energy of the model.
Eigen::Matrix< Scalar, 1, 6, Eigen::RowMajor|Options > Vector6r
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Main pinocchio namespace.
Definition: timings.cpp:28
std::vector< int > start_idx_v_fromRow
Starting index of the Joint motion subspace.
VectorXs Dinv
Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
B
pinocchio::FrameIndex FrameIndex
Matrix6x dHdq
Variation of the spatial momenta with respect to the joint configuration.
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Matrix6x Ag
Centroidal Momentum Matrix.
TangentVectorType dq_after
Generalized velocity after impact.
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Matrix6x SDinv
Used in computeMinverse.
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
MatrixXs sDUiJt
Temporary corresponding to .
std::size_t Index
Inertia Ig
Centroidal Composite Rigid Body Inertia.
h
Definition: ur5x4.py:45
void computeParents_fromRow(const Model &model)
Matrix3x staticRegressor
Matrix related to static regressor.
VectorXs impulse_c
Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
Scalar potential_energy
Potential energy of the model.
std::vector< int > nvSubtree
Dimension of the subtree motion space (for CRBA)
std::vector< int > parents_fromRow
First previous non-zero row in M (used in Cholesky Decomposition).
DataTpl()
Default constructor.
Force hg
Centroidal momentum quantity.
pinocchio::GeomIndex GeomIndex
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
MatrixXs U
Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decompositi...
JointCollectionTpl & model
void computeSupports_fromRow(const Model &model)
Eigen::Matrix< Scalar, 6, 10, Options > BodyRegressorType
The type of the body regressor.
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
SE3Tpl< Scalar, Options > SE3
Matrix6 M6tmp
Temporary for derivative algorithms.
Tensor3x d2tau_dvdv
SO Partial derivative of the joint torque vector with respect to the joint velocity.
std::vector< Scalar > mass
Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint ...
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).
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Tensor3x d2tau_dadq
SO Cross-Partial derivative of the joint torque vector with respect to the joint acceleration/configu...


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:29