GCC Code Coverage Report

Directory: ./
File: include/pinocchio/collision/collision.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 64 69 92.8%
Branches: 90 281 32.0%
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 152721 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 152721 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 152721 PINOCCHIO_CHECK_INPUT_ARGUMENT(
25 geom_model.collisionPairs.size() == geom_data.collisionResults.size());
26
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 152721 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 152721 PINOCCHIO_CHECK_INPUT_ARGUMENT(pair_id < geom_model.collisionPairs.size());
27
28 152721 const CollisionPair & pair = geom_model.collisionPairs[pair_id];
29
30
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 152721 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 152721 PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.first < geom_model.ngeoms);
31
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 152721 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 152721 PINOCCHIO_CHECK_INPUT_ARGUMENT(pair.second < geom_model.ngeoms);
32
33 152721 collision_request.distance_upper_bound =
34 152721 collision_request.security_margin + 1e-6; // TODO: change the margin
35
36 152721 fcl::CollisionResult & collision_result = geom_data.collisionResults[pair_id];
37
1/2
✓ Branch 1 taken 152721 times.
✗ Branch 2 not taken.
 152721 collision_result.clear();
38
39
1/2
✓ Branch 2 taken 152721 times.
✗ Branch 3 not taken.
 152721 fcl::Transform3f oM1(toFclTransform3f(geom_data.oMg[pair.first])),
40
1/2
✓ Branch 2 taken 152721 times.
✗ Branch 3 not taken.
 152721 oM2(toFclTransform3f(geom_data.oMg[pair.second]));
41
42 try
43 {
44 152721 GeometryData::ComputeCollision & do_computations = geom_data.collision_functors[pair_id];
45
1/2
✓ Branch 1 taken 152721 times.
✗ Branch 2 not taken.
 152721 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 305442 return collision_result.isCollision();
67 }
68
69 142411 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 142411 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 142411 PINOCCHIO_CHECK_INPUT_ARGUMENT(pair_id < geom_model.collisionPairs.size());
73 142411 fcl::CollisionRequest & collision_request = geom_data.collisionRequests[pair_id];
74
75 142411 return computeCollision(geom_model, geom_data, pair_id, collision_request);
76 }
77
78 2010 inline bool computeCollisions(
79 const GeometryModel & geom_model, GeometryData & geom_data, const bool stopAtFirstCollision)
80 {
81 2010 bool isColliding = false;
82
83
2/2
✓ Branch 1 taken 142334 times.
✓ Branch 2 taken 2010 times.
 144344 for (std::size_t cp_index = 0; cp_index < geom_model.collisionPairs.size(); ++cp_index)
84 {
85 142334 const CollisionPair & cp = geom_model.collisionPairs[cp_index];
86
87 142334 if (
88
1/2
✓ Branch 1 taken 142334 times.
✗ Branch 2 not taken.
 142334 geom_data.activeCollisionPairs[cp_index]
89
2/4
✓ Branch 0 taken 142334 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 142334 times.
✗ Branch 3 not taken.
 284668 && !(
90
1/2
✓ Branch 1 taken 142334 times.
✗ Branch 2 not taken.
 142334 geom_model.geometryObjects[cp.first].disableCollision
91
1/2
✓ Branch 2 taken 142334 times.
✗ Branch 3 not taken.
 284668 || geom_model.geometryObjects[cp.second].disableCollision))
92 {
93 142334 bool res = computeCollision(geom_model, geom_data, cp_index);
94
4/4
✓ Branch 0 taken 126380 times.
✓ Branch 1 taken 15954 times.
✓ Branch 2 taken 330 times.
✓ Branch 3 taken 126050 times.
 142334 if (!isColliding && res)
95 {
96 330 isColliding = true;
97 330 geom_data.collisionPairIndex = cp_index; // first pair to be in collision
98
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 330 times.
 330 if (stopAtFirstCollision)
99 ✗ return true;
100 }
101 }
102 }
103
104 2010 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> class JointCollectionTpl,
116 typename ConfigVectorType>
117 2004 inline bool computeCollisions(
118 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
119 DataTpl<Scalar, Options, JointCollectionTpl> & data,
120 const GeometryModel & geom_model,
121 GeometryData & geom_data,
122 const Eigen::MatrixBase<ConfigVectorType> & q,
123 const bool stopAtFirstCollision)
124 {
125 2004 updateGeometryPlacements(model, data, geom_model, geom_data, q);
126 2004 return computeCollisions(geom_model, geom_data, stopAtFirstCollision);
127 }
128
129 /* --- RADIUS -------------------------------------------------------------------- */
130 /* --- RADIUS -------------------------------------------------------------------- */
131 /* --- RADIUS -------------------------------------------------------------------- */
132
133 /// Given p1..3 being either "min" or "max", return one of the corners of the
134 /// AABB cub of the FCL object.
135 #define PINOCCHIO_GEOM_AABB(FCL, p1, p2, p3) \
136 SE3::Vector3(FCL->aabb_local.p1##_[0], FCL->aabb_local.p2##_[1], FCL->aabb_local.p3##_[2])
137
138 /// For all bodies of the model, compute the point of the geometry model
139 /// that is the further from the center of the joint. This quantity is used
140 /// in some continuous collision test.
141 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
142 1 inline void computeBodyRadius(
143 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
144 const GeometryModel & geom_model,
145 GeometryData & geom_data)
146 {
147
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 geom_data.radius.resize(model.joints.size(), 0);
148
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)
149 {
150 20 const GeometryObject::CollisionGeometryPtr & geometry = geom_object.geometry;
151
152 // Force computation of the Local AABB
153 // TODO: change for a more elegant solution
154
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 const_cast<hpp::fcl::CollisionGeometry &>(*geometry).computeLocalAABB();
155
156 20 const GeometryModel::SE3 & jMb = geom_object.placement; // placement in joint.
157 20 const Model::JointIndex i = geom_object.parentJoint;
158
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
 20 assert(i < geom_data.radius.size());
159
160 20 Scalar radius = geom_data.radius[i] * geom_data.radius[i];
161
162 // The radius is simply the one of the 8 corners of the AABB cube, expressed
163 // in the joint frame, whose norm is the highest.
164 20 radius =
165
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);
166 20 radius =
167
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);
168 20 radius =
169
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);
170 20 radius =
171
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);
172 20 radius =
173
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);
174 20 radius =
175
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);
176 20 radius =
177
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);
178 20 radius =
179
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);
180
181 // Don't forget to sqroot the squared norm before storing it.
182 20 geom_data.radius[i] = sqrt(radius);
183 }
184 1 }
185
186 #undef PINOCCHIO_GEOM_AABB
187
188 } // namespace pinocchio
189
190 #endif // ifndef __pinocchio_collision_collision_hxx__
191