pinocchio  3.0.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
data.hpp
1 //
2 // Copyright (c) 2015-2024 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/spatial/fwd.hpp"
10 #include "pinocchio/math/tensor.hpp"
11 
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 #include "pinocchio/algorithm/contact-cholesky.hpp"
20 
21 #include "pinocchio/serialization/serializable.hpp"
22 
23 #include <Eigen/Cholesky>
24 #include <Eigen/StdVector>
25 #include <Eigen/src/Core/util/Constants.h>
26 
27 #include <cstddef>
28 #include <set>
29 
30 namespace pinocchio
31 {
32 
33  template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
34  struct traits<DataTpl<_Scalar, _Options, JointCollectionTpl>>
35  {
36  typedef _Scalar Scalar;
37  };
38 
39  template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
40  struct DataTpl
41  : serialization::Serializable<DataTpl<_Scalar, _Options, JointCollectionTpl>>
42  , NumericalBase<DataTpl<_Scalar, _Options, JointCollectionTpl>>
43  {
44  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 
46  typedef _Scalar Scalar;
47  enum
48  {
49  Options = _Options
50  };
51 
52  typedef JointCollectionTpl<Scalar, Options> JointCollection;
53 
55 
61 
62  typedef pinocchio::Index Index;
63  typedef pinocchio::JointIndex JointIndex;
64  typedef pinocchio::GeomIndex GeomIndex;
65  typedef pinocchio::FrameIndex FrameIndex;
66  typedef std::vector<Index> IndexVector;
67 
70 
71  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
72  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
73 
74  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
75  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
76  typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
77  typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
78 
79  typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6c;
80  typedef Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor | Options> Vector6r;
81 
83  typedef VectorXs ConfigVectorType;
84 
88  typedef VectorXs TangentVectorType;
89 
91  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
93  typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic, Options> Matrix3x;
94 
95  typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
96  typedef Eigen::Matrix<Scalar, 6, 6, Eigen::RowMajor | Options> RowMatrix6;
97  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Options>
98  RowMatrixXs;
99 
101  typedef Eigen::Matrix<Scalar, 6, 10, Options> BodyRegressorType;
102 
105 
106  // TODO Remove when API is stabilized
107  PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
108  PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
110  PINOCCHIO_COMPILER_DIAGNOSTIC_POP
111 
114  JointDataVector joints;
115 
118 
121 
125 
129 
132 
136 
139 
142 
147 
152 
158 
161 
164 
167 
170 
173 
178  VectorXs nle;
179 
183  VectorXs g;
184 
187 
192 
195  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb; // TODO: change with dense symmetric matrix6
196 
198  MatrixXs M;
199 
201  RowMatrixXs Minv;
202 
204  MatrixXs C;
205 
208 
211 
214 
217 
220 
223 
226 
229 
232 
236 
239 
242 
245 
247  Matrix6 Itmp;
248 
250  Matrix6 M6tmp;
251  RowMatrix6 M6tmpR;
252  RowMatrix6 M6tmpR2;
253 
256 
257  // ABA internal data
260  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6
261 
264  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oYaba; // TODO: change with dense symmetric matrix6
265 
268  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6)
269  oYaba_contact; // TODO: change with dense symmetric matrix6
270 
272  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oL; // TODO: change with dense symmetric matrix6
273 
275  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oK; // TODO: change with dense symmetric matrix6
276 
278  TangentVectorType u; // Joint Inertia
279 
280  // CCRBA return quantities
284 
285  // dCCRBA return quantities
290 
297 
305 
310 
312  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb;
313 
315  std::vector<int> lastChild;
316 
318  std::vector<int> nvSubtree;
319 
321  std::vector<int> start_idx_v_fromRow;
322 
324  std::vector<int> end_idx_v_fromRow;
325 
328  MatrixXs U;
329 
331  VectorXs D;
332 
335  VectorXs Dinv;
336 
338  VectorXs tmp;
339 
341  std::vector<int> parents_fromRow;
342 
347  std::vector<std::vector<int>> supports_fromRow;
348 
350  std::vector<int> nvSubtree_fromRow;
351 
360 
363 
366 
370 
374 
377 
380 
383 
386  RowMatrixXs dtau_dq;
387 
389  RowMatrixXs dtau_dv;
390 
393  RowMatrixXs ddq_dq;
394 
397  RowMatrixXs ddq_dv;
398 
401  RowMatrixXs ddq_dtau;
402 
405  MatrixXs dvc_dq;
406  MatrixXs dac_dq;
407  MatrixXs dac_dv;
408  MatrixXs dac_da;
409 
411  MatrixXs osim;
412 
415  MatrixXs dlambda_dq;
416  MatrixXs dlambda_dv;
417  MatrixXs dlambda_dtau;
418  MatrixXs dlambda_dx_prox, drhs_prox;
419 
421  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf;
422 
427  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com;
428 
433  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
434 
439  PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
440 
443  std::vector<Scalar> mass;
444 
450 
453 
456 
459 
460  // Temporary variables used in forward dynamics
461 
463  MatrixXs JMinvJt;
464 
466  Eigen::LLT<MatrixXs> llt_JMinvJt;
467 
470  VectorXs lambda_c;
471 
474  VectorXs lambda_c_prox;
475 
477  VectorXs diff_lambda_c;
478 
480  MatrixXs sDUiJt;
481 
483  VectorXs torque_residual;
484 
487 
490  VectorXs impulse_c;
491 
494 
497 
500 
501  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) KA;
502  PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) LA;
503  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lA;
504  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lambdaA;
505  PINOCCHIO_ALIGNED_STD_VECTOR(int) par_cons_ind;
506  PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_bias;
507  PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) KAS;
508  PINOCCHIO_ALIGNED_STD_VECTOR(int) constraint_ind;
509  Eigen::LLT<MatrixXs> osim_llt;
510 
511 #if defined(_MSC_VER)
512  // Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check
513  // operator precedence for possible error
514  #pragma warning(disable : 4554)
515 #endif
516 
519 
520 #if defined(_MSC_VER)
521  #pragma warning(default : 4554) // C4554 enabled after tensor definition
522 #endif
523 
526 
529 
532 
533 #if defined(_MSC_VER)
534  // Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check
535  // operator precedence for possible error
536  #pragma warning(disable : 4554)
537 #endif
538 
542 
546 
550 
556 
557 #if defined(_MSC_VER)
558  #pragma warning(default : 4554) // C4554 enabled after tensor definition
559 #endif
560 
561  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6)
562  extended_motion_propagator; // Stores force propagator to the base link
563  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) extended_motion_propagator2;
564  PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) spatial_inv_inertia; // Stores spatial inverse inertia
565  PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_descendant;
566  PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_ancestor;
567  PINOCCHIO_ALIGNED_STD_VECTOR(int) constraints_supported_dim;
568  PINOCCHIO_ALIGNED_STD_VECTOR(std::set<size_t>) constraints_supported;
569  PINOCCHIO_ALIGNED_STD_VECTOR(size_t) joints_supporting_constraints;
570  PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_joints;
571  PINOCCHIO_ALIGNED_STD_VECTOR(std::vector<size_t>) constraints_on_joint;
572 
578  explicit DataTpl(const Model & model);
579 
584  {
585  }
586 
587  private:
588  void computeLastChild(const Model & model);
589  void computeParents_fromRow(const Model & model);
590  void computeSupports_fromRow(const Model & model);
591  };
592 
593 } // namespace pinocchio
594 
595 /* --- Details -------------------------------------------------------------- */
596 /* --- Details -------------------------------------------------------------- */
597 /* --- Details -------------------------------------------------------------- */
598 #include "pinocchio/multibody/data.hxx"
599 
600 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
601  #include "pinocchio/multibody/data.txx"
602 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
603 
604 #endif // ifndef __pinocchio_multibody_data_hpp__
Main pinocchio namespace.
Definition: treeview.dox:11
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:198
PINOCCHIO_ALIGNED_STD_VECTOR(Force) of_augmented
Vector of body forces expressed in the world frame. For each body, the force represents the sum of al...
Tensor3x d2tau_dvdv
SO Partial derivative of the joint torque vector with respect to the joint velocity.
Definition: data.hpp:545
MatrixXs JMinvJt
Inverse of the operational-space inertia matrix.
Definition: data.hpp:463
Scalar potential_energy
Potential energy of the system.
Definition: data.hpp:455
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
Definition: data.hpp:216
Inertia Ig
Centroidal Composite Rigid Body Inertia.
Definition: data.hpp:309
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Definition: data.hpp:213
Matrix6x IS
Used in computeMinverse.
Definition: data.hpp:225
MatrixXs osim
Operational space inertia matrix;.
Definition: data.hpp:411
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_gf
Vector of joint accelerations expressed at the origin of the world including the gravity contribution...
PINOCCHIO_ALIGNED_STD_VECTOR(Force) h
Vector of spatial momenta expressed in the local frame of the joint.
std::vector< int > nvSubtree
Dimension of the subtree motion space (for CRBA)
Definition: data.hpp:318
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_gf
Vector of joint accelerations due to the gravity field.
VectorXs primal_rhs_contact
Primal RHS in contact dynamic equations.
Definition: data.hpp:531
VectorXs diff_lambda_c
Difference between two consecutive iterations of the proxy algorithm.
Definition: data.hpp:477
MatrixXs jointTorqueRegressor
Matrix related to joint torque regressor.
Definition: data.hpp:499
Scalar mechanical_energy
Mechanical energy of the system.
Definition: data.hpp:458
std::vector< int > lastChild
Index of the last child (for CRBA)
Definition: data.hpp:315
Matrix6x Ag
Centroidal Momentum Matrix.
Definition: data.hpp:283
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) Ycrb
Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported ...
TangentVectorType u
Intermediate quantity corresponding to apparent torque [ABA].
Definition: data.hpp:278
DataTpl()
Default constructor.
Definition: data.hpp:583
oYaba_contact
Articulated Body Inertia matrix with contact apparent inertia, of a given the subtree and expressed i...
Definition: data.hpp:269
VectorXs torque_residual
Temporary corresponding to the residual torque .
Definition: data.hpp:483
Tensor3x d2tau_dqdq
SO Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: data.hpp:541
Tensor< Scalar, 3, Options > Tensor3x
&#160;
Definition: data.hpp:104
Tensor3x d2tau_dqdv
SO Cross-Partial derivative of the joint torque vector with respect to the joint configuration/veloci...
Definition: data.hpp:549
Matrix6x dHdq
Variation of the spatial momenta set with respect to the joint configuration.
Definition: data.hpp:207
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition: data.hpp:470
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) B
Combined variations of the inertia matrix consistent with Christoffel symbols.
MatrixXs dlambda_dq
Partial derivatives of the constraints forces with respect to the joint configuration,...
Definition: data.hpp:415
Tensor3x d2tau_dadq
SO Cross-Partial derivative of the joint torque vector with respect to the joint acceleration/configu...
Definition: data.hpp:555
RowMatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity.
Definition: data.hpp:389
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb
Vector of sub-tree composite rigid body inertia time derivatives . See Data::Ycrb for more details.
VectorXs Dinv
Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Definition: data.hpp:335
VectorXs D
Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
Definition: data.hpp:331
VectorXs impulse_c
Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
Definition: data.hpp:490
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
Definition: data.hpp:382
std::vector< int > end_idx_v_fromRow
End index of the Joint motion subspace.
Definition: data.hpp:324
PINOCCHIO_ALIGNED_STD_VECTOR(Force) oh
Vector of spatial momenta expressed at the origin of the world.
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Definition: data.hpp:376
Matrix6 Itmp
Temporary for derivative algorithms.
Definition: data.hpp:247
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v
Vector of joint velocities expressed in the local frame of the joint.
VectorXs g
Vector of generalized gravity (dim model.nv).
Definition: data.hpp:183
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oYaba
Articulated Body Inertia matrix of the subtree expressed in the WORLD coordinate frame.
MatrixXs sDUiJt
Temporary corresponding to .
Definition: data.hpp:480
DataTpl(const Model &model)
Default constructor of pinocchio::Data from a pinocchio::Model.
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa
Vector of joint accelerations expressed at the origin of the world.
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Definition: data.hpp:88
VectorXs lambda_c_prox
Proximal Lagrange Multipliers used in the computation of the Forward Dynamics computations.
Definition: data.hpp:474
std::vector< int > parents_fromRow
First previous non-zero row in M (used in Cholesky Decomposition).
Definition: data.hpp:341
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: data.hpp:172
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) liMi
Vector of relative joint placements (wrt the body parent).
Eigen::LLT< MatrixXs > llt_JMinvJt
Cholesky decompostion of .
Definition: data.hpp:466
Eigen::Matrix< Scalar, 6, 10, Options > BodyRegressorType
The type of the body regressor.
Definition: data.hpp:101
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: data.hpp:91
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias
Rigid Body Inertia supported by the joint expressed in the world frame.
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Definition: data.hpp:210
Matrix3x Jcom
Jacobian of center of mass.
Definition: data.hpp:449
RowMatrixXs ddq_dtau
Partial derivative of the joint acceleration vector with respect to the joint torques.
Definition: data.hpp:401
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_drift
Vector of joint accelerations expressed at the origin of the world. These accelerations are used in t...
RowMatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Definition: data.hpp:397
std::vector< int > nvSubtree_fromRow
Subtree of the current row index (used in Cholesky Decomposition).
Definition: data.hpp:350
TangentVectorType ddq
The joint accelerations computed from ABA.
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:347
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMi
Vector of absolute joint placements (wrt the world).
TangentVectorType dq_after
Generalized velocity after impact.
Definition: data.hpp:486
RowMatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: data.hpp:386
MatrixXs dvc_dq
Stack of partial derivative of the contact frame acceleration with respect to the joint parameters.
Definition: data.hpp:405
MatrixXs U
Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decompositi...
Definition: data.hpp:328
Force hg
Centroidal momentum quantity.
Definition: data.hpp:296
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb
Time variation of Composite Rigid Body Inertia expressed in the world frame.
Scalar kinetic_energy
Kinetic energy of the system.
Definition: data.hpp:452
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
Definition: data.hpp:373
Matrix6x SDinv
Used in computeMinverse.
Definition: data.hpp:219
Tensor3x kinematic_hessians
Tensor containing the kinematic Hessian of all the joints.
Definition: data.hpp:518
PINOCCHIO_ALIGNED_STD_VECTOR(Force) f
Vector of body forces expressed in the local frame of the joint. For each body, the force represents ...
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx
Left variation of the inertia matrix.
Matrix6 M6tmp
Temporary for derivative algorithms.
Definition: data.hpp:250
ContactCholeskyDecomposition contact_chol
Cholesky decomposition of the KKT contact matrix.
Definition: data.hpp:525
PINOCCHIO_COMPILER_DIAGNOSTIC_POP JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model,...
Definition: data.hpp:114
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:443
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
Definition: data.hpp:289
VectorXs primal_dual_contact_solution
RHS vector when solving the contact dynamics KKT problem.
Definition: data.hpp:528
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov
Vector of joint velocities expressed at the origin of the world.
Force dhg
Centroidal momentum time derivative.
Definition: data.hpp:304
Matrix6x J
Jacobian of joint placements.
Definition: data.hpp:359
Matrix6x psid
psidot Derivative of Jacobian w.r.t to the parent body moving v(p(j)) x Sj
Definition: data.hpp:369
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:201
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis,...
Definition: data.hpp:178
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: data.hpp:93
PINOCCHIO_ALIGNED_STD_VECTOR(Force) of
Vector of body forces expressed at the origin of the world. For each body, the force represents the s...
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: data.hpp:362
MatrixXs C
The Coriolis matrix (a square matrix of dim model.nv).
Definition: data.hpp:204
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: data.hpp:83
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_augmented
Vector of joint accelerations expressed at the origin of the world. These accelerations are used in t...
std::vector< int > start_idx_v_fromRow
Starting index of the Joint motion subspace.
Definition: data.hpp:321
VectorXs tmp
Temporary of size NV used in Cholesky Decomposition.
Definition: data.hpp:338
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a
Vector of joint accelerations expressed in the local frame of the joint.
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMf
Vector of absolute operationnel frame placements (wrt the world).
Matrix6x UDinv
Used in computeMinverse.
Definition: data.hpp:222
RowMatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Definition: data.hpp:393
Matrix3x staticRegressor
Matrix related to static regressor.
Definition: data.hpp:493
BodyRegressorType bodyRegressor
Body regressor.
Definition: data.hpp:496
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) vxI
Right variation of the inertia matrix.
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba
Articulated Body Inertia matrix of the subtree expressed in the LOCAL coordinate frame of the joint.
Matrix6x ddJ
Second derivative of the Jacobian with respect to the time.
Definition: data.hpp:365
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Definition: data.hpp:379
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb
Composite Rigid Body Inertia expressed in the world frame.
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Definition: frame.hpp:56
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72