pinocchio  2.6.3
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
geometry.hpp
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,
18  GeometryData & geom_data,
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>
60  bool computeCollisions(const int num_threads,
61  const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
62  DataTpl<Scalar,Options,JointCollectionTpl> & data,
63  const GeometryModel & geom_model,
64  GeometryData & geom_data,
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>
73  void computeCollisions(const int num_threads,
74  GeometryPoolTpl<Scalar,Options,JointCollectionTpl> & pool,
75  const Eigen::MatrixBase<ConfigVectorPool> & q,
76  const Eigen::MatrixBase<CollisionVectorResult> & res,
77  const bool stopAtFirstCollision = false)
78  {
79  typedef GeometryPoolTpl<Scalar,Options,JointCollectionTpl> Pool;
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__
bool computeCollisions(const GeometryModel &geom_model, GeometryData &geom_data, const bool stopAtFirstCollision=false)
Calls computeCollision for every active pairs of GeometryData. This function assumes that updateGeome...
Main pinocchio namespace.
Definition: treeview.dox:24
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.