parallel/rnea.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2021-2022 INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_parallel_rnea_hpp__
6 #define __pinocchio_algorithm_parallel_rnea_hpp__
7 
11 
12 namespace pinocchio
13 {
31  template<
32  typename Scalar,
33  int Options,
34  template<typename, int> class JointCollectionTpl,
35  typename ConfigVectorPool,
36  typename TangentVectorPool1,
37  typename TangentVectorPool2,
38  typename TangentVectorPool3>
40  const size_t num_threads,
42  const Eigen::MatrixBase<ConfigVectorPool> & q,
43  const Eigen::MatrixBase<TangentVectorPool1> & v,
44  const Eigen::MatrixBase<TangentVectorPool2> & a,
45  const Eigen::MatrixBase<TangentVectorPool3> & tau)
46  {
48  typedef typename Pool::Model Model;
49  typedef typename Pool::Data Data;
50  typedef typename Pool::ModelVector ModelVector;
51  typedef typename Pool::DataVector DataVector;
52 
53  PINOCCHIO_CHECK_INPUT_ARGUMENT(pool.size() > 0, "The pool should have at least one element");
54  PINOCCHIO_CHECK_INPUT_ARGUMENT(num_threads <= pool.size(), "The pool is too small");
55 
56  const ModelVector & models = pool.getModels();
57  const Model & model_check = models[0];
58  DataVector & datas = pool.getDatas();
59  TangentVectorPool3 & res = tau.const_cast_derived();
60 
61  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.rows(), model_check.nq);
62  PINOCCHIO_CHECK_ARGUMENT_SIZE(v.rows(), model_check.nv);
63  PINOCCHIO_CHECK_ARGUMENT_SIZE(a.rows(), model_check.nv);
64  PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), model_check.nv);
65 
66  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), v.cols());
67  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), a.cols());
68  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), res.cols());
69 
71  const Eigen::DenseIndex batch_size = res.cols();
72  Eigen::DenseIndex i = 0;
73 
74 #pragma omp parallel for schedule( \
75  static) // we use static here as this is the same computationnal cost for all threads
76  for (i = 0; i < batch_size; i++)
77  {
78  const int thread_id = omp_get_thread_num();
79  const Model & model = models[(size_t)thread_id];
80  Data & data = datas[(size_t)thread_id];
81  res.col(i) = rnea(model, data, q.col(i), v.col(i), a.col(i));
82  }
83  }
84 } // namespace pinocchio
85 
86 #endif // ifndef __pinocchio_algorithm_parallel_rnea_hpp__
PINOCCHIO_CHECK_ARGUMENT_SIZE
#define PINOCCHIO_CHECK_ARGUMENT_SIZE(...)
Macro to check if the size of an element is equal to the expected size.
Definition: include/pinocchio/macros.hpp:216
pinocchio::DataTpl
Definition: context/generic.hpp:25
PINOCCHIO_CHECK_INPUT_ARGUMENT
#define PINOCCHIO_CHECK_INPUT_ARGUMENT(...)
Macro to check an assert-like condition and throw a std::invalid_argument exception (with a message) ...
Definition: include/pinocchio/macros.hpp:192
pinocchio::Options
Options
Definition: joint-configuration.hpp:1082
omp.hpp
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
run-algo-in-parallel.num_threads
num_threads
Definition: run-algo-in-parallel.py:10
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::res
ReturnType res
Definition: spatial/classic-acceleration.hpp:57
run-algo-in-parallel.batch_size
int batch_size
Definition: run-algo-in-parallel.py:11
pinocchio::python::context::Model
ModelTpl< Scalar, Options > Model
Definition: bindings/python/context/generic.hpp:63
rnea.hpp
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
pinocchio::python::context::Data
DataTpl< Scalar, Options > Data
Definition: bindings/python/context/generic.hpp:64
pinocchio::Data
DataTpl< context::Scalar, context::Options > Data
Definition: multibody/fwd.hpp:34
pinocchio::ModelPoolTpl
Definition: multibody/pool/fwd.hpp:17
run-algo-in-parallel.pool
pool
Definition: run-algo-in-parallel.py:8
pinocchio::rneaInParallel
void rneaInParallel(const size_t 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: parallel/rnea.hpp:39
model.hpp
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
a
Vec3f a
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1084
pinocchio::rnea
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(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 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: multibody/model.hpp:101
pinocchio::set_default_omp_options
void set_default_omp_options(const size_t num_threads=(size_t) omp_get_max_threads())
Definition: omp.hpp:12
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: multibody/model.hpp:98
pinocchio::Model
ModelTpl< context::Scalar, context::Options > Model
Definition: multibody/fwd.hpp:33
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:12