Directory: | ./ |
---|---|
File: | include/pinocchio/collision/collision.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 61 | 69 | 88.4% |
Branches: | 86 | 279 | 30.8% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_collision_collision_hxx__ | ||
6 | #define __pinocchio_collision_collision_hxx__ | ||
7 | |||
8 | #include "pinocchio/multibody/model.hpp" | ||
9 | #include "pinocchio/multibody/data.hpp" | ||
10 | |||
11 | #include "pinocchio/multibody/geometry.hpp" | ||
12 | #include "pinocchio/algorithm/geometry.hpp" | ||
13 | #include "pinocchio/collision/distance.hpp" | ||
14 | |||
15 | namespace pinocchio | ||
16 | { | ||
17 | |||
18 | 71 | inline bool computeCollision( | |
19 | const GeometryModel & geom_model, | ||
20 | GeometryData & geom_data, | ||
21 | const PairIndex pair_id, | ||
22 | fcl::CollisionRequest & collision_request) | ||
23 | { | ||
24 |
1/4✗ Branch 2 not taken.
✓ Branch 3 taken 71 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
71 | PINOCCHIO_CHECK_INPUT_ARGUMENT( |
25 | geom_model.collisionPairs.size() == geom_data.collisionResults.size()); | ||
26 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 71 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
71 | PINOCCHIO_CHECK_INPUT_ARGUMENT(pair_id < geom_model.collisionPairs.size()); |
27 | |||
28 | 71 | const CollisionPair & pair = geom_model.collisionPairs[pair_id]; | |
29 | |||
30 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 71 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
71 | PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.first < geom_model.ngeoms); |
31 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 71 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
71 | PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.second < geom_model.ngeoms); |
32 | |||
33 | 71 | collision_request.distance_upper_bound = | |
34 | 71 | collision_request.security_margin + 1e-6; // TODO: change the margin | |
35 | |||
36 | 71 | fcl::CollisionResult & collision_result = geom_data.collisionResults[pair_id]; | |
37 | 71 | collision_result.clear(); | |
38 | |||
39 |
1/2✓ Branch 2 taken 71 times.
✗ Branch 3 not taken.
|
71 | fcl::Transform3f oM1(toFclTransform3f(geom_data.oMg[pair.first])), |
40 |
1/2✓ Branch 2 taken 71 times.
✗ Branch 3 not taken.
|
71 | oM2(toFclTransform3f(geom_data.oMg[pair.second])); |
41 | |||
42 | try | ||
43 | { | ||
44 | 71 | GeometryData::ComputeCollision & do_computations = geom_data.collision_functors[pair_id]; | |
45 |
1/2✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
|
71 | do_computations(oM1, oM2, collision_request, collision_result); |
46 | } | ||
47 | ✗ | catch (std::invalid_argument & e) | |
48 | { | ||
49 | ✗ | PINOCCHIO_THROW_PRETTY( | |
50 | std::invalid_argument, "Problem when trying to check the collision of collision pair #" | ||
51 | << pair_id << " (" << pair.first << "," << pair.second << ")" | ||
52 | << std::endl | ||
53 | << "hpp-fcl original error:\n" | ||
54 | << e.what() << std::endl); | ||
55 | } | ||
56 | ✗ | catch (std::logic_error & e) | |
57 | { | ||
58 | ✗ | PINOCCHIO_THROW_PRETTY( | |
59 | std::logic_error, "Problem when trying to check the collision of collision pair #" | ||
60 | << pair_id << " (" << pair.first << "," << pair.second << ")" | ||
61 | << std::endl | ||
62 | << "hpp-fcl original error:\n" | ||
63 | << e.what() << std::endl); | ||
64 | } | ||
65 | |||
66 | 142 | return collision_result.isCollision(); | |
67 | } | ||
68 | |||
69 | 71 | inline bool computeCollision( | |
70 | const GeometryModel & geom_model, GeometryData & geom_data, const PairIndex pair_id) | ||
71 | { | ||
72 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 71 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
71 | PINOCCHIO_CHECK_INPUT_ARGUMENT(pair_id < geom_model.collisionPairs.size()); |
73 | 71 | fcl::CollisionRequest & collision_request = geom_data.collisionRequests[pair_id]; | |
74 | |||
75 | 71 | return computeCollision(geom_model, geom_data, pair_id, collision_request); | |
76 | } | ||
77 | |||
78 | 3 | inline bool computeCollisions( | |
79 | const GeometryModel & geom_model, GeometryData & geom_data, const bool stopAtFirstCollision) | ||
80 | { | ||
81 | 3 | bool isColliding = false; | |
82 | |||
83 |
2/2✓ Branch 1 taken 1844 times.
✓ Branch 2 taken 3 times.
|
1847 | for (std::size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index) |
84 | { | ||
85 | 1844 | const CollisionPair & cp = geom_model.collisionPairs[cp_index]; | |
86 | |||
87 | 1844 | if ( | |
88 |
1/2✓ Branch 1 taken 1844 times.
✗ Branch 2 not taken.
|
1844 | geom_data.activeCollisionPairs[cp_index] |
89 |
2/4✓ Branch 0 taken 1844 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1844 times.
✗ Branch 3 not taken.
|
3688 | && !( |
90 |
1/2✓ Branch 1 taken 1844 times.
✗ Branch 2 not taken.
|
1844 | geom_model.geometryObjects[cp.first].disableCollision |
91 |
1/2✓ Branch 2 taken 1844 times.
✗ Branch 3 not taken.
|
3688 | || geom_model.geometryObjects[cp.second].disableCollision)) |
92 | { | ||
93 | 1844 | bool res = computeCollision(geom_model, geom_data, cp_index); | |
94 |
2/4✓ Branch 0 taken 1844 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 1844 times.
|
1844 | if (!isColliding && res) |
95 | { | ||
96 | ✗ | isColliding = true; | |
97 | ✗ | geom_data.collisionPairIndex = cp_index; // first pair to be in collision | |
98 | ✗ | if (stopAtFirstCollision) | |
99 | ✗ | return true; | |
100 | } | ||
101 | } | ||
102 | } | ||
103 | |||
104 | 3 | return isColliding; | |
105 | } | ||
106 | |||
107 | /* --- COLLISIONS ----------------------------------------------------------------- */ | ||
108 | /* --- COLLISIONS ----------------------------------------------------------------- */ | ||
109 | /* --- COLLISIONS ----------------------------------------------------------------- */ | ||
110 | |||
111 | // WARNING, if stopAtFirstcollision = true, then the collisions vector will not be fulfilled. | ||
112 | template< | ||
113 | typename Scalar, | ||
114 | int Options, | ||
115 | template<typename, int> | ||
116 | class JointCollectionTpl, | ||
117 | typename ConfigVectorType> | ||
118 | 2004 | inline bool computeCollisions( | |
119 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
120 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
121 | const GeometryModel & geom_model, | ||
122 | GeometryData & geom_data, | ||
123 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
124 | const bool stopAtFirstCollision) | ||
125 | { | ||
126 | 2004 | updateGeometryPlacements(model, data, geom_model, geom_data, q); | |
127 | 2004 | return computeCollisions(geom_model, geom_data, stopAtFirstCollision); | |
128 | } | ||
129 | |||
130 | /* --- RADIUS -------------------------------------------------------------------- */ | ||
131 | /* --- RADIUS -------------------------------------------------------------------- */ | ||
132 | /* --- RADIUS -------------------------------------------------------------------- */ | ||
133 | |||
134 | /// Given p1..3 being either "min" or "max", return one of the corners of the | ||
135 | /// AABB cub of the FCL object. | ||
136 | #define PINOCCHIO_GEOM_AABB(FCL, p1, p2, p3) \ | ||
137 | SE3::Vector3(FCL->aabb_local.p1##_[0], FCL->aabb_local.p2##_[1], FCL->aabb_local.p3##_[2]) | ||
138 | |||
139 | /// For all bodies of the model, compute the point of the geometry model | ||
140 | /// that is the further from the center of the joint. This quantity is used | ||
141 | /// in some continuous collision test. | ||
142 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
143 | 1 | inline void computeBodyRadius( | |
144 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
145 | const GeometryModel & geom_model, | ||
146 | GeometryData & geom_data) | ||
147 | { | ||
148 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | geom_data.radius.resize(model.joints.size(), 0); |
149 |
18/30✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 20 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 20 times.
✓ Branch 25 taken 20 times.
✓ Branch 26 taken 20 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 20 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 21 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 21 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 20 times.
✓ Branch 37 taken 1 times.
✓ Branch 38 taken 20 times.
✓ Branch 39 taken 1 times.
|
41 | BOOST_FOREACH (const GeometryObject & geom_object, geom_model.geometryObjects) |
150 | { | ||
151 | 20 | const GeometryObject::CollisionGeometryPtr & geometry = geom_object.geometry; | |
152 | |||
153 | // Force computation of the Local AABB | ||
154 | // TODO: change for a more elegant solution | ||
155 |
1/2✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
|
20 | const_cast<hpp::fcl::CollisionGeometry &>(*geometry).computeLocalAABB(); |
156 | |||
157 | 20 | const GeometryModel::SE3 & jMb = geom_object.placement; // placement in joint. | |
158 | 20 | const Model::JointIndex i = geom_object.parentJoint; | |
159 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
|
20 | assert(i < geom_data.radius.size()); |
160 | |||
161 | 20 | Scalar radius = geom_data.radius[i] * geom_data.radius[i]; | |
162 | |||
163 | // The radius is simply the one of the 8 corners of the AABB cube, expressed | ||
164 | // in the joint frame, whose norm is the highest. | ||
165 | 20 | radius = | |
166 |
6/12✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
|
20 | std::max(jMb.act(PINOCCHIO_GEOM_AABB(geometry, min, min, min)).squaredNorm(), radius); |
167 | 20 | radius = | |
168 |
6/12✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
|
20 | std::max(jMb.act(PINOCCHIO_GEOM_AABB(geometry, min, min, max)).squaredNorm(), radius); |
169 | 20 | radius = | |
170 |
6/12✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
|
20 | std::max(jMb.act(PINOCCHIO_GEOM_AABB(geometry, min, max, min)).squaredNorm(), radius); |
171 | 20 | radius = | |
172 |
6/12✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
|
20 | std::max(jMb.act(PINOCCHIO_GEOM_AABB(geometry, min, max, max)).squaredNorm(), radius); |
173 | 20 | radius = | |
174 |
6/12✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
|
20 | std::max(jMb.act(PINOCCHIO_GEOM_AABB(geometry, max, min, min)).squaredNorm(), radius); |
175 | 20 | radius = | |
176 |
6/12✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
|
20 | std::max(jMb.act(PINOCCHIO_GEOM_AABB(geometry, max, min, max)).squaredNorm(), radius); |
177 | 20 | radius = | |
178 |
6/12✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
|
20 | std::max(jMb.act(PINOCCHIO_GEOM_AABB(geometry, max, max, min)).squaredNorm(), radius); |
179 | 20 | radius = | |
180 |
6/12✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
|
20 | std::max(jMb.act(PINOCCHIO_GEOM_AABB(geometry, max, max, max)).squaredNorm(), radius); |
181 | |||
182 | // Don't forget to sqroot the squared norm before storing it. | ||
183 | 20 | geom_data.radius[i] = sqrt(radius); | |
184 | } | ||
185 | 1 | } | |
186 | |||
187 | #undef PINOCCHIO_GEOM_AABB | ||
188 | |||
189 | } // namespace pinocchio | ||
190 | |||
191 | #endif // ifndef __pinocchio_collision_collision_hxx__ | ||
192 |