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__