5 #ifndef __pinocchio_algorithm_parallel_aba_hpp__ 6 #define __pinocchio_algorithm_parallel_aba_hpp__ 10 #include "pinocchio/multibody/pool/model.hpp" 11 #include "pinocchio/algorithm/aba.hpp" 31 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl,
typename ConfigVectorPool,
typename TangentVectorPool1,
typename TangentVectorPool2,
typename TangentVectorPool3>
32 void aba(
const int num_threads,
34 const Eigen::MatrixBase<ConfigVectorPool> & q,
35 const Eigen::MatrixBase<TangentVectorPool1> & v,
36 const Eigen::MatrixBase<TangentVectorPool2> & tau,
37 const Eigen::MatrixBase<TangentVectorPool3> & a)
40 typedef typename Pool::Model
Model;
41 typedef typename Pool::Data
Data;
42 typedef typename Pool::DataVector DataVector;
44 const Model & model = pool.
model();
45 DataVector & datas = pool.
datas();
46 TangentVectorPool3 & res = a.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) =
aba(model,data,q.col(i),v.col(i),tau.col(i));
72 #endif // ifndef __pinocchio_algorithm_parallel_aba_hpp__ const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(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 > &tau)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
const DataVector & datas() const
Returns the data vectors.
int nv
Dimension of the velocity vector space.
int size() const
Returns the size of the pool.
Main pinocchio namespace.
const Model & model() const
Returns the model stored within the pool.
int nq
Dimension of the configuration vector representation.