Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_collision_distance_hxx__ |
6 |
|
|
#define __pinocchio_collision_distance_hxx__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/collision/distance.hpp" |
9 |
|
|
#include "pinocchio/algorithm/geometry.hpp" |
10 |
|
|
#include "pinocchio/multibody/geometry.hpp" |
11 |
|
|
|
12 |
|
|
namespace pinocchio |
13 |
|
|
{ |
14 |
|
|
|
15 |
|
|
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> |
16 |
|
|
inline std::size_t computeDistances( |
17 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
18 |
|
|
const DataTpl<Scalar, Options, JointCollectionTpl> & data, |
19 |
|
|
const GeometryModel & geom_model, |
20 |
|
|
GeometryData & geom_data) |
21 |
|
|
{ |
22 |
|
|
assert(model.check(data) && "data is not consistent with model."); |
23 |
|
|
updateGeometryPlacements(model, data, geom_model, geom_data); |
24 |
|
|
return computeDistances(geom_model, geom_data); |
25 |
|
|
} |
26 |
|
|
|
27 |
|
|
template< |
28 |
|
|
typename Scalar, |
29 |
|
|
int Options, |
30 |
|
|
template<typename, int> |
31 |
|
|
class JointCollectionTpl, |
32 |
|
|
typename ConfigVectorType> |
33 |
|
1 |
inline std::size_t computeDistances( |
34 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
35 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & data, |
36 |
|
|
const GeometryModel & geom_model, |
37 |
|
|
GeometryData & geom_data, |
38 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q) |
39 |
|
|
{ |
40 |
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 |
assert(model.check(data) && "data is not consistent with model."); |
41 |
|
1 |
updateGeometryPlacements(model, data, geom_model, geom_data, q); |
42 |
|
1 |
return computeDistances(geom_model, geom_data); |
43 |
|
|
} |
44 |
|
|
|
45 |
|
✗ |
inline fcl::DistanceResult & computeDistance( |
46 |
|
|
const GeometryModel & geom_model, GeometryData & geom_data, const PairIndex pair_id) |
47 |
|
|
{ |
48 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair_id < geom_model.collisionPairs.size()); |
49 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
50 |
|
|
geom_model.collisionPairs.size() == geom_data.collisionResults.size()); |
51 |
|
✗ |
const CollisionPair & pair = geom_model.collisionPairs[pair_id]; |
52 |
|
|
|
53 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.first < geom_model.ngeoms); |
54 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.second < geom_model.ngeoms); |
55 |
|
|
|
56 |
|
✗ |
fcl::DistanceRequest & distance_request = geom_data.distanceRequests[pair_id]; |
57 |
|
✗ |
fcl::DistanceResult & distance_result = geom_data.distanceResults[pair_id]; |
58 |
|
✗ |
distance_result.clear(); |
59 |
|
|
|
60 |
|
✗ |
fcl::Transform3f oM1(toFclTransform3f(geom_data.oMg[pair.first])), |
61 |
|
✗ |
oM2(toFclTransform3f(geom_data.oMg[pair.second])); |
62 |
|
|
|
63 |
|
|
try |
64 |
|
|
{ |
65 |
|
✗ |
GeometryData::ComputeDistance & do_computations = geom_data.distance_functors[pair_id]; |
66 |
|
✗ |
do_computations(oM1, oM2, distance_request, distance_result); |
67 |
|
|
} |
68 |
|
✗ |
catch (std::invalid_argument & e) |
69 |
|
|
{ |
70 |
|
✗ |
std::stringstream ss; |
71 |
|
✗ |
ss << "Problem when trying to compute the distance of collision pair #" << pair_id << " (" |
72 |
|
✗ |
<< pair.first << "," << pair.second << ")" << std::endl; |
73 |
|
✗ |
ss << "hpp-fcl original error:\n" << e.what() << std::endl; |
74 |
|
✗ |
throw std::invalid_argument(ss.str()); |
75 |
|
|
} |
76 |
|
|
|
77 |
|
✗ |
return geom_data.distanceResults[pair_id]; |
78 |
|
|
} |
79 |
|
|
|
80 |
|
✗ |
inline std::size_t computeDistances(const GeometryModel & geom_model, GeometryData & geom_data) |
81 |
|
|
{ |
82 |
|
✗ |
std::size_t min_index = geom_model.collisionPairs.size(); |
83 |
|
✗ |
double min_dist = std::numeric_limits<double>::infinity(); |
84 |
|
|
|
85 |
|
✗ |
for (std::size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index) |
86 |
|
|
{ |
87 |
|
✗ |
const CollisionPair & cp = geom_model.collisionPairs[cp_index]; |
88 |
|
|
|
89 |
|
✗ |
if ( |
90 |
|
✗ |
geom_data.activeCollisionPairs[cp_index] |
91 |
|
✗ |
&& !( |
92 |
|
✗ |
geom_model.geometryObjects[cp.first].disableCollision |
93 |
|
✗ |
|| geom_model.geometryObjects[cp.second].disableCollision)) |
94 |
|
|
{ |
95 |
|
✗ |
computeDistance(geom_model, geom_data, cp_index); |
96 |
|
✗ |
if (geom_data.distanceResults[cp_index].min_distance < min_dist) |
97 |
|
|
{ |
98 |
|
✗ |
min_index = cp_index; |
99 |
|
✗ |
min_dist = geom_data.distanceResults[cp_index].min_distance; |
100 |
|
|
} |
101 |
|
|
} |
102 |
|
|
} |
103 |
|
|
|
104 |
|
✗ |
return min_index; |
105 |
|
|
} |
106 |
|
|
|
107 |
|
|
} // namespace pinocchio |
108 |
|
|
|
109 |
|
|
#endif // ifndef __pinocchio_collision_distance_hxx__ |
110 |
|
|
|