src/algorithm/parallel/geometry.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2021 INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_parallel_geometry_hpp__
6 #define __pinocchio_algorithm_parallel_geometry_hpp__
7 
8 #include <omp.h>
9 
10 #include "pinocchio/multibody/pool/geometry.hpp"
11 #include "pinocchio/algorithm/geometry.hpp"
12 
13 namespace pinocchio
14 {
15 
16  inline bool computeCollisions(const int num_threads,
17  const GeometryModel & geom_model,
19  const bool stopAtFirstCollision = false)
20  {
21  bool is_colliding = false;
22 
23  omp_set_num_threads(num_threads);
24  std::size_t cp_index = 0;
25 
26 #pragma omp parallel for
27  for(cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
28  {
29  if(stopAtFirstCollision && is_colliding) continue;
30  const CollisionPair & cp = geom_model.collisionPairs[cp_index];
31 
32  if(geom_data.activeCollisionPairs[cp_index]
33  && !( geom_model.geometryObjects[cp.first].disableCollision
34  || geom_model.geometryObjects[cp.second].disableCollision))
35  {
36  fcl::CollisionRequest & collision_request = geom_data.collisionRequests[cp_index];
37  collision_request.distance_upper_bound = collision_request.security_margin + 1e-6; // TODO: change the margin
38 
39  fcl::CollisionResult & collision_result = geom_data.collisionResults[cp_index];
40  collision_result.clear();
41 
42  fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[cp.first ])),
43  oM2 (toFclTransform3f(geom_data.oMg[cp.second]));
44 
45  const GeometryData::ComputeCollision & do_computations = geom_data.collision_functors[cp_index];
46  std::size_t res = do_computations(oM1, oM2, collision_request, collision_result);
47 
48  if(!is_colliding && res)
49  {
50  is_colliding = true;
51  geom_data.collisionPairIndex = cp_index; // first pair to be in collision
52  }
53  }
54  }
55 
56  return is_colliding;
57  }
58 
59  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
63  const GeometryModel & geom_model,
65  const Eigen::MatrixBase<ConfigVectorType> & q,
66  const bool stopAtFirstCollision = false)
67  {
68  updateGeometryPlacements(model, data, geom_model, geom_data, q);
69  return computeCollisions(num_threads, geom_model, geom_data, stopAtFirstCollision);
70  }
71 
72  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorPool, typename CollisionVectorResult>
75  const Eigen::MatrixBase<ConfigVectorPool> & q,
76  const Eigen::MatrixBase<CollisionVectorResult> & res,
77  const bool stopAtFirstCollision = false)
78  {
80  typedef typename Pool::Model Model;
81  typedef typename Pool::Data Data;
82  typedef typename Pool::GeometryModel GeometryModel;
83  typedef typename Pool::GeometryData GeometryData;
84  typedef typename Pool::DataVector DataVector;
85  typedef typename Pool::GeometryDataVector GeometryDataVector;
86 
87  const Model & model = pool.model();
88  const GeometryModel & geometry_model = pool.geometry_model();
89  DataVector & datas = pool.datas();
90  GeometryDataVector & geometry_datas = pool.geometry_datas();
91  CollisionVectorResult & res_ = res.const_cast_derived();
92 
93  PINOCCHIO_CHECK_INPUT_ARGUMENT(num_threads <= pool.size(), "The pool is too small");
94  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.rows(), model.nq);
95  PINOCCHIO_CHECK_ARGUMENT_SIZE(q.cols(), res.size());
96  res_.fill(false);
97 
98  omp_set_num_threads(num_threads);
99  const Eigen::DenseIndex batch_size = res.size();
100  Eigen::DenseIndex i = 0;
101 
102 #pragma omp parallel for
103  for(i = 0; i < batch_size; i++)
104  {
105  const int thread_id = omp_get_thread_num();
106  Data & data = datas[(size_t)thread_id];
107  GeometryData & geometry_data = geometry_datas[(size_t)thread_id];
108  res_[i] = computeCollisions(model,data,geometry_model,geometry_data,q.col(i),stopAtFirstCollision);
109  }
110  }
111 }
112 
113 #endif // ifndef __pinocchio_algorithm_parallel_geometry_hpp__
const DataVector & datas() const
Returns the data vectors.
const GeometryDataVector & geometry_datas() const
Vector of Geometry Data.
ModelTpl< double > Model
CollisionPairVector collisionPairs
Vector of collision pairs.
std::vector< bool > activeCollisionPairs
Vector of collision pairs.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
bool computeCollisions(const int num_threads, const GeometryModel &geom_model, GeometryData &geom_data, const bool stopAtFirstCollision=false)
int size() const
Returns the size of the pool.
GeometryObjectVector geometryObjects
Vector of GeometryObjects used for collision computations.
#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.
DataTpl< double > Data
hpp::fcl::Transform3f toFclTransform3f(const SE3 &m)
Main pinocchio namespace.
Definition: timings.cpp:28
res
void updateGeometryPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const GeometryModel &geom_model, GeometryData &geom_data, const Eigen::MatrixBase< ConfigVectorType > &q)
Apply a forward kinematics and update the placement of the geometry objects.
const GeometryModel & geometry_model() const
Returns the geometry model.
#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:30