pinocchio  3.2.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
broadphase.hpp
1 //
2 // Copyright (c) 2022 INRIA
3 //
4 
5 #ifndef __pinocchio_collision_broadphase_hpp__
6 #define __pinocchio_collision_broadphase_hpp__
7 
8 #include <hpp/fcl/broadphase/broadphase_collision_manager.h>
9 
10 #include "pinocchio/multibody/fcl.hpp"
11 
12 #include "pinocchio/algorithm/geometry.hpp"
13 
14 #include "pinocchio/collision/broadphase-manager.hpp"
15 #include "pinocchio/collision/broadphase-callbacks.hpp"
16 
17 namespace pinocchio
18 {
19 
33  template<typename BroadPhaseManagerDerived>
36  CollisionCallBackBase * callback)
37  {
38  PINOCCHIO_CHECK_INPUT_ARGUMENT(broadphase_manager.check(callback));
39  broadphase_manager.collide(callback);
40  callback->done();
41  return callback->collision;
42  }
43 
56  template<typename BroadPhaseManagerDerived>
59  const bool stopAtFirstCollision = false)
60  {
61  CollisionCallBackDefault callback(
62  broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(),
63  stopAtFirstCollision);
64 
65  return computeCollisions(broadphase_manager, &callback);
66  }
67 
88  template<
89  typename Scalar,
90  int Options,
91  template<typename, int>
92  class JointCollectionTpl,
93  typename BroadPhaseManagerDerived,
94  typename ConfigVectorType>
95  inline bool computeCollisions(
99  CollisionCallBackBase * callback,
100  const Eigen::MatrixBase<ConfigVectorType> & q)
101  {
103  model, data, broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(), q);
104 
105  broadphase_manager.update(false);
106  return computeCollisions(broadphase_manager, &callback);
107  }
108 
128  template<
129  typename Scalar,
130  int Options,
131  template<typename, int>
132  class JointCollectionTpl,
133  typename BroadPhaseManagerDerived,
134  typename ConfigVectorType>
135  inline bool computeCollisions(
139  const Eigen::MatrixBase<ConfigVectorType> & q,
140  const bool stopAtFirstCollision = false)
141  {
143  model, data, broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(), q);
144 
145  broadphase_manager.update(false);
146 
147  CollisionCallBackDefault callback(
148  broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(),
149  stopAtFirstCollision);
150  return computeCollisions(broadphase_manager, &callback);
151  }
152 
153 } // namespace pinocchio
154 
155 /* --- Details -------------------------------------------------------------------- */
156 #include "pinocchio/collision/broadphase.hxx"
157 
158 #endif // ifndef __pinocchio_collision_broadphase_hpp__
Main pinocchio namespace.
Definition: treeview.dox:11
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.
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
bool collide(CollisionObject &obj, CollisionCallBackBase *callback) const
Performs collision test between one object and all the objects belonging to the manager.
const GeometryModel & getGeometryModel() const
Returns the geometry model associated to the manager.
bool check() const
Check whether the base broad phase manager is aligned with the current collision_objects.
const GeometryData & getGeometryData() const
Returns the geometry data associated to the manager.
void update(bool compute_local_aabb=false)
Update the manager from the current geometry positions and update the underlying FCL broad phase mana...
Interface for Pinocchio collision callback functors.