Program Listing for File data.hpp
↰ Return to documentation for file (include/pinocchio/multibody/data.hpp
)
//
// Copyright (c) 2015-2024 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_multibody_data_hpp__
#define __pinocchio_multibody_data_hpp__
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/math/tensor.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/force.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/joint/joint-generic.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/algorithm/contact-cholesky.hpp"
#include "pinocchio/serialization/serializable.hpp"
#include <Eigen/Cholesky>
#include <Eigen/StdVector>
#include <Eigen/src/Core/util/Constants.h>
#include <cstddef>
#include <set>
namespace pinocchio
{
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct traits<DataTpl<_Scalar, _Options, JointCollectionTpl>>
{
typedef _Scalar Scalar;
};
template<typename _Scalar, int _Options, template<typename, int> class JointCollectionTpl>
struct DataTpl
: serialization::Serializable<DataTpl<_Scalar, _Options, JointCollectionTpl>>
, NumericalBase<DataTpl<_Scalar, _Options, JointCollectionTpl>>
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef _Scalar Scalar;
enum
{
Options = _Options
};
typedef JointCollectionTpl<Scalar, Options> JointCollection;
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
typedef SE3Tpl<Scalar, Options> SE3;
typedef MotionTpl<Scalar, Options> Motion;
typedef ForceTpl<Scalar, Options> Force;
typedef InertiaTpl<Scalar, Options> Inertia;
typedef FrameTpl<Scalar, Options> Frame;
typedef pinocchio::Index Index;
typedef pinocchio::JointIndex JointIndex;
typedef pinocchio::GeomIndex GeomIndex;
typedef pinocchio::FrameIndex FrameIndex;
typedef std::vector<Index> IndexVector;
typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel;
typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointData;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
typedef Eigen::Matrix<Scalar, 1, Eigen::Dynamic, Options | Eigen::RowMajor> RowVectorXs;
typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6c;
typedef Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor | Options> Vector6r;
typedef VectorXs ConfigVectorType;
typedef VectorXs TangentVectorType;
typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic, Options> Matrix3x;
typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
typedef Eigen::Matrix<Scalar, 6, 6, Eigen::RowMajor | Options> RowMatrix6;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Options>
RowMatrixXs;
typedef Eigen::Matrix<Scalar, 6, 10, Options> BodyRegressorType;
typedef Tensor<Scalar, 3, Options> Tensor3x;
// TODO Remove when API is stabilized
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
typedef ContactCholeskyDecompositionTpl<Scalar, Options> ContactCholeskyDecomposition;
PINOCCHIO_COMPILER_DIAGNOSTIC_POP
JointDataVector joints;
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a;
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa;
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_drift;
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_augmented;
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_gf;
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa_gf;
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) v;
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) ov;
PINOCCHIO_ALIGNED_STD_VECTOR(Force) f;
PINOCCHIO_ALIGNED_STD_VECTOR(Force) of;
PINOCCHIO_ALIGNED_STD_VECTOR(Force) of_augmented;
PINOCCHIO_ALIGNED_STD_VECTOR(Force) h;
PINOCCHIO_ALIGNED_STD_VECTOR(Force) oh;
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMi;
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) liMi;
TangentVectorType tau;
VectorXs nle;
VectorXs g;
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) oMf;
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) Ycrb;
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) dYcrb; // TODO: change with dense symmetric matrix6
MatrixXs M;
RowMatrixXs Minv;
MatrixXs C;
Matrix6x dHdq;
Matrix6x dFdq;
Matrix6x dFdv;
Matrix6x dFda;
Matrix6x SDinv;
Matrix6x UDinv;
Matrix6x IS;
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) vxI;
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Ivx;
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) B;
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oinertias;
PINOCCHIO_ALIGNED_STD_VECTOR(Inertia) oYcrb;
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) doYcrb;
Matrix6 Itmp;
Matrix6 M6tmp;
RowMatrix6 M6tmpR;
RowMatrix6 M6tmpR2;
TangentVectorType ddq;
// ABA internal data
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) Yaba; // TODO: change with dense symmetric matrix6
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oYaba; // TODO: change with dense symmetric matrix6
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6)
oYaba_contact; // TODO: change with dense symmetric matrix6
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oL; // TODO: change with dense symmetric matrix6
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) oK; // TODO: change with dense symmetric matrix6
TangentVectorType u; // Joint Inertia
// CCRBA return quantities
Matrix6x Ag;
// dCCRBA return quantities
Matrix6x dAg;
Force hg;
Force dhg;
Inertia Ig;
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) Fcrb;
std::vector<int> lastChild;
std::vector<int> nvSubtree;
std::vector<int> start_idx_v_fromRow;
std::vector<int> end_idx_v_fromRow;
MatrixXs U;
VectorXs D;
VectorXs Dinv;
VectorXs tmp;
std::vector<int> parents_fromRow;
std::vector<std::vector<int>> supports_fromRow;
std::vector<int> nvSubtree_fromRow;
Matrix6x J;
Matrix6x dJ;
Matrix6x ddJ;
Matrix6x psid;
Matrix6x psidd;
Matrix6x dVdq;
Matrix6x dAdq;
Matrix6x dAdv;
RowMatrixXs dtau_dq;
RowMatrixXs dtau_dv;
RowMatrixXs ddq_dq;
RowMatrixXs ddq_dv;
RowMatrixXs ddq_dtau;
MatrixXs dvc_dq;
MatrixXs dac_dq;
MatrixXs dac_dv;
MatrixXs dac_da;
MatrixXs osim;
MatrixXs dlambda_dq;
MatrixXs dlambda_dv;
MatrixXs dlambda_dtau;
MatrixXs dlambda_dx_prox, drhs_prox;
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) iMf;
PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) com;
PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) vcom;
PINOCCHIO_ALIGNED_STD_VECTOR(Vector3) acom;
std::vector<Scalar> mass;
Matrix3x Jcom;
Scalar kinetic_energy;
Scalar potential_energy;
Scalar mechanical_energy;
// Temporary variables used in forward dynamics
MatrixXs JMinvJt;
Eigen::LLT<MatrixXs> llt_JMinvJt;
VectorXs lambda_c;
VectorXs lambda_c_prox;
VectorXs diff_lambda_c;
MatrixXs sDUiJt;
VectorXs torque_residual;
TangentVectorType dq_after;
VectorXs impulse_c;
Matrix3x staticRegressor;
BodyRegressorType bodyRegressor;
MatrixXs jointTorqueRegressor;
RowVectorXs kineticEnergyRegressor;
RowVectorXs potentialEnergyRegressor;
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6x) KA;
PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) LA;
PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lA;
PINOCCHIO_ALIGNED_STD_VECTOR(VectorXs) lambdaA;
PINOCCHIO_ALIGNED_STD_VECTOR(int) par_cons_ind;
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a_bias;
PINOCCHIO_ALIGNED_STD_VECTOR(MatrixXs) KAS;
PINOCCHIO_ALIGNED_STD_VECTOR(int) constraint_ind;
Eigen::LLT<MatrixXs> osim_llt;
#if defined(_MSC_VER)
// Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check
// operator precedence for possible error
#pragma warning(disable : 4554)
#endif
Tensor3x kinematic_hessians;
#if defined(_MSC_VER)
#pragma warning(default : 4554) // C4554 enabled after tensor definition
#endif
ContactCholeskyDecomposition contact_chol;
VectorXs primal_dual_contact_solution;
VectorXs primal_rhs_contact;
#if defined(_MSC_VER)
// Eigen tensor warning: Eigen\CXX11\src/Tensor/Tensor.h(76,1): warning C4554: '&': check
// operator precedence for possible error
#pragma warning(disable : 4554)
#endif
Tensor3x d2tau_dqdq;
Tensor3x d2tau_dvdv;
Tensor3x d2tau_dqdv;
Tensor3x d2tau_dadq;
#if defined(_MSC_VER)
#pragma warning(default : 4554) // C4554 enabled after tensor definition
#endif
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6)
extended_motion_propagator; // Stores force propagator to the base link
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) extended_motion_propagator2;
PINOCCHIO_ALIGNED_STD_VECTOR(Matrix6) spatial_inv_inertia; // Stores spatial inverse inertia
PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_descendant;
PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_ancestor;
PINOCCHIO_ALIGNED_STD_VECTOR(int) constraints_supported_dim;
PINOCCHIO_ALIGNED_STD_VECTOR(std::set<size_t>) constraints_supported;
PINOCCHIO_ALIGNED_STD_VECTOR(size_t) joints_supporting_constraints;
PINOCCHIO_ALIGNED_STD_VECTOR(size_t) accumulation_joints;
PINOCCHIO_ALIGNED_STD_VECTOR(std::vector<size_t>) constraints_on_joint;
explicit DataTpl(const Model & model);
DataTpl()
{
}
private:
void computeLastChild(const Model & model);
void computeParents_fromRow(const Model & model);
void computeSupports_fromRow(const Model & model);
};
} // namespace pinocchio
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/data.hxx"
#if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
#include "pinocchio/multibody/data.txx"
#endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
#endif // ifndef __pinocchio_multibody_data_hpp__