GCC Code Coverage Report

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