pinocchio  3.9.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
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
17namespace 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::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::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::BroadPhaseManagerBase::getGeometryData
const GeometryData & getGeometryData() const
Returns the geometry data associated to the manager.
Definition broadphase-manager-base.hpp:139
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::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::getGeometryModel
const GeometryModel & getGeometryModel() const
Returns the geometry model associated to the manager.
Definition broadphase-manager-base.hpp:133
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