data.hpp

//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_data_hpp__
#define __pinocchio_data_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 <iostream>
#include <Eigen/Cholesky>

namespace pinocchio
{
 
  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
  struct DataTpl
  {
    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 container::aligned_vector<JointModel> JointModelVector;
    typedef container::aligned_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 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;
    
    JointDataVector joints;
    
    container::aligned_vector<Motion> a;
    
    container::aligned_vector<Motion> oa;
    
    container::aligned_vector<Motion> a_gf;
    
    container::aligned_vector<Motion> oa_gf;
    
    container::aligned_vector<Motion> v;
    
    container::aligned_vector<Motion> ov;
    
    container::aligned_vector<Force> f;
    
    container::aligned_vector<Force> of;
    
    container::aligned_vector<Force> h;
    
    container::aligned_vector<Force> oh;
    
    container::aligned_vector<SE3> oMi;

    container::aligned_vector<SE3> liMi;
    
    TangentVectorType tau;
    
    VectorXs nle;
    
    VectorXs g;

    container::aligned_vector<SE3> oMf;

    container::aligned_vector<Inertia> Ycrb;
    
    container::aligned_vector<typename Inertia::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;

    container::aligned_vector<typename Inertia::Matrix6> vxI;
    
    container::aligned_vector<typename Inertia::Matrix6> Ivx;
    
    container::aligned_vector<Inertia> oYcrb;
    
    container::aligned_vector<typename Inertia::Matrix6> doYcrb;
    
    typename Inertia::Matrix6 Itmp;
    
    Matrix6 M6tmp;
    RowMatrix6 M6tmpR;
    RowMatrix6 M6tmpR2;
    
    TangentVectorType ddq;
    
    // ABA internal data
    container::aligned_vector<typename Inertia::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;

    container::aligned_vector<Matrix6x> Fcrb;

    std::vector<int> lastChild;
    std::vector<int> nvSubtree;

    MatrixXs U;
    
    VectorXs D;
    
    VectorXs Dinv;
    
    VectorXs tmp;
    
    std::vector<int> parents_fromRow;
    
    std::vector<int> nvSubtree_fromRow;
    
    Matrix6x J;
    
    Matrix6x dJ;
    
    Matrix6x dVdq;
    
    Matrix6x dAdq;
    
    Matrix6x dAdv;
    
    MatrixXs dtau_dq;
    
    MatrixXs dtau_dv;
    
    MatrixXs ddq_dq;
    
    MatrixXs ddq_dv;
    
    container::aligned_vector<SE3> iMf;

    container::aligned_vector<Vector3> com;
    
    container::aligned_vector<Vector3> vcom;
    
    container::aligned_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;
    
    // data related to regressor
    Matrix3x staticRegressor;
    
    explicit DataTpl(const Model & model);

  private:
    void computeLastChild(const Model & model);
    void computeParents_fromRow(const Model & model);

  };

} // namespace pinocchio

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/data.hxx"

#endif // ifndef __pinocchio_data_hpp__