5 #ifndef __pinocchio_algorithm_parallel_rnea_hpp__
6 #define __pinocchio_algorithm_parallel_rnea_hpp__
10 #include "pinocchio/multibody/pool/model.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
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)
40 typedef typename Pool::Model
Model;
41 typedef typename Pool::Data
Data;
42 typedef typename Pool::DataVector DataVector;
45 DataVector & datas = pool.
datas();
46 TangentVectorPool3 & res = tau.const_cast_derived();
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);
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());
58 omp_set_num_threads(num_threads);
59 const Eigen::DenseIndex batch_size = res.cols();
60 Eigen::DenseIndex i = 0;
62 #pragma omp parallel for
63 for(i = 0; i < batch_size; i++)
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));
72 #endif // ifndef __pinocchio_algorithm_parallel_rnea_hpp__