algorithm/parallel/aba.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2021 INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_parallel_aba_hpp__
6 #define __pinocchio_algorithm_parallel_aba_hpp__
7 
8 #include <omp.h>
9 
10 #include "pinocchio/multibody/pool/model.hpp"
11 #include "pinocchio/algorithm/aba.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 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)
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 = a.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) = aba(model,data,q.col(i),v.col(i),tau.col(i));
68  }
69  }
70 }
71 
72 #endif // ifndef __pinocchio_algorithm_parallel_aba_hpp__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
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.
ModelTpl< double > Model
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.
Definition: src/macros.hpp:147
const Model & model() const
Returns the model stored within the pool.
Vec3f a
DataTpl< double > Data
Main pinocchio namespace.
Definition: timings.cpp:28
res
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) ...
Definition: src/macros.hpp:127
JointCollectionTpl & model
int nq
Dimension of the configuration vector representation.


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:28