GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
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 |
|||
39 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
40 |
|||
41 |
typedef SE3Tpl<Scalar,Options> SE3; |
||
42 |
typedef MotionTpl<Scalar,Options> Motion; |
||
43 |
typedef ForceTpl<Scalar,Options> Force; |
||
44 |
typedef InertiaTpl<Scalar,Options> Inertia; |
||
45 |
typedef FrameTpl<Scalar,Options> Frame; |
||
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 |
|||
53 |
typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModel; |
||
54 |
typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointData; |
||
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 |
|||
65 |
/// \brief Dense vectorized version of a joint configuration vector. |
||
66 |
typedef VectorXs ConfigVectorType; |
||
67 |
|||
68 |
/// \brief Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc). |
||
69 |
/// It also handles the notion of co-tangent vector (e.g. torque, etc). |
||
70 |
typedef VectorXs TangentVectorType; |
||
71 |
|||
72 |
/// \brief The 6d jacobian type (temporary) |
||
73 |
typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> Matrix6x; |
||
74 |
/// \brief The 3d jacobian type (temporary) |
||
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 |
|||
81 |
/// \brief The type of the body regressor |
||
82 |
typedef Eigen::Matrix<Scalar,6,10,Options> BodyRegressorType; |
||
83 |
|||
84 |
/// \brief The type of Tensor for Kinematics and Dynamics second order derivatives |
||
85 |
typedef Tensor<Scalar,3,Options> Tensor3x; |
||
86 |
|||
87 |
/// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, |
||
88 |
/// encapsulated in JointDataAccessor. |
||
89 |
JointDataVector joints; |
||
90 |
|||
91 |
/// \brief Vector of joint accelerations expressed at the centers of the joints frames. |
||
92 |
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a; |
||
93 |
|||
94 |
/// \brief Vector of joint accelerations expressed at the origin of the world. |
||
95 |
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa; |
||
96 |
|||
97 |
/// \brief Vector of joint accelerations due to the gravity field. |
||
98 |
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_gf; |
||
99 |
|||
100 |
/// \brief Vector of joint accelerations expressed at the origin of the world including gravity contribution. |
||
101 |
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_gf; |
||
102 |
|||
103 |
/// \brief Vector of joint velocities expressed at the centers of the joints. |
||
104 |
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v; |
||
105 |
|||
106 |
/// \brief Vector of joint velocities expressed at the origin. |
||
107 |
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov; |
||
108 |
|||
109 |
/// \brief Vector of body forces expressed in the local frame of the joint. For each body, the force represents the sum of |
||
110 |
/// all external forces acting on the body. |
||
111 |
PINOCCHIO_ALIGNED_STD_VECTOR(Force) f; |
||
112 |
|||
113 |
/// \brief Vector of body forces expressed in the world frame. For each body, the force represents the sum of |
||
114 |
/// all external forces acting on the body. |
||
115 |
PINOCCHIO_ALIGNED_STD_VECTOR(Force) of; |
||
116 |
|||
117 |
/// \brief Vector of spatial momenta expressed in the local frame of the joint. |
||
118 |
PINOCCHIO_ALIGNED_STD_VECTOR(Force) h; |
||
119 |
|||
120 |
/// \brief Vector of spatial momenta expressed in the world frame. |
||
121 |
PINOCCHIO_ALIGNED_STD_VECTOR(Force) oh; |
||
122 |
|||
123 |
/// \brief Vector of absolute joint placements (wrt the world). |
||
124 |
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMi; |
||
125 |
|||
126 |
/// \brief Vector of relative joint placements (wrt the body parent). |
||
127 |
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) liMi; |
||
128 |
|||
129 |
/// \brief Vector of joint torques (dim model.nv). |
||
130 |
TangentVectorType tau; |
||
131 |
|||
132 |
/// \brief Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis, centrifugal and gravitational effects. |
||
133 |
/// \note In the multibody dynamics equation \f$ M\ddot{q} + b(q, \dot{q}) = \tau \f$, |
||
134 |
/// the non linear effects are associated to the term \f$b\f$. |
||
135 |
VectorXs nle; |
||
136 |
|||
137 |
/// \brief Vector of generalized gravity (dim model.nv). |
||
138 |
/// \note In the multibody dynamics equation \f$ M\ddot{q} + c(q, \dot{q}) + g(q) = \tau \f$, |
||
139 |
/// the gravity effect is associated to the \f$g\f$ term. |
||
140 |
VectorXs g; |
||
141 |
|||
142 |
/// \brief Vector of absolute operationnel frame placements (wrt the world). |
||
143 |
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMf; |
||
144 |
|||
145 |
/// \brief Vector of sub-tree composite rigid body inertias, i.e. the apparent inertia of the subtree supported by the joint |
||
146 |
/// and expressed in the local frame of the joint.. |
||
147 |
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) Ycrb; |
||
148 |
|||
149 |
/// \brief Vector of sub-tree composite rigid body inertia time derivatives \f$ \dot{Y}_{crb}\f$. See Data::Ycrb for more details. |
||
150 |
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb; // TODO: change with dense symmetric matrix6 |
||
151 |
|||
152 |
/// \brief The joint space inertia matrix (a square matrix of dim model.nv). |
||
153 |
MatrixXs M; |
||
154 |
|||
155 |
/// \brief The inverse of the joint space inertia matrix (a square matrix of dim model.nv). |
||
156 |
RowMatrixXs Minv; |
||
157 |
|||
158 |
/// \brief The Coriolis matrix (a square matrix of dim model.nv). |
||
159 |
MatrixXs C; |
||
160 |
|||
161 |
/// \brief Variation of the spatial momenta with respect to the joint configuration. |
||
162 |
Matrix6x dHdq; |
||
163 |
|||
164 |
/// \brief Variation of the forceset with respect to the joint configuration. |
||
165 |
Matrix6x dFdq; |
||
166 |
|||
167 |
/// \brief Variation of the forceset with respect to the joint velocity. |
||
168 |
Matrix6x dFdv; |
||
169 |
|||
170 |
/// \brief Variation of the forceset with respect to the joint acceleration. |
||
171 |
Matrix6x dFda; |
||
172 |
|||
173 |
/// \brief Used in computeMinverse |
||
174 |
Matrix6x SDinv; |
||
175 |
|||
176 |
/// \brief Used in computeMinverse |
||
177 |
Matrix6x UDinv; |
||
178 |
|||
179 |
/// \brief Used in computeMinverse |
||
180 |
Matrix6x IS; |
||
181 |
|||
182 |
/// \brief Right variation of the inertia matrix |
||
183 |
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) vxI; |
||
184 |
|||
185 |
/// \brief Left variation of the inertia matrix |
||
186 |
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx; |
||
187 |
|||
188 |
/// \brief Combined variations of the inertia matrix \f$ B_i = \frac{1}{2} [(v_i\times∗)I_i + (I_i v_i)\times^{∗} − I_i(v_i\times)] \f$ consistent with Christoffel symbols. |
||
189 |
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) B; |
||
190 |
|||
191 |
/// \brief Rigid Body Inertia supported by the joint expressed in the world frame |
||
192 |
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias; |
||
193 |
|||
194 |
/// \brief Composite Rigid Body Inertia expressed in the world frame |
||
195 |
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb; |
||
196 |
|||
197 |
/// \brief Time variation of Composite Rigid Body Inertia expressed in the world frame |
||
198 |
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb; |
||
199 |
|||
200 |
/// \brief Temporary for derivative algorithms |
||
201 |
Matrix6 Itmp; |
||
202 |
|||
203 |
/// \brief Temporary for derivative algorithms |
||
204 |
Matrix6 M6tmp; |
||
205 |
RowMatrix6 M6tmpR; |
||
206 |
RowMatrix6 M6tmpR2; |
||
207 |
|||
208 |
/// \brief The joint accelerations computed from ABA |
||
209 |
TangentVectorType ddq; |
||
210 |
|||
211 |
// ABA internal data |
||
212 |
/// \brief Inertia matrix of the subtree expressed as dense matrix [ABA] |
||
213 |
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6 |
||
214 |
|||
215 |
/// \brief Intermediate quantity corresponding to apparent torque [ABA] |
||
216 |
TangentVectorType u; // Joint Inertia |
||
217 |
|||
218 |
// CCRBA return quantities |
||
219 |
/// \brief Centroidal Momentum Matrix |
||
220 |
/// \note \f$ hg = A_g \dot{q}\f$ maps the joint velocity set to the centroidal momentum. |
||
221 |
Matrix6x Ag; |
||
222 |
|||
223 |
// dCCRBA return quantities |
||
224 |
/// \brief Centroidal Momentum Matrix Time Variation |
||
225 |
/// \note \f$ \dot{h_g} = A_g \ddot{q}\ + \dot{A_g}\dot{q}\f$ maps the joint velocity and acceleration vectors to the time variation of the centroidal momentum. |
||
226 |
Matrix6x dAg; |
||
227 |
|||
228 |
/// \brief Centroidal momentum quantity. |
||
229 |
/// \note The centroidal momentum is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame). |
||
230 |
/// \note \f$ h_g = \left( m\dot{c}, L_{g} \right); \f$. \f$ h_g \f$ is the stack of the linear momentum and the angular momemtum vectors. |
||
231 |
/// |
||
232 |
Force hg; |
||
233 |
|||
234 |
/// \brief Centroidal momentum time derivative. |
||
235 |
/// \note The centroidal momentum time derivative is expressed in the frame centered at the CoM and aligned with the inertial frame (i.e. the world frame). |
||
236 |
/// \note \f$ \dot{h_g} = \left( m\ddot{c}, \dot{L}_{g} \right); \f$. \f$ \dot{h_g} \f$ is the stack of the linear momentum variation and the angular momemtum variation. |
||
237 |
/// |
||
238 |
Force dhg; |
||
239 |
|||
240 |
/// \brief Centroidal Composite Rigid Body Inertia. |
||
241 |
/// \note \f$ hg = Ig v_{\text{mean}}\f$ map a mean velocity to the current centroidal momentum quantity. |
||
242 |
Inertia Ig; |
||
243 |
|||
244 |
/// \brief Spatial forces set, used in CRBA and CCRBA |
||
245 |
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb; |
||
246 |
|||
247 |
/// \brief Index of the last child (for CRBA) |
||
248 |
std::vector<int> lastChild; |
||
249 |
|||
250 |
/// \brief Dimension of the subtree motion space (for CRBA) |
||
251 |
std::vector<int> nvSubtree; |
||
252 |
|||
253 |
/// \brief Starting index of the Joint motion subspace |
||
254 |
std::vector<int> start_idx_v_fromRow; |
||
255 |
|||
256 |
/// \brief End index of the Joint motion subspace |
||
257 |
std::vector<int> end_idx_v_fromRow; |
||
258 |
|||
259 |
/// \brief Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decomposition. |
||
260 |
MatrixXs U; |
||
261 |
|||
262 |
/// \brief Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition. |
||
263 |
VectorXs D; |
||
264 |
|||
265 |
/// \brief Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition. |
||
266 |
VectorXs Dinv; |
||
267 |
|||
268 |
/// \brief Temporary of size NV used in Cholesky Decomposition. |
||
269 |
VectorXs tmp; |
||
270 |
|||
271 |
/// \brief First previous non-zero row in M (used in Cholesky Decomposition). |
||
272 |
std::vector<int> parents_fromRow; |
||
273 |
|||
274 |
/// \brief Each element of this vector corresponds to the ordered list of indexes belonging to the supporting tree of the |
||
275 |
/// given index at the row level. It may be helpful to retrieve the sparsity pattern through it. |
||
276 |
std::vector< std::vector<int> > supports_fromRow; |
||
277 |
|||
278 |
/// \brief Subtree of the current row index (used in Cholesky Decomposition). |
||
279 |
std::vector<int> nvSubtree_fromRow; |
||
280 |
|||
281 |
/// \brief Jacobian of joint placements. |
||
282 |
/// \note The columns of J corresponds to the basis of the spatial velocities of each joint and expressed at the origin of the inertial frame. In other words, if \f$ v_{J_{i}} = S_{i} \dot{q}_{i}\f$ is the relative velocity of the joint i regarding to its parent, then \f$J = \begin{bmatrix} ^{0}X_{1} S_{1} & \cdots & ^{0}X_{i} S_{i} & \cdots & ^{0}X_{\text{nj}} S_{\text{nj}} \end{bmatrix} \f$. This Jacobian has no special meaning. To get the jacobian of a precise joint, you need to call pinocchio::getJointJacobian |
||
283 |
Matrix6x J; |
||
284 |
|||
285 |
/// \brief Derivative of the Jacobian with respect to the time. |
||
286 |
Matrix6x dJ; |
||
287 |
|||
288 |
/// \brief Second derivative of the Jacobian with respect to the time. |
||
289 |
Matrix6x ddJ; |
||
290 |
|||
291 |
/// \brief psidot Derivative of Jacobian w.r.t to the parent body moving |
||
292 |
/// v(p(j)) x Sj |
||
293 |
Matrix6x psid; |
||
294 |
|||
295 |
/// \brief psiddot Second Derivative of Jacobian w.r.t to the parent body |
||
296 |
/// moving a(p(j)) x Sj + v(p(j)) x psidj |
||
297 |
Matrix6x psidd; |
||
298 |
|||
299 |
/// \brief Variation of the spatial velocity set with respect to the joint configuration. |
||
300 |
Matrix6x dVdq; |
||
301 |
|||
302 |
/// \brief Variation of the spatial acceleration set with respect to the joint configuration. |
||
303 |
Matrix6x dAdq; |
||
304 |
|||
305 |
/// \brief Variation of the spatial acceleration set with respect to the joint velocity. |
||
306 |
Matrix6x dAdv; |
||
307 |
|||
308 |
/// \brief Partial derivative of the joint torque vector with respect to the joint configuration. |
||
309 |
MatrixXs dtau_dq; |
||
310 |
|||
311 |
/// \brief Partial derivative of the joint torque vector with respect to the joint velocity. |
||
312 |
MatrixXs dtau_dv; |
||
313 |
|||
314 |
/// \brief Partial derivative of the joint acceleration vector with respect to the joint configuration. |
||
315 |
MatrixXs ddq_dq; |
||
316 |
|||
317 |
/// \brief Partial derivative of the joint acceleration vector with respect to the joint velocity. |
||
318 |
MatrixXs ddq_dv; |
||
319 |
|||
320 |
/// \brief Vector of joint placements wrt to algorithm end effector. |
||
321 |
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf; |
||
322 |
|||
323 |
/// \brief 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 \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element com[0] corresponds to the center of mass position of the whole model and expressed in the global frame. |
||
324 |
PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com; |
||
325 |
|||
326 |
/// \brief Vector of subtree center of mass linear velocities expressed in the root joint of the subtree. In other words, vcom[j] is the CoM linear velocity of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element vcom[0] corresponds to the velocity of the CoM of the whole model expressed in the global frame. |
||
327 |
PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom; |
||
328 |
|||
329 |
/// \brief Vector of subtree center of mass linear accelerations expressed in the root joint of the subtree. In other words, acom[j] is the CoM linear acceleration of the subtree supported by joint \f$ j \f$ and expressed in the joint frame \f$ j \f$. The element acom[0] corresponds to the acceleration of the CoM of the whole model expressed in the global frame. |
||
330 |
PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom; |
||
331 |
|||
332 |
/// \brief Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint \f$ j \f$. The element mass[0] corresponds to the total mass of the model. |
||
333 |
std::vector<Scalar> mass; |
||
334 |
|||
335 |
/// \brief Jacobian of center of mass. |
||
336 |
/// \note This Jacobian maps the joint velocity vector to the velocity of the center of mass, expressed in the inertial frame. In other words, \f$ v_{\text{CoM}} = J_{\text{CoM}} \dot{q}\f$. |
||
337 |
Matrix3x Jcom; |
||
338 |
|||
339 |
/// \brief Kinetic energy of the model. |
||
340 |
Scalar kinetic_energy; |
||
341 |
|||
342 |
/// \brief Potential energy of the model. |
||
343 |
Scalar potential_energy; |
||
344 |
|||
345 |
// Temporary variables used in forward dynamics |
||
346 |
|||
347 |
/// \brief Inverse of the operational-space inertia matrix |
||
348 |
MatrixXs JMinvJt; |
||
349 |
|||
350 |
/// \brief Cholesky decompostion of \f$JMinvJt\f$. |
||
351 |
Eigen::LLT<MatrixXs> llt_JMinvJt; |
||
352 |
|||
353 |
/// \brief Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics. |
||
354 |
VectorXs lambda_c; |
||
355 |
|||
356 |
/// \brief Temporary corresponding to \f$ \sqrt{D} U^{-1} J^{\top} \f$. |
||
357 |
MatrixXs sDUiJt; |
||
358 |
|||
359 |
/// \brief Temporary corresponding to the residual torque \f$ \tau - b(q,\dot{q}) \f$. |
||
360 |
VectorXs torque_residual; |
||
361 |
|||
362 |
/// \brief Generalized velocity after impact. |
||
363 |
TangentVectorType dq_after; |
||
364 |
|||
365 |
/// \brief Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics. |
||
366 |
VectorXs impulse_c; |
||
367 |
|||
368 |
/// \brief Matrix related to static regressor |
||
369 |
Matrix3x staticRegressor; |
||
370 |
|||
371 |
/// \brief Body regressor |
||
372 |
BodyRegressorType bodyRegressor; |
||
373 |
|||
374 |
/// \brief Matrix related to joint torque regressor |
||
375 |
MatrixXs jointTorqueRegressor; |
||
376 |
|||
377 |
/// \brief Tensor containing the kinematic Hessian of all the joints. |
||
378 |
Tensor3x kinematic_hessians; |
||
379 |
|||
380 |
/// \brief SO Partial derivative of the joint torque vector with respect to |
||
381 |
/// the joint configuration. |
||
382 |
Tensor3x d2tau_dqdq; |
||
383 |
|||
384 |
/// \brief SO Partial derivative of the joint torque vector with respect to |
||
385 |
/// the joint velocity. |
||
386 |
Tensor3x d2tau_dvdv; |
||
387 |
|||
388 |
/// \brief SO Cross-Partial derivative of the joint torque vector with |
||
389 |
/// respect to the joint configuration/velocity. |
||
390 |
Tensor3x d2tau_dqdv; |
||
391 |
|||
392 |
/// \brief SO Cross-Partial derivative of the joint torque vector with |
||
393 |
/// respect to the joint acceleration/configuration. This also equals to the |
||
394 |
/// first-order partial derivative of the Mass Matrix w.r.t joint |
||
395 |
/// configuration. |
||
396 |
Tensor3x d2tau_dadq; |
||
397 |
|||
398 |
/// |
||
399 |
/// \brief Default constructor of pinocchio::Data from a pinocchio::Model. |
||
400 |
/// |
||
401 |
/// \param[in] model The model structure of the rigid body system. |
||
402 |
/// |
||
403 |
explicit DataTpl(const Model & model); |
||
404 |
|||
405 |
/// |
||
406 |
/// \brief Default constructor |
||
407 |
/// |
||
408 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
7 |
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 |
Generated by: GCOVR (Version 4.2) |