GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2021 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 |
/// @cond DEV |
||
21 |
|||
22 |
namespace pinocchio |
||
23 |
{ |
||
24 |
|||
25 |
47 |
inline GeometryData::GeometryData(const GeometryModel & geom_model) |
|
26 |
47 |
: oMg(geom_model.ngeoms) |
|
27 |
, activeCollisionPairs(geom_model.collisionPairs.size(), true) |
||
28 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
29 |
✓✗ | 47 |
, distanceRequests(geom_model.collisionPairs.size(), hpp::fcl::DistanceRequest(true)) |
30 |
, distanceResults(geom_model.collisionPairs.size()) |
||
31 |
✓✗ | 47 |
, collisionRequests(geom_model.collisionPairs.size(), hpp::fcl::CollisionRequest(::hpp::fcl::NO_REQUEST,1)) |
32 |
, collisionResults(geom_model.collisionPairs.size()) |
||
33 |
, radius() |
||
34 |
, collisionPairIndex(0) |
||
35 |
#endif // PINOCCHIO_WITH_HPP_FCL |
||
36 |
, innerObjects() |
||
37 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
141 |
, outerObjects() |
38 |
{ |
||
39 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
40 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
6387 |
BOOST_FOREACH(hpp::fcl::CollisionRequest & creq, collisionRequests) |
41 |
{ |
||
42 |
3170 |
creq.enable_cached_gjk_guess = true; |
|
43 |
} |
||
44 |
#if HPP_FCL_VERSION_AT_LEAST(1, 4, 5) |
||
45 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
6387 |
BOOST_FOREACH(hpp::fcl::DistanceRequest & dreq, distanceRequests) |
46 |
{ |
||
47 |
3170 |
dreq.enable_cached_gjk_guess = true; |
|
48 |
} |
||
49 |
#endif |
||
50 |
✓✗ | 47 |
collision_functors.reserve(geom_model.collisionPairs.size()); |
51 |
✓✗ | 47 |
distance_functors.reserve(geom_model.collisionPairs.size()); |
52 |
|||
53 |
47 |
for(size_t cp_index = 0; |
|
54 |
✓✓ | 3217 |
cp_index < geom_model.collisionPairs.size(); ++cp_index) |
55 |
{ |
||
56 |
3170 |
const CollisionPair & cp = geom_model.collisionPairs[cp_index]; |
|
57 |
3170 |
const GeometryObject & obj_1 = geom_model.geometryObjects[cp.first]; |
|
58 |
3170 |
const GeometryObject & obj_2 = geom_model.geometryObjects[cp.second]; |
|
59 |
|||
60 |
✓✗✓✗ |
3170 |
collision_functors.push_back(ComputeCollision(obj_1,obj_2)); |
61 |
✓✗✓✗ |
3170 |
distance_functors.push_back(ComputeDistance(obj_1,obj_2)); |
62 |
} |
||
63 |
#endif |
||
64 |
✓✗ | 47 |
fillInnerOuterObjectMaps(geom_model); |
65 |
47 |
} |
|
66 |
|||
67 |
22 |
inline GeometryData::GeometryData(const GeometryData & other) |
|
68 |
22 |
: oMg (other.oMg) |
|
69 |
22 |
, activeCollisionPairs (other.activeCollisionPairs) |
|
70 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
71 |
22 |
, distanceRequests (other.distanceRequests) |
|
72 |
22 |
, distanceResults (other.distanceResults) |
|
73 |
22 |
, collisionRequests (other.collisionRequests) |
|
74 |
22 |
, collisionResults (other.collisionResults) |
|
75 |
22 |
, radius (other.radius) |
|
76 |
22 |
, collisionPairIndex (other.collisionPairIndex) |
|
77 |
22 |
, collision_functors (other.collision_functors) |
|
78 |
22 |
, distance_functors (other.distance_functors) |
|
79 |
#endif // PINOCCHIO_WITH_HPP_FCL |
||
80 |
22 |
, innerObjects (other.innerObjects) |
|
81 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
22 |
, outerObjects (other.outerObjects) |
82 |
22 |
{} |
|
83 |
|||
84 |
1 |
inline GeometryData& GeometryData::operator=(const GeometryData & other) |
|
85 |
{ |
||
86 |
✓✗ | 1 |
if (this != &other) |
87 |
{ |
||
88 |
1 |
oMg = other.oMg; |
|
89 |
1 |
activeCollisionPairs = other.activeCollisionPairs; |
|
90 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
91 |
1 |
distanceRequests = other.distanceRequests; |
|
92 |
1 |
distanceResults = other.distanceResults; |
|
93 |
1 |
collisionRequests = other.collisionRequests; |
|
94 |
1 |
collisionResults = other.collisionResults; |
|
95 |
1 |
radius = other.radius; |
|
96 |
1 |
collisionPairIndex = other.collisionPairIndex; |
|
97 |
1 |
collision_functors = other.collision_functors; |
|
98 |
1 |
distance_functors = other.distance_functors; |
|
99 |
#endif // PINOCCHIO_WITH_HPP_FCL |
||
100 |
1 |
innerObjects = other.innerObjects; |
|
101 |
1 |
outerObjects = other.outerObjects; |
|
102 |
} |
||
103 |
1 |
return *this; |
|
104 |
} |
||
105 |
|||
106 |
76 |
inline GeometryData::~GeometryData() {} |
|
107 |
|||
108 |
template<typename S2, int O2, template<typename,int> class JointCollectionTpl> |
||
109 |
2 |
GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object, |
|
110 |
const ModelTpl<S2,O2,JointCollectionTpl> & model) |
||
111 |
{ |
||
112 |
✓✗ | 2 |
if(object.parentFrame < (FrameIndex)model.nframes) |
113 |
✗✓✗✗ |
2 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(model.frames[object.parentFrame].parent == object.parentJoint, |
114 |
"The object joint parent and its frame joint parent do not match."); |
||
115 |
|||
116 |
2 |
GeomIndex idx = (GeomIndex) (ngeoms ++); |
|
117 |
2 |
geometryObjects.push_back(object); |
|
118 |
2 |
geometryObjects.back().parentJoint = model.frames[object.parentFrame].parent; |
|
119 |
2 |
return idx; |
|
120 |
} |
||
121 |
|||
122 |
1172 |
inline GeomIndex GeometryModel::addGeometryObject(const GeometryObject & object) |
|
123 |
{ |
||
124 |
1172 |
GeomIndex idx = (GeomIndex) (ngeoms ++); |
|
125 |
1172 |
geometryObjects.push_back(object); |
|
126 |
1172 |
return idx; |
|
127 |
} |
||
128 |
|||
129 |
1 |
inline void GeometryModel::removeGeometryObject(const std::string& name) |
|
130 |
{ |
||
131 |
1 |
GeomIndex i=0; |
|
132 |
1 |
GeometryObjectVector::iterator it; |
|
133 |
✓✗ | 2 |
for (it=geometryObjects.begin(); it!=geometryObjects.end(); ++it, ++i){ |
134 |
✓✓ | 2 |
if (it->name == name){ |
135 |
1 |
break; |
|
136 |
} |
||
137 |
} |
||
138 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_THROW(it != geometryObjects.end(),std::invalid_argument, (std::string("Object ") + name + std::string(" does not belong to model")).c_str()); |
139 |
// Remove all collision pairs that contain i as first or second index, |
||
140 |
✓✓ | 4 |
for (CollisionPairVector::iterator itCol = collisionPairs.begin(); itCol != collisionPairs.end(); ++itCol){ |
141 |
✓✓✓✓ ✓✓ |
3 |
if ((itCol->first == i) || (itCol->second == i)) { |
142 |
✓✗ | 2 |
itCol = collisionPairs.erase(itCol); itCol--; |
143 |
} else { |
||
144 |
// Indices of objects after the one that is removed should be decreased by one. |
||
145 |
✗✓ | 1 |
if (itCol->first > i) itCol->first--; |
146 |
✓✗ | 1 |
if (itCol->second > i) itCol->second--; |
147 |
} |
||
148 |
} |
||
149 |
✓✗ | 1 |
geometryObjects.erase(it); |
150 |
1 |
ngeoms--; |
|
151 |
1 |
} |
|
152 |
|||
153 |
1844 |
inline GeomIndex GeometryModel::getGeometryId(const std::string & name) const |
|
154 |
{ |
||
155 |
#if BOOST_VERSION / 100 % 1000 >= 60 |
||
156 |
using namespace boost::placeholders; |
||
157 |
#endif |
||
158 |
GeometryObjectVector::const_iterator it |
||
159 |
= std::find_if(geometryObjects.begin(), |
||
160 |
geometryObjects.end(), |
||
161 |
✓✗✓✗ ✓✗ |
3688 |
boost::bind(&GeometryObject::name, _1) == name |
162 |
✓✗ | 1844 |
); |
163 |
1844 |
return GeomIndex(it - geometryObjects.begin()); |
|
164 |
} |
||
165 |
|||
166 |
inline bool GeometryModel::existGeometryName(const std::string & name) const |
||
167 |
{ |
||
168 |
#if BOOST_VERSION / 100 % 1000 >= 60 |
||
169 |
using namespace boost::placeholders; |
||
170 |
#endif |
||
171 |
return std::find_if(geometryObjects.begin(), |
||
172 |
geometryObjects.end(), |
||
173 |
boost::bind(&GeometryObject::name, _1) == name) != geometryObjects.end(); |
||
174 |
} |
||
175 |
|||
176 |
47 |
inline void GeometryData::fillInnerOuterObjectMaps(const GeometryModel & geomModel) |
|
177 |
{ |
||
178 |
47 |
innerObjects.clear(); |
|
179 |
47 |
outerObjects.clear(); |
|
180 |
|||
181 |
✓✓ | 946 |
for( GeomIndex gid = 0; gid<geomModel.geometryObjects.size(); gid++) |
182 |
✓✗✓✗ |
899 |
innerObjects[geomModel.geometryObjects[gid].parentJoint].push_back(gid); |
183 |
|||
184 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
6387 |
BOOST_FOREACH(const CollisionPair & pair, geomModel.collisionPairs) |
185 |
{ |
||
186 |
✓✗✓✗ |
3170 |
outerObjects[geomModel.geometryObjects[pair.first].parentJoint].push_back(pair.second); |
187 |
} |
||
188 |
47 |
} |
|
189 |
|||
190 |
1 |
inline std::ostream & operator<< (std::ostream & os, const GeometryModel & geomModel) |
|
191 |
{ |
||
192 |
1 |
os << "Nb geometry objects = " << geomModel.ngeoms << std::endl; |
|
193 |
|||
194 |
✓✓ | 4 |
for(GeomIndex i=0;i<(GeomIndex)(geomModel.ngeoms);++i) |
195 |
{ |
||
196 |
3 |
os << geomModel.geometryObjects[i] <<std::endl; |
|
197 |
} |
||
198 |
|||
199 |
1 |
return os; |
|
200 |
} |
||
201 |
|||
202 |
1 |
inline std::ostream & operator<< (std::ostream & os, const GeometryData & geomData) |
|
203 |
{ |
||
204 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
205 |
1 |
os << "Number of collision pairs = " << geomData.activeCollisionPairs.size() << std::endl; |
|
206 |
|||
207 |
✓✓ | 4 |
for(PairIndex i=0;i<(PairIndex)(geomData.activeCollisionPairs.size());++i) |
208 |
{ |
||
209 |
✓✗ | 3 |
os << "Pairs " << i << (geomData.activeCollisionPairs[i]?" active":" inactive") << std::endl; |
210 |
} |
||
211 |
#else |
||
212 |
os << "WARNING** Without fcl library, no collision checking or distance computations are possible. Only geometry placements can be computed." << std::endl; |
||
213 |
#endif |
||
214 |
1 |
os << "Number of geometry objects = " << geomData.oMg.size() << std::endl; |
|
215 |
|||
216 |
1 |
return os; |
|
217 |
} |
||
218 |
|||
219 |
3800 |
inline void GeometryModel::addCollisionPair(const CollisionPair & pair) |
|
220 |
{ |
||
221 |
✗✓✗✗ |
3800 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.first < ngeoms, |
222 |
"The input pair.first is larger than the number of geometries contained in the GeometryModel"); |
||
223 |
✗✓✗✗ |
3800 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.second < ngeoms, |
224 |
"The input pair.second is larger than the number of geometries contained in the GeometryModel"); |
||
225 |
✓✗ | 3800 |
if (!existCollisionPair(pair)) { collisionPairs.push_back(pair); } |
226 |
3800 |
} |
|
227 |
|||
228 |
2 |
inline void GeometryModel::setCollisionPairs(const MatrixXb & map, |
|
229 |
const bool upper) |
||
230 |
{ |
||
231 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(map.rows(),(Eigen::DenseIndex)ngeoms, |
232 |
"Input map does not have the correct number of rows."); |
||
233 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(map.cols(),(Eigen::DenseIndex)ngeoms, |
234 |
"Input map does not have the correct number of columns."); |
||
235 |
2 |
removeAllCollisionPairs(); |
|
236 |
✓✓ | 42 |
for(Eigen::DenseIndex i = 0; i < (Eigen::DenseIndex)ngeoms; ++i) |
237 |
{ |
||
238 |
✓✓ | 420 |
for(Eigen::DenseIndex j = i+1; j < (Eigen::DenseIndex)ngeoms; ++j) |
239 |
{ |
||
240 |
✓✓ | 380 |
if(upper) |
241 |
{ |
||
242 |
✓✗ | 190 |
if(map(i,j)) |
243 |
✓✗ | 190 |
collisionPairs.push_back(CollisionPair((std::size_t)i,(std::size_t)j)); |
244 |
} |
||
245 |
else |
||
246 |
{ |
||
247 |
✓✗ | 190 |
if(map(j,i)) |
248 |
✓✗ | 190 |
collisionPairs.push_back(CollisionPair((std::size_t)i,(std::size_t)j)); |
249 |
} |
||
250 |
} |
||
251 |
} |
||
252 |
2 |
} |
|
253 |
|||
254 |
10 |
inline void GeometryModel::addAllCollisionPairs() |
|
255 |
{ |
||
256 |
10 |
removeAllCollisionPairs(); |
|
257 |
✓✓ | 218 |
for (GeomIndex i = 0; i < ngeoms; ++i) |
258 |
{ |
||
259 |
208 |
const JointIndex joint_i = geometryObjects[i].parentJoint; |
|
260 |
✓✓ | 3043 |
for (GeomIndex j = i+1; j < ngeoms; ++j) |
261 |
{ |
||
262 |
2835 |
const JointIndex joint_j = geometryObjects[j].parentJoint; |
|
263 |
✓✓ | 2835 |
if (joint_i != joint_j) |
264 |
✓✗ | 2739 |
addCollisionPair(CollisionPair(i,j)); |
265 |
} |
||
266 |
} |
||
267 |
10 |
} |
|
268 |
|||
269 |
inline void GeometryModel::removeCollisionPair(const CollisionPair & pair) |
||
270 |
{ |
||
271 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.first < ngeoms, |
||
272 |
"The input pair.first is larger than the number of geometries contained in the GeometryModel"); |
||
273 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.second < ngeoms, |
||
274 |
"The input pair.second is larger than the number of geometries contained in the GeometryModel"); |
||
275 |
|||
276 |
CollisionPairVector::iterator it = std::find(collisionPairs.begin(), |
||
277 |
collisionPairs.end(), |
||
278 |
pair); |
||
279 |
if (it != collisionPairs.end()) { collisionPairs.erase(it); } |
||
280 |
} |
||
281 |
|||
282 |
12 |
inline void GeometryModel::removeAllCollisionPairs() { collisionPairs.clear(); } |
|
283 |
|||
284 |
4560 |
inline bool GeometryModel::existCollisionPair(const CollisionPair & pair) const |
|
285 |
{ |
||
286 |
4560 |
return (std::find(collisionPairs.begin(), |
|
287 |
collisionPairs.end(), |
||
288 |
✓✗ | 4560 |
pair) != collisionPairs.end()); |
289 |
} |
||
290 |
|||
291 |
1 |
inline PairIndex GeometryModel::findCollisionPair(const CollisionPair & pair) const |
|
292 |
{ |
||
293 |
CollisionPairVector::const_iterator it = std::find(collisionPairs.begin(), |
||
294 |
collisionPairs.end(), |
||
295 |
✓✗ | 1 |
pair); |
296 |
|||
297 |
✓✗ | 1 |
return (PairIndex) std::distance(collisionPairs.begin(), it); |
298 |
} |
||
299 |
|||
300 |
inline void GeometryData::activateCollisionPair(const PairIndex pair_id) |
||
301 |
{ |
||
302 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair_id < activeCollisionPairs.size(), |
||
303 |
"The input argument pair_id is larger than the number of collision pairs contained in activeCollisionPairs."); |
||
304 |
activeCollisionPairs[pair_id] = true; |
||
305 |
} |
||
306 |
|||
307 |
2 |
inline void GeometryData::activateAllCollisionPairs() |
|
308 |
{ |
||
309 |
2 |
std::fill(activeCollisionPairs.begin(),activeCollisionPairs.end(),true); |
|
310 |
2 |
} |
|
311 |
|||
312 |
2 |
inline void GeometryData::setActiveCollisionPairs(const GeometryModel & geom_model, |
|
313 |
const MatrixXb & map, |
||
314 |
const bool upper) |
||
315 |
{ |
||
316 |
2 |
const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms; |
|
317 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(map.rows(),ngeoms,"Input map does not have the correct number of rows."); |
318 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(map.cols(),ngeoms,"Input map does not have the correct number of columns."); |
319 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(geom_model.collisionPairs.size(),activeCollisionPairs.size(),"Current geometry data and the input geometry model are not conistent."); |
320 |
|||
321 |
✓✓ | 382 |
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k) |
322 |
{ |
||
323 |
380 |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
|
324 |
|||
325 |
Eigen::DenseIndex i,j; |
||
326 |
✓✓ | 380 |
if(upper) |
327 |
{ |
||
328 |
190 |
j = (Eigen::DenseIndex)std::max(cp.first,cp.second); |
|
329 |
190 |
i = (Eigen::DenseIndex)std::min(cp.first,cp.second); |
|
330 |
} |
||
331 |
else |
||
332 |
{ |
||
333 |
190 |
i = (Eigen::DenseIndex)std::max(cp.first,cp.second); |
|
334 |
190 |
j = (Eigen::DenseIndex)std::min(cp.first,cp.second); |
|
335 |
} |
||
336 |
|||
337 |
380 |
activeCollisionPairs[k] = map(i,j); |
|
338 |
} |
||
339 |
2 |
} |
|
340 |
|||
341 |
2 |
inline void GeometryData::setGeometryCollisionStatus(const GeometryModel & geom_model, |
|
342 |
const GeomIndex geom_id, |
||
343 |
const bool enable_collision) |
||
344 |
{ |
||
345 |
✗✓✗✗ |
2 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(geom_id < geom_model.ngeoms, |
346 |
"The index of the geometry is not valid"); |
||
347 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(geom_model.collisionPairs.size(),activeCollisionPairs.size(), |
348 |
"Current geometry data and the input geometry model are not conistent."); |
||
349 |
|||
350 |
✓✓ | 382 |
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k) |
351 |
{ |
||
352 |
380 |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
|
353 |
✓✓✗✓ |
380 |
if(cp.first == geom_id || cp.second == geom_id) |
354 |
{ |
||
355 |
38 |
activeCollisionPairs[k] = enable_collision; |
|
356 |
} |
||
357 |
} |
||
358 |
|||
359 |
2 |
} |
|
360 |
|||
361 |
#ifdef PINOCCHIO_WITH_HPP_FCL |
||
362 |
2 |
inline void GeometryData::setSecurityMargins(const GeometryModel & geom_model, |
|
363 |
const MatrixXs & security_margin_map, |
||
364 |
const bool upper) |
||
365 |
{ |
||
366 |
2 |
const Eigen::DenseIndex ngeoms = (Eigen::DenseIndex)geom_model.ngeoms; |
|
367 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(security_margin_map.rows(),ngeoms,"Input map does not have the correct number of rows."); |
368 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(security_margin_map.cols(),ngeoms,"Input map does not have the correct number of columns."); |
369 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(geom_model.collisionPairs.size(),collisionRequests.size(),"Current geometry data and the input geometry model are not conistent."); |
370 |
|||
371 |
✓✓ | 382 |
for(size_t k = 0; k < geom_model.collisionPairs.size(); ++k) |
372 |
{ |
||
373 |
380 |
const CollisionPair & cp = geom_model.collisionPairs[k]; |
|
374 |
|||
375 |
Eigen::DenseIndex i,j; |
||
376 |
✓✓ | 380 |
if(upper) |
377 |
{ |
||
378 |
190 |
j = (Eigen::DenseIndex)std::max(cp.first,cp.second); |
|
379 |
190 |
i = (Eigen::DenseIndex)std::min(cp.first,cp.second); |
|
380 |
} |
||
381 |
else |
||
382 |
{ |
||
383 |
190 |
i = (Eigen::DenseIndex)std::max(cp.first,cp.second); |
|
384 |
190 |
j = (Eigen::DenseIndex)std::min(cp.first,cp.second); |
|
385 |
} |
||
386 |
|||
387 |
380 |
collisionRequests[k].security_margin = security_margin_map(i,j); |
|
388 |
} |
||
389 |
2 |
} |
|
390 |
#endif // ifdef PINOCCHIO_WITH_HPP_FCL |
||
391 |
|||
392 |
inline void GeometryData::deactivateCollisionPair(const PairIndex pair_id) |
||
393 |
{ |
||
394 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair_id < activeCollisionPairs.size(), |
||
395 |
"The input argument pair_id is larger than the number of collision pairs contained in activeCollisionPairs."); |
||
396 |
activeCollisionPairs[pair_id] = false; |
||
397 |
} |
||
398 |
|||
399 |
4 |
inline void GeometryData::deactivateAllCollisionPairs() |
|
400 |
{ |
||
401 |
4 |
std::fill(activeCollisionPairs.begin(),activeCollisionPairs.end(),false); |
|
402 |
4 |
} |
|
403 |
|||
404 |
} // namespace pinocchio |
||
405 |
|||
406 |
/// @endcond |
||
407 |
|||
408 |
#endif // ifndef __pinocchio_multibody_geometry_hxx__ |
Generated by: GCOVR (Version 4.2) |