timings-parallel.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2021 INRIA
3 //
4 
9 
10 #ifdef PINOCCHIO_WITH_HPP_FCL
15 
24 #endif
25 
29 
30 #include <iostream>
31 
33 
34 int main(int /*argc*/, const char ** /*argv*/)
35 {
36  using namespace Eigen;
37  using namespace pinocchio;
38 
39  typedef Eigen::Matrix<bool, Eigen::Dynamic, 1> VectorXb;
40  // typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic> MatrixXb;
41 
43 #ifdef NDEBUG
44  const int NBT = 4000;
45 
46 #else
47  const int NBT = 1;
48  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
49 #endif
50 
51  const int BATCH_SIZE = 256;
52  const size_t NUM_THREADS = (size_t)omp_get_max_threads();
53 
54  const std::string filename =
56  + std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf");
57 
60 
61  std::cout << "nq = " << model.nq << std::endl;
62  std::cout << "nv = " << model.nv << std::endl;
63  std::cout << "name = " << model.name << std::endl;
64 
65 #ifdef PINOCCHIO_WITH_HPP_FCL
66  const std::string package_path = PINOCCHIO_MODEL_DIR;
67  hpp::fcl::MeshLoaderPtr mesh_loader = std::make_shared<hpp::fcl::CachedMeshLoader>();
68  const std::string srdf_filename =
69  PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf");
70  std::vector<std::string> package_paths(1, package_path);
71  pinocchio::GeometryModel geometry_model;
73  model, filename, COLLISION, geometry_model, package_paths, mesh_loader);
74 
75  geometry_model.addAllCollisionPairs();
77 
78  GeometryData geometry_data(geometry_model);
79  {
80  int num_active_collision_pairs = 0;
81  for (size_t k = 0; k < geometry_data.activeCollisionPairs.size(); ++k)
82  {
83  if (geometry_data.activeCollisionPairs[k])
84  num_active_collision_pairs++;
85  }
86  std::cout << "active collision pairs = " << num_active_collision_pairs << std::endl;
87  }
88 #endif
89 
90  std::cout << "--" << std::endl;
91  std::cout << "NUM_THREADS: " << NUM_THREADS << std::endl;
92  std::cout << "BATCH_SIZE: " << BATCH_SIZE << std::endl;
93  std::cout << "--" << std::endl;
94 
96  const VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
97 
98  MatrixXd qs(model.nq, BATCH_SIZE);
99  MatrixXd vs(model.nv, BATCH_SIZE);
100  MatrixXd as(model.nv, BATCH_SIZE);
101  MatrixXd taus(model.nv, BATCH_SIZE);
102  MatrixXd res(model.nv, BATCH_SIZE);
103 
104  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) q_vec(NBT);
105  for (size_t i = 0; i < NBT; ++i)
106  {
107  q_vec[i] = randomConfiguration(model, -qmax, qmax);
108  }
109 
110  for (Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
111  {
112  qs.col(i) = randomConfiguration(model, -qmax, qmax);
113  vs.col(i) = Eigen::VectorXd::Random(model.nv);
114  as.col(i) = Eigen::VectorXd::Random(model.nv);
115  taus.col(i) = Eigen::VectorXd::Random(model.nv);
116  }
117 
118  ModelPool pool(model, NUM_THREADS);
119 
120  timer.tic();
121  SMOOTH(NBT)
122  {
123  for (Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
124  res.col(i) = rnea(model, data, qs.col(i), vs.col(i), as.col(i));
125  }
126  std::cout << "mean RNEA = \t\t\t\t";
127  timer.toc(std::cout, NBT * BATCH_SIZE);
128 
129  for (size_t num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
130  {
131  timer.tic();
132  SMOOTH(NBT)
133  {
134  rneaInParallel(num_threads, pool, qs, vs, as, res);
135  }
136  double elapsed = timer.toc() / (NBT * BATCH_SIZE);
137  std::stringstream ss;
138  ss << "mean RNEA pool (";
139  ss << num_threads;
140  ss << " threads) = \t\t";
141  ss << elapsed << " us" << std::endl;
142  std::cout << ss.str();
143  }
144 
145  std::cout << "--" << std::endl;
146 
147  timer.tic();
148  SMOOTH(NBT)
149  {
150  for (Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
151  res.col(i) = aba(model, data, qs.col(i), vs.col(i), taus.col(i), Convention::WORLD);
152  }
153  std::cout << "mean ABA = \t\t\t\t";
154  timer.toc(std::cout, NBT * BATCH_SIZE);
155 
156  for (size_t num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
157  {
158  timer.tic();
159  SMOOTH(NBT)
160  {
161  abaInParallel(num_threads, pool, qs, vs, taus, res);
162  }
163  double elapsed = timer.toc() / (NBT * BATCH_SIZE);
164  std::stringstream ss;
165  ss << "mean ABA pool (";
166  ss << num_threads;
167  ss << " threads) = \t\t";
168  ss << elapsed << " us" << std::endl;
169  std::cout << ss.str();
170  }
171 
172 #ifdef PINOCCHIO_WITH_HPP_FCL
173  std::cout << "--" << std::endl;
174  const int NBT_COLLISION = math::max(NBT, 1);
175  timer.tic();
176  SMOOTH((size_t)NBT_COLLISION)
177  {
178  computeCollisions(model, data, geometry_model, geometry_data, q_vec[_smooth]);
179  }
180  std::cout << "non parallel collision = \t\t\t";
181  timer.toc(std::cout, NBT_COLLISION);
182 
183  for (size_t num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
184  {
185  timer.tic();
186  SMOOTH((size_t)NBT_COLLISION)
187  {
189  num_threads, model, data, geometry_model, geometry_data, q_vec[_smooth]);
190  }
191  double elapsed = timer.toc() / (NBT_COLLISION);
192  std::stringstream ss;
193  ss << "parallel collision (";
194  ss << num_threads;
195  ss << " threads) = \t\t";
196  ss << elapsed << " us" << std::endl;
197  std::cout << ss.str();
198  }
199 
200  std::cout << "--" << std::endl;
201  GeometryPool geometry_pool(model, geometry_model, NUM_THREADS);
202  VectorXb collision_res(BATCH_SIZE);
203  collision_res.fill(false);
204 
205  timer.tic();
206  SMOOTH((size_t)(NBT_COLLISION / BATCH_SIZE))
207  {
208  for (Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
209  computeCollisions(model, data, geometry_model, geometry_data, qs.col(i));
210  }
211  std::cout << "non parallel collision = \t\t\t";
212  timer.toc(std::cout, NBT_COLLISION);
213 
214  for (size_t num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
215  {
216  timer.tic();
217  SMOOTH((size_t)(NBT_COLLISION / BATCH_SIZE))
218  {
219  computeCollisionsInParallel(num_threads, geometry_pool, qs, collision_res);
220  }
221  double elapsed = timer.toc() / (NBT_COLLISION);
222  std::stringstream ss;
223  ss << "pool parallel collision (";
224  ss << num_threads;
225  ss << " threads) = \t\t";
226  ss << elapsed << " us" << std::endl;
227  std::cout << ss.str();
228  }
229 
230  std::cout << "--" << std::endl;
231  {
233  model, geometry_model, NUM_THREADS);
234  VectorXb collision_res(BATCH_SIZE);
235  collision_res.fill(false);
236 
237  timer.tic();
238  SMOOTH((size_t)(NBT_COLLISION / BATCH_SIZE))
239  {
240  for (Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
241  computeCollisions(model, data, geometry_model, geometry_data, qs.col(i));
242  }
243  std::cout << "non parallel collision = \t\t\t";
244  timer.toc(std::cout, NBT_COLLISION);
245 
246  for (size_t num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
247  {
248  timer.tic();
249  SMOOTH((size_t)(NBT_COLLISION / BATCH_SIZE))
250  {
252  }
253  double elapsed = timer.toc() / (NBT_COLLISION);
254  std::stringstream ss;
255  ss << "pool parallel collision (";
256  ss << num_threads;
257  ss << " threads) = \t\t";
258  ss << elapsed << " us" << std::endl;
259  std::cout << ss.str();
260  }
261  }
262 
263  std::cout << "--" << std::endl;
264  {
266  model, geometry_model, NUM_THREADS);
267  VectorXb collision_res(BATCH_SIZE);
268  collision_res.fill(false);
269 
270  timer.tic();
271  SMOOTH((size_t)(NBT_COLLISION / BATCH_SIZE))
272  {
273  for (Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
274  computeCollisions(model, data, geometry_model, geometry_data, qs.col(i));
275  }
276  std::cout << "non parallel collision = \t\t\t";
277  timer.toc(std::cout, NBT_COLLISION);
278 
279  for (size_t num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
280  {
281  timer.tic();
282  SMOOTH((size_t)(NBT_COLLISION / BATCH_SIZE))
283  {
285  }
286  double elapsed = timer.toc() / (NBT_COLLISION);
287  std::stringstream ss;
288  ss << "pool parallel collision (";
289  ss << num_threads;
290  ss << " threads) = \t\t";
291  ss << elapsed << " us" << std::endl;
292  std::cout << ss.str();
293  }
294  }
295 #endif
296 
297  std::cout << "--" << std::endl;
298  return 0;
299 }
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
broadphase_dynamic_AABB_tree.h
broadphase_dynamic_AABB_tree_array.h
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:88
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::computeCollisions
bool computeCollisions(BroadPhaseManagerBase< BroadPhaseManagerDerived > &broadphase_manager, CollisionCallBackBase *callback)
Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeome...
Definition: broadphase.hpp:34
loader.h
pinocchio::BroadPhaseManagerPoolBase
Definition: collision/pool/broadphase-manager.hpp:22
rnea.hpp
continuous.BATCH_SIZE
int BATCH_SIZE
Definition: continuous.py:34
broadphase_bruteforce.h
main
int main(int, const char **)
Definition: timings-parallel.cpp:34
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:20
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
aba.hpp
pinocchio::python::context::JointModelFreeFlyer
JointModelFreeFlyerTpl< Scalar > JointModelFreeFlyer
Definition: bindings/python/context/generic.hpp:124
pinocchio::aba
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, const Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
pinocchio::GeometryData
Definition: multibody/geometry.hpp:233
pinocchio::GeometryModel::addAllCollisionPairs
void addAllCollisionPairs()
Add all possible collision pairs.
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:325
tree-broadphase-manager.hpp
timer.hpp
pinocchio::abaInParallel
void abaInParallel(const size_t num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &tau, const Eigen::MatrixBase< TangentVectorPool3 > &a)
A parallel version of the Articulated Body algorithm. It computes the forward dynamics,...
Definition: algorithm/parallel/aba.hpp:41
anymal-simulation.model
model
Definition: anymal-simulation.py:12
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
res
res
joint-configuration.hpp
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
broadphase_SSaP.h
pinocchio::GeometryData::activeCollisionPairs
std::vector< bool > activeCollisionPairs
Vector of collision pairs.
Definition: multibody/geometry.hpp:267
filename
filename
urdf.hpp
srdf.hpp
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:40
pinocchio::urdf::buildGeom
GeometryModel & buildGeom(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const std::string &filename, const GeometryType type, GeometryModel &geom_model, const std::vector< std::string > &package_paths=std::vector< std::string >(), ::hpp::fcl::MeshLoaderPtr mesh_loader=::hpp::fcl::MeshLoaderPtr())
Build The GeometryModel from a URDF file. Search for meshes in the directories specified by the user ...
collisions.srdf_filename
string srdf_filename
Definition: collisions.py:28
PinocchioTicToc::US
@ US
Definition: timer.hpp:53
geometry.hpp
pinocchio::python::VectorXb
Eigen::Matrix< bool, Eigen::Dynamic, 1 > VectorXb
Definition: bindings/python/collision/parallel/broadphase.cpp:28
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::computeCollisionsInParallel
void computeCollisionsInParallel(const size_t num_threads, BroadPhaseManagerPoolBase< BroadPhaseManagerDerived, Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< CollisionVectorResult > &res, const bool stopAtFirstCollisionInConfiguration=false, const bool stopAtFirstCollisionInBatch=false)
Definition: parallel/broadphase.hpp:26
broadphase_SaP.h
pinocchio::GeometryModel
Definition: multibody/geometry.hpp:50
PINOCCHIO_ALIGNED_STD_VECTOR
#define PINOCCHIO_ALIGNED_STD_VECTOR(Type)
Definition: container/aligned-vector.hpp:11
pinocchio::COLLISION
@ COLLISION
Definition: multibody/geometry-object.hpp:27
pinocchio::VectorXb
Eigen::Matrix< bool, Eigen::Dynamic, 1 > VectorXb
Definition: fwd.hpp:144
sample-models.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
fwd.hpp
PinocchioTicToc
Definition: timer.hpp:47
meshcat-viewer.qs
qs
Definition: meshcat-viewer.py:127
broadphase_interval_tree.h
CppAD::max
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
Definition: autodiff/cppad.hpp:180
pinocchio::GeometryPoolTpl
Definition: multibody/pool/fwd.hpp:24
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
broadphase_spatialhash.h
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11
pinocchio::srdf::removeCollisionPairs
void removeCollisionPairs(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, GeometryModel &geom_model, const std::string &filename, const bool verbose=false)
Deactive all possible collision pairs mentioned in the SRDF file. It throws if the SRDF file is incor...
PinocchioTicToc::tic
void tic()
Definition: timer.hpp:82
broadphase.hpp


pinocchio
Author(s):
autogenerated on Fri Jun 7 2024 02:40:49