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>
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>
92  class JointCollectionTpl,
93  typename BroadPhaseManagerDerived,
94  typename ConfigVectorType>
95  inline bool computeCollisions(
96  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
97  DataTpl<Scalar, Options, JointCollectionTpl> & data,
98  BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
99  CollisionCallBackBase * callback,
100  const Eigen::MatrixBase<ConfigVectorType> & q)
101  {
102  updateGeometryPlacements(
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(
136  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
137  DataTpl<Scalar, Options, JointCollectionTpl> & data,
138  BroadPhaseManagerBase<BroadPhaseManagerDerived> & broadphase_manager,
139  const Eigen::MatrixBase<ConfigVectorType> & q,
140  const bool stopAtFirstCollision = false)
141  {
142  updateGeometryPlacements(
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__
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