pinocchio  3.3.1
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>
34  bool computeCollisions(
35  BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
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>
57  bool computeCollisions(
58  BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
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> class JointCollectionTpl,
92  typename BroadPhaseManagerDerived,
93  typename ConfigVectorType>
94  inline bool computeCollisions(
95  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
96  DataTpl<Scalar, Options, JointCollectionTpl> & data,
97  BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
98  CollisionCallBackBase * callback,
99  const Eigen::MatrixBase<ConfigVectorType> & q)
100  {
101  updateGeometryPlacements(
102  model, data, broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(), q);
103 
104  broadphase_manager.update(false);
105  return computeCollisions(broadphase_manager, &callback);
106  }
107 
127  template<
128  typename Scalar,
129  int Options,
130  template<typename, int> class JointCollectionTpl,
131  typename BroadPhaseManagerDerived,
132  typename ConfigVectorType>
133  inline bool computeCollisions(
134  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
135  DataTpl<Scalar, Options, JointCollectionTpl> & data,
136  BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
137  const Eigen::MatrixBase<ConfigVectorType> & q,
138  const bool stopAtFirstCollision = false)
139  {
140  updateGeometryPlacements(
141  model, data, broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(), q);
142 
143  broadphase_manager.update(false);
144 
145  CollisionCallBackDefault callback(
146  broadphase_manager.getGeometryModel(), broadphase_manager.getGeometryData(),
147  stopAtFirstCollision);
148  return computeCollisions(broadphase_manager, &callback);
149  }
150 
151 } // namespace pinocchio
152 
153 /* --- Details -------------------------------------------------------------------- */
154 #include "pinocchio/collision/broadphase.hxx"
155 
156 #endif // ifndef __pinocchio_collision_broadphase_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::updateGeometryPlacements
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.
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
pinocchio::BroadPhaseManagerBase::collide
bool collide(CollisionObject &obj, CollisionCallBackBase *callback) const
Performs collision test between one object and all the objects belonging to the manager.
Definition: broadphase-manager-base.hpp:99
pinocchio::BroadPhaseManagerBase::getGeometryModel
const GeometryModel & getGeometryModel() const
Returns the geometry model associated to the manager.
Definition: broadphase-manager-base.hpp:133
pinocchio::BroadPhaseManagerBase::check
bool check() const
Check whether the base broad phase manager is aligned with the current collision_objects.
Definition: broadphase-manager-base.hpp:64
pinocchio::BroadPhaseManagerBase::getGeometryData
const GeometryData & getGeometryData() const
Returns the geometry data associated to the manager.
Definition: broadphase-manager-base.hpp:139
pinocchio::BroadPhaseManagerBase::update
void update(bool compute_local_aabb=false)
Update the manager from the current geometry positions and update the underlying FCL broad phase mana...
Definition: broadphase-manager-base.hpp:82
pinocchio::CollisionCallBackBase
Interface for Pinocchio collision callback functors.
Definition: broadphase-callbacks.hpp:20