pinocchio  2.7.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 
88  void update(const Model & model,
89  const Data & data)
90  {
91  m_model = model;
92  update(data);
93  }
94 
96  int size() const { return m_size; }
97 
99  void resize(const int new_size)
100  {
101  m_datas.resize((size_t)new_size);
102  if(m_size < new_size)
103  {
104  typename DataVector::iterator it = m_datas.begin();
105  std::advance(it, (long)(new_size - m_size));
106  std::fill(it,m_datas.end(),m_datas[0]);
107  }
108  do_resize(new_size); // call Derived::do_resize();
109  m_size = new_size;
110  }
111 
113  const DataVector & datas() const { return m_datas; }
114 
116  DataVector & datas() { return m_datas; }
117 
119  const Data & data(const size_t index) const
120  {
121  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_datas.size(),
122  "Index greater than the size of the datas vector.");
123  return m_datas[index];
124  }
125 
127  Data & data(const size_t index)
128  {
129  PINOCCHIO_CHECK_INPUT_ARGUMENT(index < m_datas.size(),
130  "Index greater than the size of the datas vector.");
131  return m_datas[index];
132  }
133 
135  virtual ~ModelPoolTpl() {};
136 
137  protected:
138 
140  Model m_model;
141 
143  DataVector m_datas;
144 
146  int m_size;
147 
149  virtual void do_resize(const int new_size)
150  {
151  PINOCCHIO_UNUSED_VARIABLE(new_size);
152  }
153 
154  };
155 
156  typedef ModelPoolTpl<double,0,JointCollectionDefaultTpl> ModelPool;
157 }
158 
159 #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:146
pinocchio::DataTpl
Definition: data.hpp:29
pinocchio::ModelPoolTpl::size
int size() const
Returns the size of the pool.
Definition: model.hpp:96
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
&#160;
Definition: model.hpp:143
pinocchio::ModelPoolTpl::do_resize
virtual void do_resize(const int new_size)
&#160;
Definition: model.hpp:149
pinocchio::ModelPoolTpl::update
void update(const Model &model, const Data &data)
Update the model and data with the new input values. In this case, all the geometry_datas will be rep...
Definition: model.hpp:88
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:116
pinocchio::ModelPoolTpl::~ModelPoolTpl
virtual ~ModelPoolTpl()
&#160;
Definition: model.hpp:135
pinocchio::ModelPoolTpl::data
const Data & data(const size_t index) const
Return a specific data.
Definition: model.hpp:119
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:99
pinocchio::ModelPoolTpl::m_model
Model m_model
Model stored within the pool.
Definition: model.hpp:135
pinocchio::ModelPoolTpl::data
Data & data(const size_t index)
Returns a specific data.
Definition: model.hpp:127
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl >
pinocchio::ModelPoolTpl::datas
const DataVector & datas() const
Returns the data vectors.
Definition: model.hpp:113
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11