 |
pinocchio
2.6.20
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
|
|
6 #ifndef __pinocchio_multibody_data_hpp__
7 #define __pinocchio_multibody_data_hpp__
9 #include "pinocchio/math/tensor.hpp"
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"
20 #include "pinocchio/serialization/serializable.hpp"
23 #include <Eigen/Cholesky>
28 template<
typename _Scalar,
int _Options,
template<
typename,
int>
class JointCollectionTpl>
32 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 typedef _Scalar Scalar;
35 enum { Options = _Options };
37 typedef JointCollectionTpl<Scalar,Options> JointCollection;
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;
56 typedef PINOCCHIO_ALIGNED_STD_VECTOR(
JointModel) JointModelVector;
57 typedef PINOCCHIO_ALIGNED_STD_VECTOR(
JointData) JointDataVector;
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;
73 typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options>
Matrix6x;
75 typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options>
Matrix3x;
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;
92 PINOCCHIO_ALIGNED_STD_VECTOR(
Motion) a;
95 PINOCCHIO_ALIGNED_STD_VECTOR(
Motion) oa;
98 PINOCCHIO_ALIGNED_STD_VECTOR(
Motion) a_gf;
101 PINOCCHIO_ALIGNED_STD_VECTOR(
Motion) oa_gf;
104 PINOCCHIO_ALIGNED_STD_VECTOR(
Motion) v;
107 PINOCCHIO_ALIGNED_STD_VECTOR(
Motion) ov;
111 PINOCCHIO_ALIGNED_STD_VECTOR(
Force) f;
115 PINOCCHIO_ALIGNED_STD_VECTOR(
Force) of;
118 PINOCCHIO_ALIGNED_STD_VECTOR(
Force) h;
121 PINOCCHIO_ALIGNED_STD_VECTOR(
Force) oh;
124 PINOCCHIO_ALIGNED_STD_VECTOR(
SE3) oMi;
127 PINOCCHIO_ALIGNED_STD_VECTOR(
SE3) liMi;
143 PINOCCHIO_ALIGNED_STD_VECTOR(
SE3) oMf;
147 PINOCCHIO_ALIGNED_STD_VECTOR(
Inertia) Ycrb;
150 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb;
183 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) vxI;
186 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx;
189 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) B;
192 PINOCCHIO_ALIGNED_STD_VECTOR(
Inertia) oinertias;
195 PINOCCHIO_ALIGNED_STD_VECTOR(
Inertia) oYcrb;
198 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
213 PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba;
245 PINOCCHIO_ALIGNED_STD_VECTOR(
Matrix6x) Fcrb;
321 PINOCCHIO_ALIGNED_STD_VECTOR(
SE3) iMf;
324 PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com;
327 PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
330 PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
411 void computeLastChild(
const Model & model);
412 void computeParents_fromRow(
const Model & model);
413 void computeSupports_fromRow(
const Model & model);
422 #include "pinocchio/multibody/data.hxx"
424 #endif // ifndef __pinocchio_multibody_data_hpp__
DataTpl()
Default constructor.
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
VectorXs tmp
Temporary of size NV used in Cholesky Decomposition.
BodyRegressorType bodyRegressor
Body regressor.
Matrix6x Ag
Centroidal Momentum Matrix.
std::vector< std::vector< int > > supports_fromRow
Each element of this vector corresponds to the ordered list of indexes belonging to the supporting tr...
Tensor< Scalar, 3, Options > Tensor3x
 
