force-base.hpp

// BSD 3-Clause License
//
// Copyright (C) 2021-2023, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_MULTIBODY_FORCE_BASE_HPP_
#define CROCODDYL_MULTIBODY_FORCE_BASE_HPP_
 
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/spatial/force.hpp>
 
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/multibody/fwd.hpp"
 
namespace crocoddyl {
 
template <typename _Scalar>
struct ForceDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;
  typedef typename pinocchio::DataTpl<Scalar> PinocchioData;
  typedef typename pinocchio::SE3Tpl<Scalar> SE3;
  typedef typename pinocchio::ForceTpl<Scalar> Force;
 
  template <template <typename Scalar> class Model>
  ForceDataAbstractTpl(Model<Scalar>* const model, PinocchioData* const data)
      : pinocchio(data),
        frame(0),
        type(model->get_type()),
        jMf(SE3::Identity()),
        Jc(model->get_nc(), model->get_state()->get_nv()),
        f(Force::Zero()),
        fext(Force::Zero()),
        df_dx(model->get_nc(), model->get_state()->get_ndx()),
        df_du(model->get_nc(), model->get_nu()) {
    Jc.setZero();
    df_dx.setZero();
    df_du.setZero();
  }
  virtual ~ForceDataAbstractTpl() {}
 
  PinocchioData* pinocchio;        
  pinocchio::FrameIndex frame;     
  pinocchio::ReferenceFrame type;  
  SE3 jMf;      
  MatrixXs Jc;  
  Force f;      
  Force fext;   
  MatrixXs df_dx;  
  MatrixXs df_du;  
};
 
}  // namespace crocoddyl
 
#endif  // CROCODDYL_MULTIBODY_FORCE_BASE_HPP_