pinocchio  2.6.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
model.hpp
1 //
2 // Copyright (c) 2021 INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_pool_model_hpp__
6 #define __pinocchio_multibody_pool_model_hpp__
7 
8 #include <algorithm>
9 #include <omp.h>
10 
11 #include "pinocchio/multibody/model.hpp"
12 #include "pinocchio/multibody/data.hpp"
13 
14 #include "pinocchio/utils/openmp.hpp"
15 
16 namespace pinocchio
17 {
18  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
20  {
21  public:
22 
23  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 
25  typedef _Scalar Scalar;
26  enum { Options = _Options };
27 
30 
31  typedef std::vector<Model,Eigen::aligned_allocator<Model> > ModelVector;
32  typedef std::vector<Data,Eigen::aligned_allocator<Data> > DataVector;
33 
39  explicit ModelPoolTpl(const Model & model,
40  const int pool_size = omp_get_max_threads())
41  : m_model(model)
42  , m_datas((size_t)pool_size, Data(model))
43  , m_size(pool_size)
44  {}
45 
50  ModelPoolTpl(const ModelPoolTpl & pool_model)
51  : m_model(pool_model.m_model)
52  , m_datas(pool_model.m_datas)
53  , m_size(pool_model.m_size)
54  {}
55 
57  const Model & model() const { return m_model; }
58 
60  Model & model() { return m_model; }
61 
66  void update(const Model & model)
67  {
68  m_model = model;
69  update(Data(model));
70  }
71 
76  void update(const Data & data)
77  {
78  std::fill(m_datas.begin(),m_datas.end(),data);
79  }
80 
82  int size() const { return m_size; }
83 
85  void resize(const int new_size)
86  {
87  m_datas.resize((size_t)new_size);
88  if(m_size < new_size)
89  {
90  typename DataVector::iterator it = m_datas.begin();
91  std::advance(it, (size_t)(new_size - m_size));
92  std::fill(it,m_datas.end(),m_datas[0]);
93  }
94  do_resize(new_size); // call Derived::do_resize();
95  m_size = new_size;
96  }
97 
99  const DataVector & datas() const { return m_datas; }
100 
102  DataVector & datas() { return m_datas; }
103 
105  const Data & data(const size_t index) const
106  {
107  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_datas.size(),
108  "Index greater than the size of the datas vector.");
109  return m_datas[index];
110  }
111 
113  Data & data(const size_t index)
114  {
115  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_datas.size(),
116  "Index greater than the size of the datas vector.");
117  return m_datas[index];
118  }
119 
121  virtual ~ModelPoolTpl() {};
122 
123  protected:
124 
126  Model m_model;
127 
129  DataVector m_datas;
130 
132  int m_size;
133 
135  virtual void do_resize(const int new_size)
136  {
137  PINOCCHIO_UNUSED_VARIABLE(new_size);
138  }
139 
140  };
141 
142  typedef ModelPoolTpl<double,0,JointCollectionDefaultTpl> ModelPool;
143 }
144 
145 #endif // ifndef __pinocchio_multibody_pool_model_hpp__
pinocchio::ModelPoolTpl::ModelPoolTpl
ModelPoolTpl(const ModelPoolTpl &pool_model)
Copy constructor from an other PoolModel.
Definition: model.hpp:50
pinocchio::ModelPoolTpl::m_size
int m_size
Number of threads used for parallel computations.
Definition: model.hpp:132
pinocchio::DataTpl
Definition: data.hpp:29
pinocchio::ModelPoolTpl::size
int size() const
Returns the size of the pool.
Definition: model.hpp:82
pinocchio::ModelPoolTpl::model
const Model & model() const
Returns the model stored within the pool.
Definition: model.hpp:57
pinocchio::ModelPoolTpl::m_datas
DataVector m_datas
 
Definition: model.hpp:129
pinocchio::ModelPoolTpl::do_resize
virtual void do_resize(const int new_size)
 
Definition: model.hpp:135
pinocchio::ModelPoolTpl::update
void update(const Data &data)
Update all the datas with the input data value.
Definition: model.hpp:76
pinocchio::ModelPoolTpl::datas
DataVector & datas()
Returns the data vectors.
Definition: model.hpp:102
pinocchio::ModelPoolTpl::~ModelPoolTpl
virtual ~ModelPoolTpl()
 
Definition: model.hpp:121
pinocchio::ModelPoolTpl::data
const Data & data(const size_t index) const
Return a specific data.
Definition: model.hpp:105
pinocchio::ModelPoolTpl::update
void update(const Model &model)
Update the model, meaning that all the datas will be refreshed accordingly.
Definition: model.hpp:66
pinocchio::ModelPoolTpl
Definition: model.hpp:19
pinocchio::ModelPoolTpl::model
Model & model()
Returns the model stored within the pool.
Definition: model.hpp:60
pinocchio::ModelPoolTpl::ModelPoolTpl
ModelPoolTpl(const Model &model, const int pool_size=omp_get_max_threads())
Default constructor from a model and a pool size.
Definition: model.hpp:39
pinocchio::ModelPoolTpl::resize
void resize(const int new_size)
Set the size of the pool and perform the appropriate resize.
Definition: model.hpp:85
pinocchio::ModelPoolTpl::m_model
Model m_model
Model stored within the pool.
Definition: model.hpp:121
pinocchio::ModelPoolTpl::data
Data & data(const size_t index)
Returns a specific data.
Definition: model.hpp:113
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl >
pinocchio::ModelPoolTpl::datas
const DataVector & datas() const
Returns the data vectors.
Definition: model.hpp:99
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24