GCC Code Coverage Report

Directory: ./
File: include/pinocchio/collision/broadphase-callbacks.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 66 0.0%
Branches: 0 132 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022 INRIA
3 //
4
5 #ifndef __pinocchio_collision_broadphase_callback_hpp__
6 #define __pinocchio_collision_broadphase_callback_hpp__
7
8 #include <hpp/fcl/broadphase/broadphase_callbacks.h>
9
10 #include "pinocchio/multibody/fcl.hpp"
11 #include "pinocchio/multibody/geometry.hpp"
12
13 #include "pinocchio/collision/collision.hpp"
14
15 namespace pinocchio
16 {
17
18 /// @brief Interface for Pinocchio collision callback functors
19 struct CollisionCallBackBase : hpp::fcl::CollisionCallBackBase
20 {
21 ✗ CollisionCallBackBase(const GeometryModel & geometry_model, GeometryData & geometry_data)
22 ✗ : geometry_model_ptr(&geometry_model)
23 ✗ , geometry_data_ptr(&geometry_data)
24 ✗ , collision(false)
25 ✗ , accumulate(false)
26 {
27 }
28
29 ✗ const GeometryModel & getGeometryModel() const
30 {
31 ✗ return *geometry_model_ptr;
32 }
33 const GeometryData & getGeometryData() const
34 {
35 return *geometry_data_ptr;
36 }
37 ✗ GeometryData & getGeometryData()
38 {
39 ✗ return *geometry_data_ptr;
40 }
41
42 /// \brief If true, the stopping criteria related to the collision callback has been met and one
43 /// can stop.
44 virtual bool stop() const = 0;
45
46 /// \brief Callback method called after the termination of a collisition detection algorithm.
47 /// The default implementation does nothing.
48 ✗ virtual void done() {};
49
50 protected:
51 /// @brief Geometry model associated to the callback
52 const GeometryModel * geometry_model_ptr;
53
54 /// @brief Geometry data associated to the callback
55 GeometryData * geometry_data_ptr;
56
57 public:
58 /// @brief Whether there is a collision or not
59 bool collision;
60
61 /// @brief Whether the callback is used in an accumulate mode where several collide methods are
62 /// called successively.
63 bool accumulate;
64 };
65
66 struct CollisionCallBackDefault : CollisionCallBackBase
67 {
68 ✗ CollisionCallBackDefault(
69 const GeometryModel & geometry_model,
70 GeometryData & geometry_data,
71 bool stopAtFirstCollision = false)
72 ✗ : CollisionCallBackBase(geometry_model, geometry_data)
73 ✗ , stopAtFirstCollision(stopAtFirstCollision)
74 ✗ , count(0)
75 // , visited(Eigen::MatrixXd::Zero(geometry_model.ngeoms,geometry_model.ngeoms))
76 {
77 }
78
79 ✗ void init()
80 {
81 ✗ if (accumulate) // skip reseting of the parameters
82 ✗ return;
83
84 ✗ count = 0;
85 ✗ collision = false;
86 ✗ collisionPairIndex = std::numeric_limits<PairIndex>::max();
87 // visited.setZero();
88 }
89
90 ✗ bool collide(hpp::fcl::CollisionObject * o1, hpp::fcl::CollisionObject * o2)
91 {
92
93 ✗ assert(!stop() && "must never happened");
94
95 ✗ CollisionObject & co1 = reinterpret_cast<CollisionObject &>(*o1);
96 ✗ CollisionObject & co2 = reinterpret_cast<CollisionObject &>(*o2);
97
98 ✗ const Eigen::DenseIndex go1_index = (Eigen::DenseIndex)co1.geometryObjectIndex;
99 ✗ const Eigen::DenseIndex go2_index = (Eigen::DenseIndex)co2.geometryObjectIndex;
100
101 ✗ const GeometryModel & geometry_model = *geometry_model_ptr;
102
103 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
104 go1_index < (Eigen::DenseIndex)geometry_model.ngeoms && go1_index >= 0);
105 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
106 go2_index < (Eigen::DenseIndex)geometry_model.ngeoms && go2_index >= 0);
107
108 ✗ const int pair_index = geometry_model.collisionPairMapping(go1_index, go2_index);
109 ✗ if (pair_index == -1)
110 ✗ return false;
111
112 ✗ const GeometryData & geometry_data = *geometry_data_ptr;
113 ✗ const CollisionPair & cp = geometry_model.collisionPairs[(PairIndex)pair_index];
114 const bool do_collision_check =
115 ✗ geometry_data.activeCollisionPairs[(PairIndex)pair_index]
116 ✗ && !(
117 ✗ geometry_model.geometryObjects[cp.first].disableCollision
118 ✗ || geometry_model.geometryObjects[cp.second].disableCollision);
119 ✗ if (!do_collision_check)
120 ✗ return false;
121
122 ✗ count++;
123
124 fcl::CollisionRequest collision_request(
125 ✗ geometry_data_ptr->collisionRequests[size_t(pair_index)]);
126 ✗ collision_request.gjk_variant = fcl::GJKVariant::NesterovAcceleration;
127 // collision_request.gjk_initial_guess = fcl::GJKInitialGuess::BoundingVolumeGuess;
128
129 ✗ if (
130 ✗ co1.collisionGeometry().get()
131 ✗ != geometry_model.geometryObjects[size_t(go1_index)].geometry.get()
132 ✗ || co2.collisionGeometry().get()
133 ✗ != geometry_model.geometryObjects[size_t(go2_index)].geometry.get())
134 ✗ PINOCCHIO_THROW_PRETTY(
135 std::logic_error, "go1: " << go1_index << " or go2: " << go2_index
136 << " have not been updated and have missmatching pointers.");
137 // if(!(co1.collisionGeometry()->aabb_local.volume() < 0 ||
138 // co2.collisionGeometry()->aabb_local.volume() <0)) { // TODO(jcarpent): check potential
139 // bug
140 // collision_request.gjk_initial_guess = fcl::GJKInitialGuess::BoundingVolumeGuess;
141 // }
142
143 bool res;
144 try
145 {
146 ✗ res = computeCollision(
147 ✗ *geometry_model_ptr, *geometry_data_ptr, (PairIndex)pair_index, collision_request);
148 }
149 ✗ catch (std::logic_error & e)
150 {
151 ✗ PINOCCHIO_THROW_PRETTY(
152 std::logic_error, "Geometries with index go1: "
153 << go1_index << " or go2: " << go2_index
154 << " have produced an internal error within HPP-FCL.\n what:\n"
155 << e.what());
156 }
157
158 ✗ if (res && !collision)
159 {
160 ✗ collision = true;
161 ✗ collisionPairIndex = (PairIndex)pair_index;
162 }
163
164 ✗ if (!stopAtFirstCollision)
165 ✗ return false;
166 else
167 ✗ return res;
168 }
169
170 ✗ bool stop() const final
171 {
172 ✗ if (stopAtFirstCollision && collision)
173 ✗ return true;
174
175 ✗ return false;
176 }
177
178 ✗ void done() final
179 {
180 ✗ if (collision)
181 ✗ geometry_data_ptr->collisionPairIndex = collisionPairIndex;
182 }
183
184 /// @brief Whether to stop or not when localizing a first collision
185 bool stopAtFirstCollision;
186
187 /// @brief The collision index of the first pair in collision
188 PairIndex collisionPairIndex;
189
190 /// @brief Number of visits of the collide method
191 size_t count;
192
193 // Eigen::MatrixXd visited;
194 };
195
196 } // namespace pinocchio
197
198 /* --- Details -------------------------------------------------------------------- */
199 #include "pinocchio/collision/broadphase-callbacks.hxx"
200
201 #endif // ifndef __pinocchio_collision_broadphase_callback_hpp__
202