Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/geometry.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 112 | 250 | 44.8% |
Branches: | 118 | 556 | 21.2% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2022 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_geometry_hxx__ | ||
6 | #define __pinocchio_multibody_geometry_hxx__ | ||
7 | |||
8 | #include <algorithm> | ||
9 | |||
10 | #include "pinocchio/multibody/model.hpp" | ||
11 | |||
12 | #if BOOST_VERSION / 100 % 1000 >= 60 | ||
13 | #include <boost/bind/bind.hpp> | ||
14 | #include <boost/utility.hpp> | ||
15 | #else | ||
16 | #include <boost/bind.hpp> | ||
17 | #endif | ||
18 | #include <boost/foreach.hpp> | ||
19 | |||
20 | #include <sstream> | ||
21 | /// @cond DEV | ||
22 | |||
23 | namespace pinocchio | ||
24 | { | ||
25 | |||
26 | ✗ | inline CollisionPair::CollisionPair() | |
27 | ✗ | : Base((std::numeric_limits<GeomIndex>::max)(), (std::numeric_limits<GeomIndex>::max)()) | |
28 | { | ||
29 | } | ||
30 | |||
31 | /// | ||
32 | /// \brief Default constructor of a collision pair from two collision object indexes. | ||
33 | /// \remarks The two indexes must be different, otherwise the constructor throws. | ||
34 | /// | ||
35 | /// \param[in] co1 Index of the first collision object. | ||
36 | /// \param[in] co2 Index of the second collision object. | ||
37 | /// | ||
38 | 191 | inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2) | |
39 | 191 | : Base(co1, co2) | |
40 | { | ||
41 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 191 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
191 | PINOCCHIO_CHECK_INPUT_ARGUMENT(co1 != co2, "The index of collision objects must not be equal."); |
42 | 191 | } | |
43 | |||
44 | ✗ | inline bool CollisionPair::operator==(const CollisionPair & rhs) const | |
45 | { | ||
46 | ✗ | return (first == rhs.first && second == rhs.second) | |
47 | ✗ | || (first == rhs.second && second == rhs.first); | |
48 | } | ||
49 | |||
50 | ✗ | inline bool CollisionPair::operator!=(const CollisionPair & other) const | |
51 | { | ||
52 | ✗ | return !(*this == other); | |
53 | } | ||
54 | |||
55 | ✗ | inline void CollisionPair::disp(std::ostream & os) const | |
56 | { | ||
57 | ✗ | os << "collision pair (" << first << "," << second << ")\n"; | |
58 | } | ||
59 | |||
60 | ✗ | inline std::ostream & operator<<(std::ostream & os, const CollisionPair & X) | |
61 | { | ||
62 | ✗ | X.disp(os); | |
63 | ✗ | return os; | |
64 | } | ||
65 | |||
66 | ✗ | inline GeometryModel GeometryModel::clone() const | |
67 | { | ||
68 | ✗ | GeometryModel res; | |
69 | ✗ | res.ngeoms = ngeoms; | |
70 | ✗ | res.collisionPairs = collisionPairs; | |
71 | ✗ | res.collisionPairMapping = collisionPairMapping; | |
72 | |||
73 | ✗ | res.geometryObjects.reserve(geometryObjects.size()); | |
74 | ✗ | for (const GeometryObject & geometry_object : geometryObjects) | |
75 | { | ||
76 | ✗ | res.geometryObjects.push_back(geometry_object.clone()); | |
77 | } | ||
78 | |||
79 | ✗ | return res; | |
80 | } | ||
81 | |||
82 | 13 | inline GeometryData::GeometryData(const GeometryModel & geom_model) | |
83 |
1/2✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
|
13 | : oMg(geom_model.ngeoms) |
84 |
1/2✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
|
13 | , activeCollisionPairs(geom_model.collisionPairs.size(), true) |
85 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
86 |
2/4✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
|
13 | , distanceRequests(geom_model.collisionPairs.size(), hpp::fcl::DistanceRequest(true)) |
87 |
1/2✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
|
13 | , distanceResults(geom_model.collisionPairs.size()) |
88 |
1/2✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
|
26 | , collisionRequests( |
89 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
26 | geom_model.collisionPairs.size(), hpp::fcl::CollisionRequest(::hpp::fcl::NO_REQUEST, 1)) |
90 |
1/2✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
|
13 | , collisionResults(geom_model.collisionPairs.size()) |
91 | 13 | , radius() | |
92 | 13 | , collisionPairIndex(0) | |
93 | #endif // PINOCCHIO_WITH_HPP_FCL | ||
94 | 13 | , innerObjects() | |
95 |
2/4✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
|
26 | , outerObjects() |
96 | { | ||
97 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
98 |
18/30✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 70 times.
✓ Branch 25 taken 70 times.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 83 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 83 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 70 times.
✓ Branch 37 taken 13 times.
✓ Branch 38 taken 70 times.
✓ Branch 39 taken 13 times.
|
153 | BOOST_FOREACH (hpp::fcl::CollisionRequest & creq, collisionRequests) |
99 | { | ||
100 | 70 | creq.enable_cached_gjk_guess = true; | |
101 | } | ||
102 | #if HPP_FCL_VERSION_AT_LEAST(1, 4, 5) | ||
103 |
18/30✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 70 times.
✓ Branch 25 taken 70 times.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 83 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 83 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 70 times.
✓ Branch 37 taken 13 times.
✓ Branch 38 taken 70 times.
✓ Branch 39 taken 13 times.
|
153 | BOOST_FOREACH (hpp::fcl::DistanceRequest & dreq, distanceRequests) |
104 | { | ||
105 | 70 | dreq.enable_cached_gjk_guess = true; | |
106 | } | ||
107 | #endif | ||
108 |
1/2✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
|
13 | collision_functors.reserve(geom_model.collisionPairs.size()); |
109 |
1/2✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
|
13 | distance_functors.reserve(geom_model.collisionPairs.size()); |
110 | |||
111 |
2/2✓ Branch 1 taken 70 times.
✓ Branch 2 taken 13 times.
|
83 | for (size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index) |
112 | { | ||
113 | 70 | const CollisionPair & cp = geom_model.collisionPairs[cp_index]; | |
114 | 70 | const GeometryObject & obj_1 = geom_model.geometryObjects[cp.first]; | |
115 | 70 | const GeometryObject & obj_2 = geom_model.geometryObjects[cp.second]; | |
116 | |||
117 |
2/4✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
|
70 | collision_functors.push_back(ComputeCollision(obj_1, obj_2)); |
118 |
2/4✓ Branch 1 taken 70 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 70 times.
✗ Branch 5 not taken.
|
70 | distance_functors.push_back(ComputeDistance(obj_1, obj_2)); |
119 | } | ||
120 | #endif | ||
121 |
1/2✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
|
13 | fillInnerOuterObjectMaps(geom_model); |
122 | 13 | } | |
123 | |||
124 | 12 | inline GeometryData::GeometryData(const GeometryData & other) | |
125 | 12 | : oMg(other.oMg) | |
126 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , activeCollisionPairs(other.activeCollisionPairs) |
127 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
128 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , distanceRequests(other.distanceRequests) |
129 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , distanceResults(other.distanceResults) |
130 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , collisionRequests(other.collisionRequests) |
131 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , collisionResults(other.collisionResults) |
132 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , radius(other.radius) |
133 | 12 | , collisionPairIndex(other.collisionPairIndex) | |
134 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , collision_functors(other.collision_functors) |
135 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , distance_functors(other.distance_functors) |
136 | #endif // PINOCCHIO_WITH_HPP_FCL | ||
137 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , innerObjects(other.innerObjects) |
138 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | , outerObjects(other.outerObjects) |
139 | { | ||
140 | 12 | } | |
141 | |||
142 | ✗ | inline GeometryData & GeometryData::operator=(const GeometryData & other) | |
143 | { | ||
144 | ✗ | if (this != &other) | |
145 | { | ||
146 | ✗ | oMg = other.oMg; | |
147 | ✗ | activeCollisionPairs = other.activeCollisionPairs; | |
148 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
149 | ✗ | distanceRequests = other.distanceRequests; | |
150 | ✗ | distanceResults = other.distanceResults; | |
151 | ✗ | collisionRequests = other.collisionRequests; | |
152 | ✗ | collisionResults = other.collisionResults; | |
153 | ✗ | radius = other.radius; | |
154 | ✗ | collisionPairIndex = other.collisionPairIndex; | |
155 | ✗ | collision_functors = other.collision_functors; | |
156 | ✗ | distance_functors = other.distance_functors; | |
157 | #endif // PINOCCHIO_WITH_HPP_FCL | ||
158 | ✗ | innerObjects = other.innerObjects; | |
159 | ✗ | outerObjects = other.outerObjects; | |
160 | } | ||
161 | ✗ | return *this; | |
162 | } | ||
163 | |||
164 | 25 | inline GeometryData::~GeometryData() | |
165 | { | ||
166 | 25 | } | |
167 | |||
168 | template<typename S2, int O2, template<typename, int> class JointCollectionTpl> | ||
169 | ✗ | GeomIndex GeometryModel::addGeometryObject( | |
170 | const GeometryObject & object, const ModelTpl<S2, O2, JointCollectionTpl> & model) | ||
171 | { | ||
172 | ✗ | if (object.parentFrame < (FrameIndex)model.nframes) | |
173 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT( | |
174 | model.frames[object.parentFrame].parentJoint == object.parentJoint, | ||
175 | "The object joint parent and its frame joint parent do not match."); | ||
176 | |||
177 | ✗ | Eigen::DenseIndex idx = (Eigen::DenseIndex)(ngeoms++); | |
178 | ✗ | geometryObjects.push_back(object); | |
179 | ✗ | geometryObjects.back().parentJoint = model.frames[object.parentFrame].parentJoint; | |
180 | |||
181 | ✗ | collisionPairMapping.conservativeResize(idx + 1, idx + 1); | |
182 | ✗ | collisionPairMapping.col(idx).fill(-1); | |
183 | ✗ | collisionPairMapping.row(idx).fill(-1); | |
184 | ✗ | assert(collisionPairMapping.cols() == (Eigen::DenseIndex)ngeoms); | |
185 | |||
186 | ✗ | return (GeomIndex)idx; | |
187 | } | ||
188 | |||
189 | 905 | inline GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object) | |
190 | { | ||
191 | 905 | Eigen::DenseIndex idx = (Eigen::DenseIndex)(ngeoms++); | |
192 | 905 | geometryObjects.push_back(object); | |
193 | |||
194 | 905 | collisionPairMapping.conservativeResize(idx + 1, idx + 1); | |
195 |
1/2✓ Branch 2 taken 905 times.
✗ Branch 3 not taken.
|
905 | collisionPairMapping.col(idx).fill(-1); |
196 |
1/2✓ Branch 2 taken 905 times.
✗ Branch 3 not taken.
|
905 | collisionPairMapping.row(idx).fill(-1); |
197 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 905 times.
|
905 | assert(collisionPairMapping.cols() == (Eigen::DenseIndex)ngeoms); |
198 | |||
199 | 905 | return (GeomIndex)idx; | |
200 | } | ||
201 | |||
202 | ✗ | inline void GeometryModel::removeGeometryObject(const std::string & name) | |
203 | { | ||
204 | ✗ | GeomIndex i = 0; | |
205 | ✗ | GeometryObjectVector::iterator it; | |
206 | ✗ | for (it = geometryObjects.begin(); it != geometryObjects.end(); ++it, ++i) | |
207 | { | ||
208 | ✗ | if (it->name == name) | |
209 | { | ||
210 | ✗ | break; | |
211 | } | ||
212 | } | ||
213 | ✗ | PINOCCHIO_THROW( | |
214 | it != geometryObjects.end(), std::invalid_argument, | ||
215 | (std::string("Object ") + name + std::string(" does not belong to model")).c_str()); | ||
216 | // Remove all collision pairs that contain i as first or second index, | ||
217 | ✗ | for (CollisionPairVector::iterator itCol = collisionPairs.begin(); | |
218 | ✗ | itCol != collisionPairs.end(); ++itCol) | |
219 | { | ||
220 | ✗ | if ((itCol->first == i) || (itCol->second == i)) | |
221 | { | ||
222 | ✗ | itCol = collisionPairs.erase(itCol); | |
223 | ✗ | itCol--; | |
224 | } | ||
225 | else | ||
226 | { | ||
227 | // Indices of objects after the one that is removed should be decreased by one. | ||
228 | ✗ | if (itCol->first > i) | |
229 | ✗ | itCol->first--; | |
230 | ✗ | if (itCol->second > i) | |
231 | ✗ | itCol->second--; | |
232 | } | ||
233 | } | ||
234 | ✗ | geometryObjects.erase(it); | |
235 | ✗ | ngeoms--; | |
236 | } | ||
237 | |||
238 | 1 | inline GeomIndex GeometryModel::getGeometryId(const std::string & name) const | |
239 | { | ||
240 | #if BOOST_VERSION / 100 % 1000 >= 60 | ||
241 | using namespace boost::placeholders; | ||
242 | #endif | ||
243 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
1 | GeometryObjectVector::const_iterator it = std::find_if( |
244 | geometryObjects.begin(), geometryObjects.end(), | ||
245 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | boost::bind(&GeometryObject::name, _1) == name); |
246 | 1 | return GeomIndex(it - geometryObjects.begin()); | |
247 | } | ||
248 | |||
249 | ✗ | inline bool GeometryModel::existGeometryName(const std::string & name) const | |
250 | { | ||
251 | #if BOOST_VERSION / 100 % 1000 >= 60 | ||
252 | using namespace boost::placeholders; | ||
253 | #endif | ||
254 | ✗ | return std::find_if( | |
255 | geometryObjects.begin(), geometryObjects.end(), | ||
256 | ✗ | boost::bind(&GeometryObject::name, _1) == name) | |
257 | ✗ | != geometryObjects.end(); | |
258 | } | ||
259 | |||
260 | 13 | inline void GeometryData::fillInnerOuterObjectMaps(const GeometryModel & geomModel) | |
261 | { | ||
262 | 13 | innerObjects.clear(); | |
263 | 13 | outerObjects.clear(); | |
264 | |||
265 |
2/2✓ Branch 1 taken 283 times.
✓ Branch 2 taken 13 times.
|
296 | for (GeomIndex gid = 0; gid < geomModel.geometryObjects.size(); gid++) |
266 |
2/4✓ Branch 2 taken 283 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 283 times.
✗ Branch 6 not taken.
|
283 | innerObjects[geomModel.geometryObjects[gid].parentJoint].push_back(gid); |
267 | |||
268 |
18/30✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 70 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 70 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 70 times.
✓ Branch 25 taken 70 times.
✓ Branch 26 taken 70 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 70 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 83 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 83 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 70 times.
✓ Branch 37 taken 13 times.
✓ Branch 38 taken 70 times.
✓ Branch 39 taken 13 times.
|
153 | BOOST_FOREACH (const CollisionPair & pair, geomModel.collisionPairs) |
269 | { | ||
270 |
2/4✓ Branch 2 taken 70 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 70 times.
✗ Branch 6 not taken.
|
70 | outerObjects[geomModel.geometryObjects[pair.first].parentJoint].push_back(pair.second); |
271 | } | ||
272 | 13 | } | |
273 | |||
274 | ✗ | inline std::ostream & operator<<(std::ostream & os, const GeometryModel & geomModel) | |
275 | { | ||
276 | ✗ | os << "Nb geometry objects = " << geomModel.ngeoms << std::endl; | |
277 | |||
278 | ✗ | for (GeomIndex i = 0; i < (GeomIndex)(geomModel.ngeoms); ++i) | |
279 | { | ||
280 | ✗ | os << geomModel.geometryObjects[i] << std::endl; | |
281 | } | ||
282 | |||
283 | ✗ | return os; | |
284 | } | ||
285 | |||
286 | ✗ | inline std::ostream & operator<<(std::ostream & os, const GeometryData & geomData) | |
287 | { | ||
288 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
289 | ✗ | os << "Number of collision pairs = " << geomData.activeCollisionPairs.size() << std::endl; | |
290 | |||
291 | ✗ | for (PairIndex i = 0; i < (PairIndex)(geomData.activeCollisionPairs.size()); ++i) | |
292 | { | ||
293 | ✗ | os << "Pairs " << i << (geomData.activeCollisionPairs[i] ? " active" : " inactive") | |
294 | ✗ | << std::endl; | |
295 | } | ||
296 | #else | ||
297 | os << "WARNING** Without fcl library, no collision checking or distance computations are " | ||
298 | "possible. Only geometry placements can be computed." | ||
299 | << std::endl; | ||
300 | #endif | ||
301 | ✗ | os << "Number of geometry objects = " << geomData.oMg.size() << std::endl; | |
302 | |||
303 | ✗ | return os; | |
304 | } | ||
305 | |||
306 | 191 | inline void GeometryModel::addCollisionPair(const CollisionPair & pair) | |
307 | { | ||
308 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 191 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
191 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
309 | pair.first < ngeoms, "The input pair.first is larger than the number of geometries " | ||
310 | "contained in the GeometryModel"); | ||
311 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 191 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
191 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
312 | pair.second < ngeoms, "The input pair.second is larger than the number of geometries " | ||
313 | "contained in the GeometryModel"); | ||
314 |
1/2✓ Branch 1 taken 191 times.
✗ Branch 2 not taken.
|
191 | if (!existCollisionPair(pair)) |
315 | { | ||
316 | 191 | collisionPairs.push_back(pair); | |
317 | |||
318 | 382 | collisionPairMapping((Eigen::DenseIndex)pair.second, (Eigen::DenseIndex)pair.first) = | |
319 | 191 | (int)(collisionPairs.size() - 1); | |
320 | 191 | collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second) = | |
321 | 191 | collisionPairMapping( | |
322 | 191 | (Eigen::DenseIndex)pair.second, | |
323 | 191 | (Eigen::DenseIndex)pair.first); // make symmetric | |
324 | } | ||
325 | 191 | } | |
326 | |||
327 | ✗ | inline void GeometryModel::setCollisionPairs(const MatrixXb & map, const bool upper) | |
328 | { | ||
329 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
330 | map.rows(), (Eigen::DenseIndex)ngeoms, "Input map does not have the correct number of rows."); | ||
331 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
332 | map.cols(), (Eigen::DenseIndex)ngeoms, | ||
333 | "Input map does not have the correct number of columns."); | ||
334 | ✗ | removeAllCollisionPairs(); | |
335 | |||
336 | ✗ | for (Eigen::DenseIndex i = 0; i < (Eigen::DenseIndex)ngeoms; ++i) | |
337 | { | ||
338 | ✗ | for (Eigen::DenseIndex j = i + 1; j < (Eigen::DenseIndex)ngeoms; ++j) | |
339 | { | ||
340 | ✗ | if (upper) | |
341 | { | ||
342 | ✗ | if (map(i, j)) | |
343 | ✗ | addCollisionPair(CollisionPair((std::size_t)i, (std::size_t)j)); | |
344 | } | ||
345 | else | ||
346 | { | ||
347 | ✗ | if (map(j, i)) | |
348 | ✗ | addCollisionPair(CollisionPair((std::size_t)i, (std::size_t)j)); | |
349 | } | ||
350 | } | ||
351 | } | ||
352 | } | ||
353 | |||
354 | 1 | inline void GeometryModel::addAllCollisionPairs() | |
355 | { | ||
356 | 1 | removeAllCollisionPairs(); | |
357 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 1 times.
|
21 | for (GeomIndex i = 0; i < ngeoms; ++i) |
358 | { | ||
359 | 20 | const JointIndex joint_i = geometryObjects[i].parentJoint; | |
360 |
2/2✓ Branch 0 taken 190 times.
✓ Branch 1 taken 20 times.
|
210 | for (GeomIndex j = i + 1; j < ngeoms; ++j) |
361 | { | ||
362 | 190 | const JointIndex joint_j = geometryObjects[j].parentJoint; | |
363 |
1/2✓ Branch 0 taken 190 times.
✗ Branch 1 not taken.
|
190 | if (joint_i != joint_j) |
364 |
1/2✓ Branch 2 taken 190 times.
✗ Branch 3 not taken.
|
190 | addCollisionPair(CollisionPair(i, j)); |
365 | } | ||
366 | } | ||
367 | 1 | } | |
368 | |||
369 | 120 | inline void GeometryModel::removeCollisionPair(const CollisionPair & pair) | |
370 | { | ||
371 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 120 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
120 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
372 | pair.first < ngeoms, "The input pair.first is larger than the number of geometries " | ||
373 | "contained in the GeometryModel"); | ||
374 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 120 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
120 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
375 | pair.second < ngeoms, "The input pair.second is larger than the number of geometries " | ||
376 | "contained in the GeometryModel"); | ||
377 | |||
378 | 120 | const long index = (long)findCollisionPair(pair); | |
379 | |||
380 |
1/2✓ Branch 1 taken 120 times.
✗ Branch 2 not taken.
|
120 | if (index != (long)collisionPairs.size()) |
381 | { | ||
382 | 240 | collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second) = | |
383 | 120 | collisionPairMapping((Eigen::DenseIndex)pair.second, (Eigen::DenseIndex)pair.first) = -1; | |
384 |
1/2✓ Branch 4 taken 120 times.
✗ Branch 5 not taken.
|
120 | collisionPairs.erase(collisionPairs.begin() + index); |
385 | |||
386 |
2/2✓ Branch 1 taken 2400 times.
✓ Branch 2 taken 120 times.
|
2520 | for (Eigen::DenseIndex i = 0; i < collisionPairMapping.cols(); ++i) |
387 | { | ||
388 |
2/2✓ Branch 1 taken 22800 times.
✓ Branch 2 taken 2400 times.
|
25200 | for (Eigen::DenseIndex j = i + 1; j < collisionPairMapping.cols(); ++j) |
389 | { | ||
390 |
2/2✓ Branch 1 taken 8328 times.
✓ Branch 2 taken 14472 times.
|
22800 | if (collisionPairMapping(i, j) > index) |
391 | { | ||
392 | 8328 | collisionPairMapping(i, j)--; | |
393 | 8328 | collisionPairMapping(j, i) = collisionPairMapping(i, j); | |
394 | } | ||
395 | } | ||
396 | } | ||
397 | } | ||
398 | 120 | } | |
399 | |||
400 | 1 | inline void GeometryModel::removeAllCollisionPairs() | |
401 | { | ||
402 | 1 | collisionPairs.clear(); | |
403 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | collisionPairMapping.fill(-1); |
404 | 1 | } | |
405 | |||
406 | 191 | inline bool GeometryModel::existCollisionPair(const CollisionPair & pair) const | |
407 | { | ||
408 | 191 | return collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second) | |
409 | 191 | != -1; | |
410 | } | ||
411 | |||
412 | 15780 | inline PairIndex GeometryModel::findCollisionPair(const CollisionPair & pair) const | |
413 | { | ||
414 | 15780 | int res = collisionPairMapping((Eigen::DenseIndex)pair.first, (Eigen::DenseIndex)pair.second); | |
415 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 15780 times.
|
15780 | if (res == -1) |
416 | ✗ | return collisionPairs.size(); | |
417 | else | ||
418 | 15780 | return (PairIndex)res; | |
419 | } | ||
420 | |||
421 | ✗ | inline void GeometryData::activateCollisionPair(const PairIndex pair_id) | |
422 | { | ||
423 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT( | |
424 | pair_id < activeCollisionPairs.size(), | ||
425 | "The input argument pair_id is larger than the number of " | ||
426 | "collision pairs contained in activeCollisionPairs."); | ||
427 | ✗ | activeCollisionPairs[pair_id] = true; | |
428 | } | ||
429 | |||
430 | ✗ | inline void GeometryData::activateAllCollisionPairs() | |
431 | { | ||
432 | ✗ | std::fill(activeCollisionPairs.begin(), activeCollisionPairs.end(), true); | |
433 | } | ||
434 | |||
435 | ✗ | inline void GeometryData::setActiveCollisionPairs( | |
436 | const GeometryModel & geom_model, const MatrixXb & map, const bool upper) | ||
437 | { | ||
438 | ✗ | const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms; | |
439 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
440 | map.rows(), ngeoms, "Input map does not have the correct number of rows."); | ||
441 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
442 | map.cols(), ngeoms, "Input map does not have the correct number of columns."); | ||
443 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
444 | geom_model.collisionPairs.size(), activeCollisionPairs.size(), | ||
445 | "Current geometry data and the input geometry model are not conistent."); | ||
446 | |||
447 | ✗ | for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k) | |
448 | { | ||
449 | ✗ | const CollisionPair & cp = geom_model.collisionPairs[k]; | |
450 | |||
451 | Eigen::DenseIndex i, j; | ||
452 | ✗ | if (upper) | |
453 | { | ||
454 | ✗ | j = (Eigen::DenseIndex)std::max(cp.first, cp.second); | |
455 | ✗ | i = (Eigen::DenseIndex)std::min(cp.first, cp.second); | |
456 | } | ||
457 | else | ||
458 | { | ||
459 | ✗ | i = (Eigen::DenseIndex)std::max(cp.first, cp.second); | |
460 | ✗ | j = (Eigen::DenseIndex)std::min(cp.first, cp.second); | |
461 | } | ||
462 | |||
463 | ✗ | activeCollisionPairs[k] = map(i, j); | |
464 | } | ||
465 | } | ||
466 | |||
467 | ✗ | inline void GeometryData::setGeometryCollisionStatus( | |
468 | const GeometryModel & geom_model, const GeomIndex geom_id, const bool enable_collision) | ||
469 | { | ||
470 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT( | |
471 | geom_id < geom_model.ngeoms, "The index of the geometry is not valid"); | ||
472 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
473 | geom_model.collisionPairs.size(), activeCollisionPairs.size(), | ||
474 | "Current geometry data and the input geometry model are not conistent."); | ||
475 | |||
476 | ✗ | for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k) | |
477 | { | ||
478 | ✗ | const CollisionPair & cp = geom_model.collisionPairs[k]; | |
479 | ✗ | if (cp.first == geom_id || cp.second == geom_id) | |
480 | { | ||
481 | ✗ | activeCollisionPairs[k] = enable_collision; | |
482 | } | ||
483 | } | ||
484 | } | ||
485 | |||
486 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
487 | ✗ | inline void GeometryData::setSecurityMargins( | |
488 | const GeometryModel & geom_model, | ||
489 | const MatrixXs & security_margin_map, | ||
490 | const bool upper, | ||
491 | const bool sync_distance_upper_bound) | ||
492 | { | ||
493 | ✗ | const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms; | |
494 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
495 | security_margin_map.rows(), ngeoms, "Input map does not have the correct number of rows."); | ||
496 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
497 | security_margin_map.cols(), ngeoms, "Input map does not have the correct number of columns."); | ||
498 | ✗ | PINOCCHIO_CHECK_ARGUMENT_SIZE( | |
499 | geom_model.collisionPairs.size(), collisionRequests.size(), | ||
500 | "Current geometry data and the input geometry model are not consistent."); | ||
501 | |||
502 | ✗ | for (size_t k = 0; k < geom_model.collisionPairs.size(); ++k) | |
503 | { | ||
504 | ✗ | const CollisionPair & cp = geom_model.collisionPairs[k]; | |
505 | |||
506 | Eigen::DenseIndex i, j; | ||
507 | ✗ | if (upper) | |
508 | { | ||
509 | ✗ | j = (Eigen::DenseIndex)std::max(cp.first, cp.second); | |
510 | ✗ | i = (Eigen::DenseIndex)std::min(cp.first, cp.second); | |
511 | } | ||
512 | else | ||
513 | { | ||
514 | ✗ | i = (Eigen::DenseIndex)std::max(cp.first, cp.second); | |
515 | ✗ | j = (Eigen::DenseIndex)std::min(cp.first, cp.second); | |
516 | } | ||
517 | |||
518 | ✗ | collisionRequests[k].security_margin = security_margin_map(i, j); | |
519 | ✗ | if (sync_distance_upper_bound) | |
520 | ✗ | collisionRequests[k].distance_upper_bound = collisionRequests[k].security_margin; | |
521 | } | ||
522 | } | ||
523 | #endif // ifdef PINOCCHIO_WITH_HPP_FCL | ||
524 | |||
525 | ✗ | inline void GeometryData::deactivateCollisionPair(const PairIndex pair_id) | |
526 | { | ||
527 | ✗ | PINOCCHIO_CHECK_INPUT_ARGUMENT( | |
528 | pair_id < activeCollisionPairs.size(), | ||
529 | "The input argument pair_id is larger than the number of " | ||
530 | "collision pairs contained in activeCollisionPairs."); | ||
531 | ✗ | activeCollisionPairs[pair_id] = false; | |
532 | } | ||
533 | |||
534 | ✗ | inline void GeometryData::deactivateAllCollisionPairs() | |
535 | { | ||
536 | ✗ | std::fill(activeCollisionPairs.begin(), activeCollisionPairs.end(), false); | |
537 | } | ||
538 | |||
539 | } // namespace pinocchio | ||
540 | |||
541 | /// @endcond | ||
542 | |||
543 | #endif // ifndef __pinocchio_multibody_geometry_hxx__ | ||
544 |