pinocchio  3.0.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
rnea.hpp
1 //
2 // Copyright (c) 2021-2022 INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_parallel_rnea_hpp__
6 #define __pinocchio_algorithm_parallel_rnea_hpp__
7 
8 #include "pinocchio/multibody/pool/model.hpp"
9 #include "pinocchio/algorithm/rnea.hpp"
10 #include "pinocchio/algorithm/parallel/omp.hpp"
11 
12 namespace pinocchio
13 {
31  template<
32  typename Scalar,
33  int Options,
34  template<typename, int>
35  class JointCollectionTpl,
36  typename ConfigVectorPool,
37  typename TangentVectorPool1,
38  typename TangentVectorPool2,
39  typename TangentVectorPool3>
41  const size_t num_threads,
43  const Eigen::MatrixBase<ConfigVectorPool> & q,
44  const Eigen::MatrixBase<TangentVectorPool1> & v,
45  const Eigen::MatrixBase<TangentVectorPool2> & a,
46  const Eigen::MatrixBase<TangentVectorPool3> & tau)
47  {
49  typedef typename Pool::Model Model;
50  typedef typename Pool::Data Data;
51  typedef typename Pool::ModelVector ModelVector;
52  typedef typename Pool::DataVector DataVector;
53 
54  PINOCCHIO_CHECK_INPUT_ARGUMENT(pool.size() > 0, "The pool should have at least one element");
55  PINOCCHIO_CHECK_INPUT_ARGUMENT(num_threads <= pool.size(), "The pool is too small");
56 
57  const ModelVector & models = pool.getModels();
58  const Model & model_check = models[0];
59  DataVector & datas = pool.getDatas();
60  TangentVectorPool3 & res = tau.const_cast_derived();
61 
62  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.rows(), model_check.nq);
63  PINOCCHIO_CHECK_ARGUMENT_SIZE(v.rows(), model_check.nv);
64  PINOCCHIO_CHECK_ARGUMENT_SIZE(a.rows(), model_check.nv);
65  PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), model_check.nv);
66 
67  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), v.cols());
68  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), a.cols());
69  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), res.cols());
70 
71  set_default_omp_options(num_threads);
72  const Eigen::DenseIndex batch_size = res.cols();
73  Eigen::DenseIndex i = 0;
74 
75 #pragma omp parallel for schedule( \
76  static) // we use static here as this is the same computationnal cost for all threads
77  for (i = 0; i < batch_size; i++)
78  {
79  const int thread_id = omp_get_thread_num();
80  const Model & model = models[(size_t)thread_id];
81  Data & data = datas[(size_t)thread_id];
82  res.col(i) = rnea(model, data, q.col(i), v.col(i), a.col(i));
83  }
84  }
85 } // namespace pinocchio
86 
87 #endif // ifndef __pinocchio_algorithm_parallel_rnea_hpp__
size_t size() const
Returns the size of the pool.
Definition: model.hpp:68
const DataVector & getDatas() const
Returns the data vector.
Definition: model.hpp:123
const ModelVector & getModels() const
Returns the vector of models.
Definition: model.hpp:95
Main pinocchio namespace.
Definition: treeview.dox:11
void rneaInParallel(const size_t num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
Definition: rnea.hpp:40
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
int nq
Dimension of the configuration vector representation.
Definition: model.hpp:99
int nv
Dimension of the velocity vector space.
Definition: model.hpp:102