pinocchio  2.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
rnea.hpp
1 //
2 // Copyright (c) 2021 INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_parallel_rnea_hpp__
6 #define __pinocchio_algorithm_parallel_rnea_hpp__
7 
8 #include <omp.h>
9 
10 #include "pinocchio/multibody/pool/model.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 
13 namespace pinocchio
14 {
31  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorPool, typename TangentVectorPool1, typename TangentVectorPool2, typename TangentVectorPool3>
32  void rnea(const int num_threads,
34  const Eigen::MatrixBase<ConfigVectorPool> & q,
35  const Eigen::MatrixBase<TangentVectorPool1> & v,
36  const Eigen::MatrixBase<TangentVectorPool2> & a,
37  const Eigen::MatrixBase<TangentVectorPool3> & tau)
38  {
40  typedef typename Pool::Model Model;
41  typedef typename Pool::Data Data;
42  typedef typename Pool::DataVector DataVector;
43 
44  const Model & model = pool.model();
45  DataVector & datas = pool.datas();
46  TangentVectorPool3 & res = tau.const_cast_derived();
47 
48  PINOCCHIO_CHECK_INPUT_ARGUMENT(num_threads <= pool.size(), "The pool is too small");
49  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.rows(), model.nq);
50  PINOCCHIO_CHECK_ARGUMENT_SIZE(v.rows(), model.nv);
51  PINOCCHIO_CHECK_ARGUMENT_SIZE(a.rows(), model.nv);
52  PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), model.nv);
53 
54  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), v.cols());
55  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), a.cols());
56  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), res.cols());
57 
58  omp_set_num_threads(num_threads);
59  const Eigen::DenseIndex batch_size = res.cols();
60  Eigen::DenseIndex i = 0;
61 
62 #pragma omp parallel for
63  for(i = 0; i < batch_size; i++)
64  {
65  const int thread_id = omp_get_thread_num();
66  Data & data = datas[(size_t)thread_id];
67  res.col(i) = rnea(model,data,q.col(i),v.col(i),a.col(i));
68  }
69  }
70 }
71 
72 #endif // ifndef __pinocchio_algorithm_parallel_rnea_hpp__
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::rnea
void rnea(const int 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:32
pinocchio::ModelPoolTpl
Definition: model.hpp:19
pinocchio::ModelTpl
Definition: fwd.hpp:23
pinocchio::ModelPoolTpl::datas
const DataVector & datas() const
Returns the data vectors.
Definition: model.hpp:113
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11