pinocchio  2.2.1-dirty
data.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_data_hpp__
7 #define __pinocchio_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 <iostream>
21 #include <Eigen/Cholesky>
22 
23 namespace pinocchio
24 {
25 
26  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
27  struct DataTpl
28  {
29  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
30 
31  typedef _Scalar Scalar;
32  enum { Options = _Options };
33 
34  typedef JointCollectionTpl<Scalar,Options> JointCollection;
35 
37 
43 
44  typedef pinocchio::Index Index;
45  typedef pinocchio::JointIndex JointIndex;
46  typedef pinocchio::GeomIndex GeomIndex;
47  typedef pinocchio::FrameIndex FrameIndex;
48  typedef std::vector<Index> IndexVector;
49 
52 
55 
56  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
57  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXs;
58  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
59 
61  typedef VectorXs ConfigVectorType;
62 
65  typedef VectorXs TangentVectorType;
66 
68  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x;
70  typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
71 
72  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
73  typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
74  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
75 
77  typedef Eigen::Matrix<Scalar,6,10,Options> BodyRegressorType;
78 
81 
84  JointDataVector joints;
85 
88 
91 
94 
97 
100 
103 
107 
111 
114 
117 
120 
123 
125  TangentVectorType tau;
126 
130  VectorXs nle;
131 
135  VectorXs g;
136 
139 
143 
145  container::aligned_vector<typename Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
146 
148  MatrixXs M;
149 
151  RowMatrixXs Minv;
152 
154  MatrixXs C;
155 
157  Matrix6x dHdq;
158 
160  Matrix6x dFdq;
161 
163  Matrix6x dFdv;
164 
166  Matrix6x dFda;
167 
169  Matrix6x SDinv;
170 
172  Matrix6x UDinv;
173 
175  Matrix6x IS;
176 
179 
182 
185 
188 
190  typename Inertia::Matrix6 Itmp;
191 
193  Matrix6 M6tmp;
194  RowMatrix6 M6tmpR;
195  RowMatrix6 M6tmpR2;
196 
198  TangentVectorType ddq;
199 
200  // ABA internal data
202  container::aligned_vector<typename Inertia::Matrix6> Yaba; // TODO: change with dense symmetric matrix6
203 
205  TangentVectorType u; // Joint Inertia
206 
207  // CCRBA return quantities
210  Matrix6x Ag;
211 
212  // dCCRBA return quantities
215  Matrix6x dAg;
216 
221  Force hg;
222 
227  Force dhg;
228 
231  Inertia Ig;
232 
235 
237  std::vector<int> lastChild;
238 
240  std::vector<int> nvSubtree;
241 
243  std::vector<int> start_idx_v_fromRow;
244 
246  std::vector<int> end_idx_v_fromRow;
247 
249  MatrixXs U;
250 
252  VectorXs D;
253 
255  VectorXs Dinv;
256 
258  VectorXs tmp;
259 
261  std::vector<int> parents_fromRow;
262 
265  std::vector< std::vector<int> > supports_fromRow;
266 
268  std::vector<int> nvSubtree_fromRow;
269 
272  Matrix6x J;
273 
275  Matrix6x dJ;
276 
278  Matrix6x dVdq;
279 
281  Matrix6x dAdq;
282 
284  Matrix6x dAdv;
285 
287  MatrixXs dtau_dq;
288 
290  MatrixXs dtau_dv;
291 
293  MatrixXs ddq_dq;
294 
296  MatrixXs ddq_dv;
297 
300 
303 
306 
309 
311  std::vector<Scalar> mass;
312 
315  Matrix3x Jcom;
316 
319 
322 
323  // Temporary variables used in forward dynamics
324 
326  MatrixXs JMinvJt;
327 
329  Eigen::LLT<MatrixXs> llt_JMinvJt;
330 
332  VectorXs lambda_c;
333 
335  MatrixXs sDUiJt;
336 
338  VectorXs torque_residual;
339 
341  TangentVectorType dq_after;
342 
344  VectorXs impulse_c;
345 
347  Matrix3x staticRegressor;
348 
350  BodyRegressorType bodyRegressor;
351 
354 
357 
363  explicit DataTpl(const Model & model);
364 
365  private:
366  void computeLastChild(const Model & model);
367  void computeParents_fromRow(const Model & model);
368  void computeSupports_fromRow(const Model & model);
369 
370  };
371 
372 } // namespace pinocchio
373 
374 /* --- Details -------------------------------------------------------------- */
375 /* --- Details -------------------------------------------------------------- */
376 /* --- Details -------------------------------------------------------------- */
377 #include "pinocchio/multibody/data.hxx"
378 
379 #endif // ifndef __pinocchio_data_hpp__
380 
DataTpl(const Model &model)
Default constructor of pinocchio::Data from a pinocchio::Model.
MatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Definition: data.hpp:296
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, encapsulated in JointDataAccessor.
Definition: data.hpp:84
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
Definition: data.hpp:166
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:151
Eigen::LLT< MatrixXs > llt_JMinvJt
Cholesky decompostion of .
Definition: data.hpp:329
Matrix3x Jcom
Jacobian of center of mass.
Definition: data.hpp:315
container::aligned_vector< Force > oh
Vector of spatial momenta expressed in the world frame.
Definition: data.hpp:116
container::aligned_vector< Vector3 > vcom
Vector of subtree center of mass linear velocities expressed in the root joint of the subtree...
Definition: data.hpp:305
std::vector< int > lastChild
Index of the last child (for CRBA)
Definition: data.hpp:237
Matrix6x UDinv
Used in computeMinverse.
Definition: data.hpp:172
MatrixXs jointTorqueRegressor
Matrix related to joint torque regressor.
Definition: data.hpp:353
VectorXs Dinv
Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Definition: data.hpp:255
Tensor< Scalar, 3, Options > Tensor3x
 
