data.hpp

//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_data_hpp__
#define __se3_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/frame.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
#include "pinocchio/deprecated.hh"
#include "pinocchio/utils/string-generator.hpp"
#include "pinocchio/container/aligned-vector.hpp"

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

namespace se3
{
  
  struct Data
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    
    typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;
    typedef Eigen::Matrix<double,3,Eigen::Dynamic> Matrix3x;
    typedef SE3::Vector3 Vector3;
    
    typedef Eigen::Matrix<double,6,6,Eigen::RowMajor> RowMatrix6;
    typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> RowMatrixXd;
    
    JointDataVector joints;
    
    container::aligned_vector<Motion> a;
    
    container::aligned_vector<Motion> oa;
    
    container::aligned_vector<Motion> a_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;
    
    Eigen::VectorXd tau;
    
    Eigen::VectorXd nle;
    
    Eigen::VectorXd g;

    container::aligned_vector<SE3> oMf;

    container::aligned_vector<Inertia> Ycrb;
    
    container::aligned_vector<Inertia::Matrix6> dYcrb; // TODO: change with dense symmetric matrix6
    
    Eigen::MatrixXd M;
    
    RowMatrixXd Minv;
    
    Eigen::MatrixXd C;
    
    Matrix6x dFdq;
    
    Matrix6x dFdv;
    
    Matrix6x dFda;

    Matrix6x SDinv;

    Matrix6x UDinv;

    Matrix6x IS;

    container::aligned_vector<Inertia::Matrix6> vxI;
    
    container::aligned_vector<Inertia::Matrix6> Ivx;
    
    container::aligned_vector<Inertia> oYcrb;
    
    container::aligned_vector<Inertia::Matrix6> doYcrb;
    
    Inertia::Matrix6 Itmp;
    
    RowMatrix6 M6tmpR;
    
    Eigen::VectorXd ddq;
    
    // ABA internal data
    container::aligned_vector<Inertia::Matrix6> Yaba;  // TODO: change with dense symmetric matrix6
    
    Eigen::VectorXd u;                  // Joint Inertia
    
    // CCRBA return quantities
    Matrix6x Ag;
    
    // dCCRBA return quantities
    Matrix6x dAg;
    
    Force hg;
    
    Inertia Ig;

    container::aligned_vector<Matrix6x> Fcrb;

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

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

    container::aligned_vector<Eigen::Vector3d> com;
    
    container::aligned_vector<Eigen::Vector3d> vcom;
    
    container::aligned_vector<Eigen::Vector3d> acom;
    
    std::vector<double> mass;
    
    Matrix3x Jcom;

    
    double kinetic_energy;
    
    double potential_energy;
    
    // Temporary variables used in forward dynamics
    
    Eigen::MatrixXd JMinvJt;
    
    Eigen::LLT<Eigen::MatrixXd> llt_JMinvJt;
    
    Eigen::VectorXd lambda_c;
    
    Eigen::MatrixXd sDUiJt;
    
    Eigen::VectorXd torque_residual;
    
    Eigen::VectorXd dq_after;
    
    Eigen::VectorXd impulse_c;
    
    // data related to regressor
    Matrix3x staticRegressor;
    
    explicit Data(const Model & model);

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

  };

} // namespace se3

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

#endif // ifndef __se3_data_hpp__