Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/data.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 3 | 3 | 100.0% |
Branches: | 127 | 254 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
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 | |||
54 | typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; | ||
55 | |||
56 | typedef SE3Tpl<Scalar, Options> SE3; | ||
57 | typedef MotionTpl<Scalar, Options> Motion; | ||
58 | typedef ForceTpl<Scalar, Options> Force; | ||
59 | typedef InertiaTpl<Scalar, Options> Inertia; | ||
60 | typedef FrameTpl<Scalar, Options> Frame; | ||
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 | |||
68 | typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel; | ||
69 | typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointData; | ||
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, 1, Eigen::Dynamic, Options | Eigen::RowMajor> RowVectorXs; | ||
77 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
78 | typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6; | ||
79 | |||
80 | typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6c; | ||
81 | typedef Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor | Options> Vector6r; | ||
82 | |||
83 | /// \brief Dense vectorized version of a joint configuration vector. | ||
84 | typedef VectorXs ConfigVectorType; | ||
85 | |||
86 | /// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, | ||
87 | /// etc). | ||
88 | /// It also handles the notion of co-tangent vector (e.g. torque, etc). | ||
89 | typedef VectorXs TangentVectorType; | ||
90 | |||
91 | /// \brief The 6d jacobian type (temporary) | ||
92 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x; | ||
93 | /// \brief The 3d jacobian type (temporary) | ||
94 | typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic, Options> Matrix3x; | ||
95 | |||
96 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
97 | typedef Eigen::Matrix<Scalar, 6, 6, Eigen::RowMajor | Options> RowMatrix6; | ||
98 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Options> | ||
99 | RowMatrixXs; | ||
100 | |||
101 | /// \brief The type of the body regressor | ||
102 | typedef Eigen::Matrix<Scalar, 6, 10, Options> BodyRegressorType; | ||
103 | |||
104 | /// \brief The type of Tensor for Kinematics and Dynamics second order derivatives | ||
105 | typedef Tensor<Scalar, 3, Options> Tensor3x; | ||
106 | |||
107 | // TODO Remove when API is stabilized | ||
108 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
109 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS | ||
110 | typedef ContactCholeskyDecompositionTpl<Scalar, Options> ContactCholeskyDecomposition; | ||
111 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
112 | |||
113 | /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in | ||
114 | /// model, encapsulated in JointDataAccessor. | ||
115 | JointDataVector joints; | ||
116 | |||
117 | /// \brief Vector of joint accelerations expressed in the local frame of the joint. | ||
118 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a; | ||
119 | |||
120 | /// \brief Vector of joint accelerations expressed at the origin of the world. | ||
121 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa; | ||
122 | |||
123 | /// \brief Vector of joint accelerations expressed at the origin of the world. | ||
124 | /// These accelerations are used in the context of augmented Lagrangian algorithms. | ||
125 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_drift; | ||
126 | |||
127 | /// \brief Vector of joint accelerations expressed at the origin of the world. | ||
128 | /// These accelerations are used in the context of augmented Lagrangian algorithms. | ||
129 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_augmented; | ||
130 | |||
131 | /// \brief Vector of joint accelerations due to the gravity field. | ||
132 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_gf; | ||
133 | |||
134 | /// \brief Vector of joint accelerations expressed at the origin of the world including the | ||
135 | /// gravity contribution. | ||
136 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_gf; | ||
137 | |||
138 | /// \brief Vector of joint velocities expressed in the local frame of the joint. | ||
139 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v; | ||
140 | |||
141 | /// \brief Vector of joint velocities expressed at the origin of the world. | ||
142 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov; | ||
143 | |||
144 | /// \brief Vector of body forces expressed in the local frame of the joint. For each body, the | ||
145 | /// force represents the sum of | ||
146 | /// all external forces acting on the body. | ||
147 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) f; | ||
148 | |||
149 | /// \brief Vector of body forces expressed at the origin of the world. For each body, the force | ||
150 | /// represents the sum of | ||
151 | /// all external forces acting on the body. | ||
152 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) of; | ||
153 | |||
154 | /// \brief Vector of body forces expressed in the world frame. For each body, the force | ||
155 | /// represents the sum of | ||
156 | /// all external forces acting on the body. These forces are used in the context of | ||
157 | /// augmented Lagrangian algorithms. | ||
158 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) of_augmented; | ||
159 | |||
160 | /// \brief Vector of spatial momenta expressed in the local frame of the joint. | ||
161 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) h; | ||
162 | |||
163 | /// \brief Vector of spatial momenta expressed at the origin of the world. | ||
164 | PINOCCHIO_ALIGNED_STD_VECTOR(Force) oh; | ||
165 | |||
166 | /// \brief Vector of absolute joint placements (wrt the world). | ||
167 | PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMi; | ||
168 | |||
169 | /// \brief Vector of relative joint placements (wrt the body parent). | ||
170 | PINOCCHIO_ALIGNED_STD_VECTOR(SE3) liMi; | ||
171 | |||
172 | /// \brief Vector of joint torques (dim model.nv). | ||
173 | TangentVectorType tau; | ||
174 | |||
175 | /// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the | ||
176 | /// Coriolis, centrifugal and gravitational effects. \note In the multibody dynamics equation | ||
177 | /// \f$ M\ddot{q} + b(q, \dot{q}) = \tau \f$, | ||
178 | /// the non linear effects are associated to the term \f$b\f$. | ||
179 | VectorXs nle; | ||
180 | |||
181 | /// \brief Vector of generalized gravity (dim model.nv). | ||
182 | /// \note In the multibody dynamics equation \f$ M\ddot{q} + c(q, \dot{q}) + g(q) = \tau \f$, | ||
183 | /// the gravity effect is associated to the \f$g\f$ term. | ||
184 | VectorXs g; | ||
185 | |||
186 | /// \brief Vector of absolute operationnel frame placements (wrt the world). | ||
187 | PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMf; | ||
188 | |||
189 | /// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the | ||
190 | /// subtree supported by the joint | ||
191 | /// and expressed in the local frame of the joint.. | ||
192 | PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) Ycrb; | ||
193 | |||
194 | /// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ | ||
195 | /// \dot{Y}_{crb}\f$. See Data::Ycrb for more details. | ||
196 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb; // TODO: change with dense symmetric matrix6 | ||
197 | |||
198 | /// \brief The joint space inertia matrix (a square matrix of dim model.nv). | ||
199 | MatrixXs M; | ||
200 | |||
201 | /// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv). | ||
202 | RowMatrixXs Minv; | ||
203 | |||
204 | /// \brief The Coriolis matrix (a square matrix of dim model.nv). | ||
205 | MatrixXs C; | ||
206 | |||
207 | /// \brief Variation of the spatial momenta set with respect to the joint configuration. | ||
208 | Matrix6x dHdq; | ||
209 | |||
210 | /// \brief Variation of the forceset with respect to the joint configuration. | ||
211 | Matrix6x dFdq; | ||
212 | |||
213 | /// \brief Variation of the forceset with respect to the joint velocity. | ||
214 | Matrix6x dFdv; | ||
215 | |||
216 | /// \brief Variation of the forceset with respect to the joint acceleration. | ||
217 | Matrix6x dFda; | ||
218 | |||
219 | /// \brief Used in computeMinverse | ||
220 | Matrix6x SDinv; | ||
221 | |||
222 | /// \brief Used in computeMinverse | ||
223 | Matrix6x UDinv; | ||
224 | |||
225 | /// \brief Used in computeMinverse | ||
226 | Matrix6x IS; | ||
227 | |||
228 | /// \brief Right variation of the inertia matrix | ||
229 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) vxI; | ||
230 | |||
231 | /// \brief Left variation of the inertia matrix | ||
232 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx; | ||
233 | |||
234 | /// \brief Combined variations of the inertia matrix \f$ B_i = \frac{1}{2} [(v_i\times∗)I_i + | ||
235 | /// (I_i v_i)\times^{∗} − I_i(v_i\times)] \f$ consistent with Christoffel symbols. | ||
236 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) B; | ||
237 | |||
238 | /// \brief Rigid Body Inertia supported by the joint expressed in the world frame | ||
239 | PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias; | ||
240 | |||
241 | /// \brief Composite Rigid Body Inertia expressed in the world frame | ||
242 | PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb; | ||
243 | |||
244 | /// \brief Time variation of Composite Rigid Body Inertia expressed in the world frame | ||
245 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb; | ||
246 | |||
247 | /// \brief Temporary for derivative algorithms | ||
248 | Matrix6 Itmp; | ||
249 | |||
250 | /// \brief Temporary for derivative algorithms | ||
251 | Matrix6 M6tmp; | ||
252 | RowMatrix6 M6tmpR; | ||
253 | RowMatrix6 M6tmpR2; | ||
254 | |||
255 | /// \brief The joint accelerations computed from ABA | ||
256 | TangentVectorType ddq; | ||
257 | |||
258 | // ABA internal data | ||
259 | /// \brief Articulated Body Inertia matrix of the subtree expressed in the LOCAL coordinate | ||
260 | /// frame of the joint | ||
261 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6 | ||
262 | |||
263 | /// \brief Articulated Body Inertia matrix of the subtree expressed in the WORLD coordinate | ||
264 | /// frame | ||
265 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oYaba; // TODO: change with dense symmetric matrix6 | ||
266 | |||
267 | /// \brief Articulated Body Inertia matrix with contact apparent inertia, of a given the subtree | ||
268 | /// and expressed in the WORLD coordinate frame | ||
269 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) | ||
270 | oYaba_contact; // TODO: change with dense symmetric matrix6 | ||
271 | |||
272 | /// \brief Acceleration propagator | ||
273 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oL; // TODO: change with dense symmetric matrix6 | ||
274 | |||
275 | /// \brief Inverse articulated inertia | ||
276 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oK; // TODO: change with dense symmetric matrix6 | ||
277 | |||
278 | /// \brief Intermediate quantity corresponding to apparent torque [ABA] | ||
279 | TangentVectorType u; // Joint Inertia | ||
280 | |||
281 | // CCRBA return quantities | ||
282 | /// \brief Centroidal Momentum Matrix | ||
283 | /// \note \f$ hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum. | ||
284 | Matrix6x Ag; | ||
285 | |||
286 | // dCCRBA return quantities | ||
287 | /// \brief Centroidal Momentum Matrix Time Variation | ||
288 | /// \note \f$ \dot{h_g} = A_g \ddot{q}\ + \dot{A_g}\dot{q}\f$ maps the joint velocity and | ||
289 | /// acceleration vectors to the time variation of the centroidal momentum. | ||
290 | Matrix6x dAg; | ||
291 | |||
292 | /// \brief Centroidal momentum quantity. | ||
293 | /// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with | ||
294 | /// the inertial frame (i.e. the world frame). \note \f$ h_g = \left( m\dot{c}, L_{g} \right); | ||
295 | /// \f$. \f$ h_g \f$ is the stack of the linear momentum and the angular momemtum vectors. | ||
296 | /// | ||
297 | Force hg; | ||
298 | |||
299 | /// \brief Centroidal momentum time derivative. | ||
300 | /// \note The centroidal momentum time derivative is expressed in the frame centered at the CoM | ||
301 | /// and aligned with the inertial frame (i.e. the world frame). \note \f$ \dot{h_g} = \left( | ||
302 | /// m\ddot{c}, \dot{L}_{g} \right); \f$. \f$ \dot{h_g} \f$ is the stack of the linear momentum | ||
303 | /// variation and the angular momemtum variation. | ||
304 | /// | ||
305 | Force dhg; | ||
306 | |||
307 | /// \brief Centroidal Composite Rigid Body Inertia. | ||
308 | /// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroidal momentum | ||
309 | /// quantity. | ||
310 | Inertia Ig; | ||
311 | |||
312 | /// \brief Spatial forces set, used in CRBA and CCRBA | ||
313 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb; | ||
314 | |||
315 | /// \brief Index of the last child (for CRBA) | ||
316 | std::vector<int> lastChild; | ||
317 | |||
318 | /// \brief Dimension of the subtree motion space (for CRBA) | ||
319 | std::vector<int> nvSubtree; | ||
320 | |||
321 | /// \brief Starting index of the Joint motion subspace | ||
322 | std::vector<int> start_idx_v_fromRow; | ||
323 | |||
324 | /// \brief End index of the Joint motion subspace | ||
325 | std::vector<int> end_idx_v_fromRow; | ||
326 | |||
327 | /// \brief Joint space intertia matrix square root (upper trianglular part) computed with a | ||
328 | /// Cholesky Decomposition. | ||
329 | MatrixXs U; | ||
330 | |||
331 | /// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition. | ||
332 | VectorXs D; | ||
333 | |||
334 | /// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky | ||
335 | /// Decomposition. | ||
336 | VectorXs Dinv; | ||
337 | |||
338 | /// \brief Temporary of size NV used in Cholesky Decomposition. | ||
339 | VectorXs tmp; | ||
340 | |||
341 | /// \brief First previous non-zero row in M (used in Cholesky Decomposition). | ||
342 | std::vector<int> parents_fromRow; | ||
343 | |||
344 | /// \brief Each element of this vector corresponds to the ordered list of indexes belonging to | ||
345 | /// the supporting tree of the | ||
346 | /// given index at the row level. It may be helpful to retrieve the sparsity pattern | ||
347 | /// through it. | ||
348 | std::vector<std::vector<int>> supports_fromRow; | ||
349 | |||
350 | /// \brief Subtree of the current row index (used in Cholesky Decomposition). | ||
351 | std::vector<int> nvSubtree_fromRow; | ||
352 | |||
353 | /// \brief Jacobian of joint placements. | ||
354 | /// \note The columns of J corresponds to the basis of the spatial velocities of each joint and | ||
355 | /// expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} | ||
356 | /// \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = | ||
357 | /// \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} | ||
358 | /// S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian | ||
359 | /// of a precise joint, you need to call pinocchio::getJointJacobian | ||
360 | Matrix6x J; | ||
361 | |||
362 | /// \brief Derivative of the Jacobian with respect to the time. | ||
363 | Matrix6x dJ; | ||
364 | |||
365 | /// \brief Second derivative of the Jacobian with respect to the time. | ||
366 | Matrix6x ddJ; | ||
367 | |||
368 | /// \brief psidot Derivative of Jacobian w.r.t to the parent body moving | ||
369 | /// v(p(j)) x Sj | ||
370 | Matrix6x psid; | ||
371 | |||
372 | /// \brief psiddot Second Derivative of Jacobian w.r.t to the parent body | ||
373 | /// moving a(p(j)) x Sj + v(p(j)) x psidj | ||
374 | Matrix6x psidd; | ||
375 | |||
376 | /// \brief Variation of the spatial velocity set with respect to the joint configuration. | ||
377 | Matrix6x dVdq; | ||
378 | |||
379 | /// \brief Variation of the spatial acceleration set with respect to the joint configuration. | ||
380 | Matrix6x dAdq; | ||
381 | |||
382 | /// \brief Variation of the spatial acceleration set with respect to the joint velocity. | ||
383 | Matrix6x dAdv; | ||
384 | |||
385 | /// \brief Partial derivative of the joint torque vector with respect to the joint | ||
386 | /// configuration. | ||
387 | RowMatrixXs dtau_dq; | ||
388 | |||
389 | /// \brief Partial derivative of the joint torque vector with respect to the joint velocity. | ||
390 | RowMatrixXs dtau_dv; | ||
391 | |||
392 | /// \brief Partial derivative of the joint acceleration vector with respect to the joint | ||
393 | /// configuration. | ||
394 | RowMatrixXs ddq_dq; | ||
395 | |||
396 | /// \brief Partial derivative of the joint acceleration vector with respect to the joint | ||
397 | /// velocity. | ||
398 | RowMatrixXs ddq_dv; | ||
399 | |||
400 | /// \brief Partial derivative of the joint acceleration vector with respect to the joint | ||
401 | /// torques. | ||
402 | RowMatrixXs ddq_dtau; | ||
403 | |||
404 | /// \brief Stack of partial derivative of the contact frame acceleration with respect to the | ||
405 | /// joint parameters. | ||
406 | MatrixXs dvc_dq; | ||
407 | MatrixXs dac_dq; | ||
408 | MatrixXs dac_dv; | ||
409 | MatrixXs dac_da; | ||
410 | |||
411 | /// \brief Operational space inertia matrix; | ||
412 | MatrixXs osim; | ||
413 | |||
414 | /// \brief Partial derivatives of the constraints forces with respect to the joint | ||
415 | /// configuration, velocity and torque; | ||
416 | MatrixXs dlambda_dq; | ||
417 | MatrixXs dlambda_dv; | ||
418 | MatrixXs dlambda_dtau; | ||
419 | MatrixXs dlambda_dx_prox, drhs_prox; | ||
420 | |||
421 | /// \brief Vector of joint placements wrt to algorithm end effector. | ||
422 | PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf; | ||
423 | |||
424 | /// \brief Vector of subtree center of mass positions expressed in the root joint of the | ||
425 | /// subtree. In other words, com[j] is the CoM position of the subtree supported by joint \f$ j | ||
426 | /// \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center | ||
427 | /// of mass position of the whole model and expressed in the global frame. | ||
428 | PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com; | ||
429 | |||
430 | /// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the | ||
431 | /// subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by | ||
432 | /// joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds | ||
433 | /// to the velocity of the CoM of the whole model expressed in the global frame. | ||
434 | PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom; | ||
435 | |||
436 | /// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of | ||
437 | /// the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported | ||
438 | /// by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0] | ||
439 | /// corresponds to the acceleration of the CoM of the whole model expressed in the global frame. | ||
440 | PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom; | ||
441 | |||
442 | /// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported | ||
443 | /// by joint \f$ j \f$. The element mass[0] corresponds to the total mass of the model. | ||
444 | std::vector<Scalar> mass; | ||
445 | |||
446 | /// \brief Jacobian of center of mass. | ||
447 | /// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, | ||
448 | /// expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} | ||
449 | /// \dot{q}\f$. | ||
450 | Matrix3x Jcom; | ||
451 | |||
452 | /// \brief Kinetic energy of the system. | ||
453 | Scalar kinetic_energy; | ||
454 | |||
455 | /// \brief Potential energy of the system. | ||
456 | Scalar potential_energy; | ||
457 | |||
458 | /// \brief Mechanical energy of the system. | ||
459 | Scalar mechanical_energy; | ||
460 | |||
461 | // Temporary variables used in forward dynamics | ||
462 | |||
463 | /// \brief Inverse of the operational-space inertia matrix | ||
464 | MatrixXs JMinvJt; | ||
465 | |||
466 | /// \brief Cholesky decompostion of \f$JMinvJt\f$. | ||
467 | Eigen::LLT<MatrixXs> llt_JMinvJt; | ||
468 | |||
469 | /// \brief Lagrange Multipliers corresponding to the contact forces in | ||
470 | /// pinocchio::forwardDynamics. | ||
471 | VectorXs lambda_c; | ||
472 | |||
473 | /// \brief Proximal Lagrange Multipliers used in the computation of the Forward Dynamics | ||
474 | /// computations. | ||
475 | VectorXs lambda_c_prox; | ||
476 | |||
477 | /// \brief Difference between two consecutive iterations of the proxy algorithm. | ||
478 | VectorXs diff_lambda_c; | ||
479 | |||
480 | /// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$. | ||
481 | MatrixXs sDUiJt; | ||
482 | |||
483 | /// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$. | ||
484 | VectorXs torque_residual; | ||
485 | |||
486 | /// \brief Generalized velocity after impact. | ||
487 | TangentVectorType dq_after; | ||
488 | |||
489 | /// \brief Lagrange Multipliers corresponding to the contact impulses in | ||
490 | /// pinocchio::impulseDynamics. | ||
491 | VectorXs impulse_c; | ||
492 | |||
493 | /// \brief Matrix related to static regressor | ||
494 | Matrix3x staticRegressor; | ||
495 | |||
496 | /// \brief Body regressor | ||
497 | BodyRegressorType bodyRegressor; | ||
498 | |||
499 | /// \brief Matrix related to joint torque regressor | ||
500 | MatrixXs jointTorqueRegressor; | ||
501 | |||
502 | /// \brief Matrix related to kinetic energy regressor | ||
503 | RowVectorXs kineticEnergyRegressor; | ||
504 | |||
505 | /// \brief Matrix related to potential energy regressor | ||
506 | RowVectorXs potentialEnergyRegressor; | ||
507 | |||
508 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) KA; | ||
509 | PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) LA; | ||
510 | PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lA; | ||
511 | PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lambdaA; | ||
512 | PINOCCHIO_ALIGNED_STD_VECTOR(int) par_cons_ind; | ||
513 | PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_bias; | ||
514 | PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) KAS; | ||
515 | PINOCCHIO_ALIGNED_STD_VECTOR(int) constraint_ind; | ||
516 | Eigen::LLT<MatrixXs> osim_llt; | ||
517 | |||
518 | #if defined(_MSC_VER) | ||
519 | // Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check | ||
520 | // operator precedence for possible error | ||
521 | #pragma warning(disable : 4554) | ||
522 | #endif | ||
523 | |||
524 | /// \brief Tensor containing the kinematic Hessian of all the joints. | ||
525 | Tensor3x kinematic_hessians; | ||
526 | |||
527 | #if defined(_MSC_VER) | ||
528 | #pragma warning(default : 4554) // C4554 enabled after tensor definition | ||
529 | #endif | ||
530 | |||
531 | /// \brief Cholesky decomposition of the KKT contact matrix | ||
532 | ContactCholeskyDecomposition contact_chol; | ||
533 | |||
534 | /// \brief RHS vector when solving the contact dynamics KKT problem | ||
535 | VectorXs primal_dual_contact_solution; | ||
536 | |||
537 | /// \brief Primal RHS in contact dynamic equations | ||
538 | VectorXs primal_rhs_contact; | ||
539 | |||
540 | #if defined(_MSC_VER) | ||
541 | // Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check | ||
542 | // operator precedence for possible error | ||
543 | #pragma warning(disable : 4554) | ||
544 | #endif | ||
545 | |||
546 | /// \brief SO Partial derivative of the joint torque vector with respect to | ||
547 | /// the joint configuration. | ||
548 | Tensor3x d2tau_dqdq; | ||
549 | |||
550 | /// \brief SO Partial derivative of the joint torque vector with respect to | ||
551 | /// the joint velocity. | ||
552 | Tensor3x d2tau_dvdv; | ||
553 | |||
554 | /// \brief SO Cross-Partial derivative of the joint torque vector with | ||
555 | /// respect to the joint configuration/velocity. | ||
556 | Tensor3x d2tau_dqdv; | ||
557 | |||
558 | /// \brief SO Cross-Partial derivative of the joint torque vector with | ||
559 | /// respect to the joint acceleration/configuration. This also equals to the | ||
560 | /// first-order partial derivative of the Mass Matrix w.r.t joint | ||
561 | /// configuration. | ||
562 | Tensor3x d2tau_dadq; | ||
563 | |||
564 | #if defined(_MSC_VER) | ||
565 | #pragma warning(default : 4554) // C4554 enabled after tensor definition | ||
566 | #endif | ||
567 | |||
568 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) | ||
569 | extended_motion_propagator; // Stores force propagator to the base link | ||
570 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) extended_motion_propagator2; | ||
571 | PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) spatial_inv_inertia; // Stores spatial inverse inertia | ||
572 | PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_descendant; | ||
573 | PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_ancestor; | ||
574 | PINOCCHIO_ALIGNED_STD_VECTOR(int) constraints_supported_dim; | ||
575 | PINOCCHIO_ALIGNED_STD_VECTOR(std::set<size_t>) constraints_supported; | ||
576 | PINOCCHIO_ALIGNED_STD_VECTOR(size_t) joints_supporting_constraints; | ||
577 | PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_joints; | ||
578 | PINOCCHIO_ALIGNED_STD_VECTOR(std::vector<size_t>) constraints_on_joint; | ||
579 | |||
580 | /// | ||
581 | /// \brief Default constructor of pinocchio::Data from a pinocchio::Model. | ||
582 | /// | ||
583 | /// \param[in] model The model structure of the rigid body system. | ||
584 | /// | ||
585 | explicit DataTpl(const Model & model); | ||
586 | |||
587 | /// | ||
588 | /// \brief Default constructor | ||
589 | /// | ||
590 | 7 | DataTpl() | |
591 |
127/254✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 7 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 7 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 7 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 7 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 7 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 7 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 7 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 7 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 7 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 7 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 7 times.
✗ Branch 57 not taken.
✓ Branch 59 taken 7 times.
✗ Branch 60 not taken.
✓ Branch 62 taken 7 times.
✗ Branch 63 not taken.
✓ Branch 65 taken 7 times.
✗ Branch 66 not taken.
✓ Branch 68 taken 7 times.
✗ Branch 69 not taken.
✓ Branch 71 taken 7 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 7 times.
✗ Branch 75 not taken.
✓ Branch 77 taken 7 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 7 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 7 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 7 times.
✗ Branch 87 not taken.
✓ Branch 89 taken 7 times.
✗ Branch 90 not taken.
✓ Branch 92 taken 7 times.
✗ Branch 93 not taken.
✓ Branch 95 taken 7 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 7 times.
✗ Branch 99 not taken.
✓ Branch 101 taken 7 times.
✗ Branch 102 not taken.
✓ Branch 104 taken 7 times.
✗ Branch 105 not taken.
✓ Branch 107 taken 7 times.
✗ Branch 108 not taken.
✓ Branch 110 taken 7 times.
✗ Branch 111 not taken.
✓ Branch 113 taken 7 times.
✗ Branch 114 not taken.
✓ Branch 116 taken 7 times.
✗ Branch 117 not taken.
✓ Branch 119 taken 7 times.
✗ Branch 120 not taken.
✓ Branch 122 taken 7 times.
✗ Branch 123 not taken.
✓ Branch 125 taken 7 times.
✗ Branch 126 not taken.
✓ Branch 128 taken 7 times.
✗ Branch 129 not taken.
✓ Branch 131 taken 7 times.
✗ Branch 132 not taken.
✓ Branch 134 taken 7 times.
✗ Branch 135 not taken.
✓ Branch 137 taken 7 times.
✗ Branch 138 not taken.
✓ Branch 140 taken 7 times.
✗ Branch 141 not taken.
✓ Branch 143 taken 7 times.
✗ Branch 144 not taken.
✓ Branch 146 taken 7 times.
✗ Branch 147 not taken.
✓ Branch 149 taken 7 times.
✗ Branch 150 not taken.
✓ Branch 152 taken 7 times.
✗ Branch 153 not taken.
✓ Branch 155 taken 7 times.
✗ Branch 156 not taken.
✓ Branch 158 taken 7 times.
✗ Branch 159 not taken.
✓ Branch 161 taken 7 times.
✗ Branch 162 not taken.
✓ Branch 168 taken 7 times.
✗ Branch 169 not taken.
✓ Branch 171 taken 7 times.
✗ Branch 172 not taken.
✓ Branch 174 taken 7 times.
✗ Branch 175 not taken.
✓ Branch 177 taken 7 times.
✗ Branch 178 not taken.
✓ Branch 183 taken 7 times.
✗ Branch 184 not taken.
✓ Branch 186 taken 7 times.
✗ Branch 187 not taken.
✓ Branch 189 taken 7 times.
✗ Branch 190 not taken.
✓ Branch 192 taken 7 times.
✗ Branch 193 not taken.
✓ Branch 195 taken 7 times.
✗ Branch 196 not taken.
✓ Branch 198 taken 7 times.
✗ Branch 199 not taken.
✓ Branch 201 taken 7 times.
✗ Branch 202 not taken.
✓ Branch 204 taken 7 times.
✗ Branch 205 not taken.
✓ Branch 207 taken 7 times.
✗ Branch 208 not taken.
✓ Branch 210 taken 7 times.
✗ Branch 211 not taken.
✓ Branch 213 taken 7 times.
✗ Branch 214 not taken.
✓ Branch 216 taken 7 times.
✗ Branch 217 not taken.
✓ Branch 219 taken 7 times.
✗ Branch 220 not taken.
✓ Branch 222 taken 7 times.
✗ Branch 223 not taken.
✓ Branch 225 taken 7 times.
✗ Branch 226 not taken.
✓ Branch 228 taken 7 times.
✗ Branch 229 not taken.
✓ Branch 231 taken 7 times.
✗ Branch 232 not taken.
✓ Branch 234 taken 7 times.
✗ Branch 235 not taken.
✓ Branch 237 taken 7 times.
✗ Branch 238 not taken.
✓ Branch 240 taken 7 times.
✗ Branch 241 not taken.
✓ Branch 243 taken 7 times.
✗ Branch 244 not taken.
✓ Branch 246 taken 7 times.
✗ Branch 247 not taken.
✓ Branch 249 taken 7 times.
✗ Branch 250 not taken.
✓ Branch 252 taken 7 times.
✗ Branch 253 not taken.
✓ Branch 255 taken 7 times.
✗ Branch 256 not taken.
✓ Branch 258 taken 7 times.
✗ Branch 259 not taken.
✓ Branch 261 taken 7 times.
✗ Branch 262 not taken.
✓ Branch 265 taken 7 times.
✗ Branch 266 not taken.
✓ Branch 268 taken 7 times.
✗ Branch 269 not taken.
✓ Branch 271 taken 7 times.
✗ Branch 272 not taken.
✓ Branch 274 taken 7 times.
✗ Branch 275 not taken.
✓ Branch 277 taken 7 times.
✗ Branch 278 not taken.
✓ Branch 280 taken 7 times.
✗ Branch 281 not taken.
✓ Branch 283 taken 7 times.
✗ Branch 284 not taken.
✓ Branch 286 taken 7 times.
✗ Branch 287 not taken.
✓ Branch 289 taken 7 times.
✗ Branch 290 not taken.
✓ Branch 292 taken 7 times.
✗ Branch 293 not taken.
✓ Branch 295 taken 7 times.
✗ Branch 296 not taken.
✓ Branch 298 taken 7 times.
✗ Branch 299 not taken.
✓ Branch 301 taken 7 times.
✗ Branch 302 not taken.
✓ Branch 304 taken 7 times.
✗ Branch 305 not taken.
✓ Branch 307 taken 7 times.
✗ Branch 308 not taken.
✓ Branch 310 taken 7 times.
✗ Branch 311 not taken.
✓ Branch 313 taken 7 times.
✗ Branch 314 not taken.
✓ Branch 316 taken 7 times.
✗ Branch 317 not taken.
✓ Branch 319 taken 7 times.
✗ Branch 320 not taken.
✓ Branch 322 taken 7 times.
✗ Branch 323 not taken.
✓ Branch 325 taken 7 times.
✗ Branch 326 not taken.
✓ Branch 328 taken 7 times.
✗ Branch 329 not taken.
✓ Branch 331 taken 7 times.
✗ Branch 332 not taken.
✓ Branch 334 taken 7 times.
✗ Branch 335 not taken.
✓ Branch 337 taken 7 times.
✗ Branch 338 not taken.
✓ Branch 340 taken 7 times.
✗ Branch 341 not taken.
✓ Branch 343 taken 7 times.
✗ Branch 344 not taken.
✓ Branch 346 taken 7 times.
✗ Branch 347 not taken.
✓ Branch 349 taken 7 times.
✗ Branch 350 not taken.
✓ Branch 352 taken 7 times.
✗ Branch 353 not taken.
✓ Branch 355 taken 7 times.
✗ Branch 356 not taken.
✓ Branch 358 taken 7 times.
✗ Branch 359 not taken.
✓ Branch 361 taken 7 times.
✗ Branch 362 not taken.
✓ Branch 364 taken 7 times.
✗ Branch 365 not taken.
✓ Branch 367 taken 7 times.
✗ Branch 368 not taken.
✓ Branch 370 taken 7 times.
✗ Branch 371 not taken.
✓ Branch 373 taken 7 times.
✗ Branch 374 not taken.
✓ Branch 376 taken 7 times.
✗ Branch 377 not taken.
✓ Branch 379 taken 7 times.
✗ Branch 380 not taken.
✓ Branch 382 taken 7 times.
✗ Branch 383 not taken.
✓ Branch 385 taken 7 times.
✗ Branch 386 not taken.
✓ Branch 388 taken 7 times.
✗ Branch 389 not taken.
|
7 | { |
592 | 7 | } | |
593 | |||
594 | private: | ||
595 | void computeLastChild(const Model & model); | ||
596 | void computeParents_fromRow(const Model & model); | ||
597 | void computeSupports_fromRow(const Model & model); | ||
598 | }; | ||
599 | |||
600 | } // namespace pinocchio | ||
601 | |||
602 | /* --- Details -------------------------------------------------------------- */ | ||
603 | /* --- Details -------------------------------------------------------------- */ | ||
604 | /* --- Details -------------------------------------------------------------- */ | ||
605 | #include "pinocchio/multibody/data.hxx" | ||
606 | |||
607 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
608 | #include "pinocchio/multibody/data.txx" | ||
609 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
610 | |||
611 | #endif // ifndef __pinocchio_multibody_data_hpp__ | ||
612 |