| 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 |
|
|
|