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>
34 const Eigen::MatrixBase<ConfigVectorPool> &
q,
35 const Eigen::MatrixBase<TangentVectorPool1> &
v,
36 const Eigen::MatrixBase<TangentVectorPool2> &
a,
37 const Eigen::MatrixBase<TangentVectorPool3> &
tau)
42 typedef typename Pool::DataVector DataVector;
45 DataVector & datas = pool.
datas();
46 TangentVectorPool3 &
res = tau.const_cast_derived();
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 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__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
const DataVector & datas() const
Returns the data vectors.
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...
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
int size() const
Returns the size of the pool.
#define PINOCCHIO_CHECK_ARGUMENT_SIZE(...)
Macro to check if the size of an element is equal to the expected size.
const Model & model() const
Returns the model stored within the pool.
Main pinocchio namespace.
int nv
Dimension of the velocity vector space.
#define PINOCCHIO_CHECK_INPUT_ARGUMENT(...)
Macro to check an assert-like condition and throw a std::invalid_argument exception (with a message) ...
JointCollectionTpl & model
int nq
Dimension of the configuration vector representation.