Matrix3x Jcom
Jacobian of center of mass.
Matrix6x dAg
Centroidal Momentum Matrix Time Variation.
Tensor3x d2tau_dadq
SO Cross-Partial derivative of the joint torque vector with respect to the joint acceleration/configu...
TangentVectorType tau
Vector of joint torques (dim model.nv).
MatrixXs jointTorqueRegressor
Matrix related to joint torque regressor.
TangentVectorType u
Intermediate quantity corresponding to apparent torque [ABA].
VectorXs torque_residual
Temporary corresponding to the residual torque .
Matrix6x dJ
Derivative of the Jacobian with respect to the time.
Matrix6x ddJ
Second derivative of the Jacobian with respect to the time.
Matrix6x dAdv
Variation of the spatial acceleration set with respect to the joint velocity.
std::vector< int > nvSubtree_fromRow
Subtree of the current row index (used in Cholesky Decomposition).
Matrix6x SDinv
Used in computeMinverse.
TangentVectorType dq_after
Generalized velocity after impact.
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
MatrixXs C
The Coriolis matrix (a square matrix of dim model.nv).
Tensor3x kinematic_hessians
Tensor containing the kinematic Hessian of all the joints.
Eigen::Matrix< Scalar, 6, 10, Options > BodyRegressorType
The type of the body regressor.
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Inertia Ig
Centroidal Composite Rigid Body Inertia.
Matrix3x staticRegressor
Matrix related to static regressor.
VectorXs nle
Vector of Non Linear Effects (dim model.nv). It corresponds to concatenation of the Coriolis,...
A Plucker coordinate frame attached to a parent joint inside a kinematic tree.
Matrix6 M6tmp
Temporary for derivative algorithms.
TangentVectorType ddq
The joint accelerations computed from ABA.
Scalar potential_energy
Potential energy of the model.
Scalar kinetic_energy
Kinetic energy of the model.
VectorXs g
Vector of generalized gravity (dim model.nv).
VectorXs D
Diagonal of the joint space intertia matrix obtained by a Cholesky Decomposition.
Tensor3x d2tau_dqdq
SO Partial derivative of the joint torque vector with respect to the joint configuration.
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
MatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Matrix6 Itmp
Temporary for derivative algorithms.
std::vector< int > lastChild
Index of the last child (for CRBA)
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
std::vector< int > end_idx_v_fromRow
End index of the Joint motion subspace.
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model,...
Matrix6x IS
Used in computeMinverse.
MatrixXs sDUiJt
Temporary corresponding to .
Matrix6x dFda
Variation of the forceset with respect to the joint acceleration.
MatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity.
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
MatrixXs JMinvJt
Inverse of the operational-space inertia matrix.
Matrix6x psid
psidot Derivative of Jacobian w.r.t to the parent body moving v(p(j)) x Sj
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
MatrixXs U
Joint space intertia matrix square root (upper trianglular part) computed with a Cholesky Decompositi...
VectorXs Dinv
Diagonal inverse of the joint space intertia matrix obtained by a Cholesky Decomposition.
Matrix6x dFdq
Variation of the forceset with respect to the joint configuration.
Matrix6x UDinv
Used in computeMinverse.
VectorXs impulse_c
Lagrange Multipliers corresponding to the contact impulses in pinocchio::impulseDynamics.
Tensor3x d2tau_dvdv
SO Partial derivative of the joint torque vector with respect to the joint velocity.
std::vector< Scalar > mass
Vector of subtree mass. In other words, mass[j] is the mass of the subtree supported by joint ....
std::vector< int > parents_fromRow
First previous non-zero row in M (used in Cholesky Decomposition).
Matrix6x J
Jacobian of joint placements.
Matrix6x dFdv
Variation of the forceset with respect to the joint velocity.
Tensor3x d2tau_dqdv
SO Cross-Partial derivative of the joint torque vector with respect to the joint configuration/veloci...
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Matrix6x dHdq
Variation of the spatial momenta with respect to the joint configuration.
std::vector< int > nvSubtree
Dimension of the subtree motion space (for CRBA)
std::vector< int > start_idx_v_fromRow
Starting index of the Joint motion subspace.
Matrix6x dAdq
Variation of the spatial acceleration set with respect to the joint configuration.
Matrix6x dVdq
Variation of the spatial velocity set with respect to the joint configuration.
Force dhg
Centroidal momentum time derivative.
Force hg
Centroidal momentum quantity.
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Eigen::LLT< MatrixXs > llt_JMinvJt
Cholesky decompostion of .
Main pinocchio namespace.