GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/geometry.hxx Lines: 174 189 92.1 %
Date: 2024-01-23 21:41:47 Branches: 161 520 31.0 %

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__