Program Listing for File data.hpp

↰ Return to documentation for file (include/pinocchio/multibody/data.hpp)

//
// Copyright (c) 2015-2022 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/math/tensor.hpp"

#include "pinocchio/spatial/fwd.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/serialization/serializable.hpp"

#include <iostream>
#include <Eigen/Cholesky>

namespace pinocchio
{

  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
  struct DataTpl
  : serialization::Serializable< 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,3,1,Options> Vector3;
    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;

    JointDataVector joints;

    PINOCCHIO_ALIGNED_STD_VECTOR(Motion) a;

    PINOCCHIO_ALIGNED_STD_VECTOR(Motion) oa;

    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) 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

    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;

    MatrixXs dtau_dq;

    MatrixXs dtau_dv;

    MatrixXs ddq_dq;

    MatrixXs ddq_dv;

    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;

    // Temporary variables used in forward dynamics

    MatrixXs JMinvJt;

    Eigen::LLT<MatrixXs> llt_JMinvJt;

    VectorXs lambda_c;

    MatrixXs sDUiJt;

    VectorXs torque_residual;

    TangentVectorType dq_after;

    VectorXs impulse_c;

    Matrix3x staticRegressor;

    BodyRegressorType bodyRegressor;

    MatrixXs jointTorqueRegressor;

    Tensor3x kinematic_hessians;

    Tensor3x d2tau_dqdq;

    Tensor3x d2tau_dvdv;

    Tensor3x d2tau_dqdv;

    Tensor3x d2tau_dadq;

    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"

#endif // ifndef __pinocchio_multibody_data_hpp__