Definition: data.hpp:80
container::aligned_vector< Motion > a_gf
Vector of joint accelerations due to the gravity field.
Definition: data.hpp:93
container::aligned_vector< Motion > a
Vector of joint accelerations expressed at the centers of the joints frames.
Definition: data.hpp:87
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: data.hpp:287
MatrixXs JMinvJt
Inverse of the operational-space inertia matrix.
Definition: data.hpp:326
container::aligned_vector< Inertia > oYcrb
Inertia quantities expressed in the world frame.
Definition: data.hpp:184
TangentVectorType dq_after
Generalized velocity after impact.
Definition: data.hpp:341
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
MatrixXs sDUiJt
Temporary corresponding to .
Definition: data.hpp:335
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Definition: data.hpp:160
Inertia Ig
Centroidal Composite Rigid Body Inertia.
Definition: data.hpp:231
container::aligned_vector< Force > h
Vector of spatial momenta expressed in the local frame of the joint.
Definition: data.hpp:113
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
Definition: data.hpp:284
Matrix6x Ag
Centroidal Momentum Matrix.
Definition: data.hpp:210
TangentVectorType u
Intermediate quantity corresponding to apparent torque [ABA].
Definition: data.hpp:205
VectorXs torque_residual
Temporary corresponding to the residual torque .
Definition: data.hpp:338
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Definition: data.hpp:163
container::aligned_vector< Matrix6x > Fcrb
Spatial forces set, used in CRBA and CCRBA.
Definition: data.hpp:234
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
Definition: data.hpp:215
container::aligned_vector< SE3 > oMi
Vector of absolute joint placements (wrt the world).
Definition: data.hpp:119
Matrix3x staticRegressor
Matrix related to static regressor.
Definition: data.hpp:347
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: data.hpp:61
std::vector< int > nvSubtree_fromRow
Subtree of the current row index (used in Cholesky Decomposition).
Definition: data.hpp:268
Force dhg
Centroidal momentum time derivative.
Definition: data.hpp:227
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Definition: data.hpp:281
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Definition: data.hpp:293
Matrix6x dHdq
Variation of the spatial momenta with respect to the joint configuration.
Definition: data.hpp:157
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: data.hpp:68
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:148
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Definition: data.hpp:275
Tensor3x kinematic_hessians
Tensor containing the kinematic Hessian of all the joints.
Definition: data.hpp:356
Inertia::Matrix6 Itmp
Temporary for derivative algorithms.
Definition: data.hpp:190
VectorXs g
Vector of generalized gravity (dim model.nv).
Definition: data.hpp:135
container::aligned_vector< typename Inertia::Matrix6 > Ivx
Left variation of the inertia matrix.
Definition: data.hpp:181
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis...
Definition: data.hpp:130
container::aligned_vector< Motion > ov
Vector of joint velocities expressed at the origin.
Definition: data.hpp:102
MatrixXs C
The Coriolis matrix (a square matrix of dim model.nv).
Definition: data.hpp:154
container::aligned_vector< Vector3 > acom
Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree...
Definition: data.hpp:308
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: data.hpp:70
container::aligned_vector< Motion > v
Vector of joint velocities expressed at the centers of the joints.
Definition: data.hpp:99
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition: data.hpp:198
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).
Definition: data.hpp:65
BodyRegressorType bodyRegressor
Body regressor.
Definition: data.hpp:350
container::aligned_vector< Force > f
Vector of body forces expressed in the local frame of the joint. For each body, the force represents ...
Definition: data.hpp:106
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:311
container::aligned_vector< typename Inertia::Matrix6 > doYcrb
Time variation of the inertia quantities expressed in the world frame.
Definition: data.hpp:187
MatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity. ...
Definition: data.hpp:290
VectorXs D
Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition. ...
Definition: data.hpp:252
container::aligned_vector< SE3 > oMf
Vector of absolute operationnel frame placements (wrt the world).
Definition: data.hpp:138
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition: data.hpp:332
std::vector< int > end_idx_v_fromRow
End index of the Joint motion subspace.
Definition: data.hpp:246
container::aligned_vector< Force > of
Vector of body forces expressed in the world frame. For each body, the force represents the sum of al...
Definition: data.hpp:110
MatrixXs U
Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decompositi...
Definition: data.hpp:249
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:265
Matrix6x J
Jacobian of joint placements.
Definition: data.hpp:272
VectorXs tmp
Temporary of size NV used in Cholesky Decomposition.
Definition: data.hpp:258
container::aligned_vector< Motion > oa_gf
Vector of joint accelerations expressed at the origin of the world including gravity contribution...
Definition: data.hpp:96
VectorXs impulse_c
Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
Definition: data.hpp:344
Main pinocchio namespace.
Definition: treeview.dox:24
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: data.hpp:125
container::aligned_vector< Vector3 > com
Vector of subtree center of mass positions expressed in the root joint of the subtree. In other words, com[j] is the CoM position of the subtree supported by joint and expressed in the joint frame . The element com[0] corresponds to the center of mass position of the whole model and expressed in the global frame.
Definition: data.hpp:302
std::vector< int > parents_fromRow
First previous non-zero row in M (used in Cholesky Decomposition).
Definition: data.hpp:261
std::vector< int > start_idx_v_fromRow
Starting index of the Joint motion subspace.
Definition: data.hpp:243
container::aligned_vector< typename Inertia::Matrix6 > dYcrb
Vector of sub-tree composite rigid body inertia time derivatives . See Data::Ycrb for more details...
Definition: data.hpp:145
Matrix6x IS
Used in computeMinverse.
Definition: data.hpp:175
Matrix6x SDinv
Used in computeMinverse.
Definition: data.hpp:169
container::aligned_vector< typename Inertia::Matrix6 > Yaba
Inertia matrix of the subtree expressed as dense matrix [ABA].
Definition: data.hpp:202
container::aligned_vector< SE3 > iMf
Vector of joint placements wrt to algorithm end effector.
Definition: data.hpp:299
std::vector< int > nvSubtree
Dimension of the subtree motion space (for CRBA)
Definition: data.hpp:240
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Definition: data.hpp:278
Scalar kinetic_energy
Kinetic energy of the model.
Definition: data.hpp:318
container::aligned_vector< typename Inertia::Matrix6 > vxI
Right variation of the inertia matrix.
Definition: data.hpp:178
Matrix6 M6tmp
Temporary for derivative algorithms.
Definition: data.hpp:193
Eigen::Matrix< Scalar, 6, 10, Options > BodyRegressorType
The type of the body regressor.
Definition: data.hpp:77
container::aligned_vector< SE3 > liMi
Vector of relative joint placements (wrt the body parent).
Definition: data.hpp:122
container::aligned_vector< Inertia > Ycrb
Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported ...
Definition: data.hpp:142
container::aligned_vector< Motion > oa
Vector of joint accelerations expressed at the origin of the world.
Definition: data.hpp:90
Force hg
Centroidal momentum quantity.
Definition: data.hpp:221
Scalar potential_energy
Potential energy of the model.
Definition: data.hpp:321