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;

 std::vector<int> idx_vExtended_to_idx_v_fromRow;

 std::vector<JointIndex> mimic_subtree_joint;

 MatrixXs U;

 VectorXs D;

 VectorXs Dinv;

 VectorXs tmp;

 std::vector<int> parents_fromRow;

 std::vector<int> mimic_parents_fromRow;

 std::vector<int> non_mimic_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__