GCC Code Coverage Report

Directory: ./
File: src/obstacle-user.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 30 123 24.4%
Branches: 31 226 13.7%
Line Branch Exec Source
1 // Copyright (c) 2019 CNRS
2 // Authors: Joseph Mirabel
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <coal/collision.h>
30
31 #include <hpp/core/obstacle-user.hh>
32 #include <hpp/pinocchio/body.hh>
33 #include <hpp/pinocchio/collision-object.hh>
34 #include <hpp/pinocchio/device.hh>
35 #include <hpp/util/exception-factory.hh>
36 #include <pinocchio/multibody/geometry.hpp>
37 #include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>
38
39 namespace hpp {
40 namespace core {
41 using ::pinocchio::toFclTransform3f;
42 49647 bool ObstacleUser::collide(const CollisionPairs_t& pairs,
43 CollisionRequests_t& reqs,
44 coal::CollisionResult& res, std::size_t& i,
45 pinocchio::DeviceData& data) {
46
2/2
✓ Branch 1 taken 185552 times.
✓ Branch 2 taken 49540 times.
 235123 for (i = 0; i < pairs.size(); ++i) {
47 185552 res.clear();
48
2/2
✓ Branch 3 taken 109 times.
✓ Branch 4 taken 185476 times.
 185869 if (pairs[i].collide(data, reqs[i], res) != 0) return true;
49 }
50 49540 return false;
51 }
52
53 10 void ObstacleUser::addObstacle(const CollisionObjectConstPtr_t& object) {
54
3/4
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 22 times.
✓ Branch 5 taken 10 times.
 32 for (size_type j = 0; j < robot_->nbJoints(); ++j) {
55
1/2
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
 22 JointPtr_t joint = robot_->jointAt(j);
56
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 addObstacleToJoint(object, joint, false);
57 22 }
58 10 }
59
60 22 void ObstacleUser::addObstacleToJoint(const CollisionObjectConstPtr_t& object,
61 const JointPtr_t& joint,
62 const bool includeChildren) {
63
1/2
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
 22 BodyPtr_t body = joint->linkedBody();
64
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 if (body) {
65
3/4
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 22 times.
✓ Branch 5 taken 22 times.
 44 for (size_type o = 0; o < body->nbInnerObjects(); ++o) {
66 // TODO: check the objects are not in same joint
67
3/6
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 22 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 22 times.
✗ Branch 11 not taken.
 22 cPairs_.push_back(CollisionPair_t(body->innerObjectAt(o), object));
68
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 cRequests_.push_back(defaultRequest_);
69 }
70 }
71
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 22 times.
 22 if (includeChildren) {
72 ✗ for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
73 ✗ addObstacleToJoint(object, joint->childJoint(i), includeChildren);
74 }
75 }
76 22 }
77
78 struct CollisionPairComparision {
79 CollisionPair_t a;
80 ✗ CollisionPairComparision(const CollisionPair_t& p) : a(p) {}
81 ✗ bool operator()(const CollisionPair_t& b) {
82 ✗ return (&(a.first->pinocchio()) == &(b.first->pinocchio())) &&
83 ✗ (&(a.second->pinocchio()) == &(b.second->pinocchio()));
84 }
85 };
86
87 ✗ void ObstacleUser::removeObstacleFromJoint(
88 const JointPtr_t& joint, const CollisionObjectConstPtr_t& obstacle) {
89 ✗ BodyPtr_t body = joint->linkedBody();
90 ✗ if (body) {
91 ✗ for (size_type o = 0; o < body->nbInnerObjects(); ++o) {
92 ✗ CollisionPair_t colPair(body->innerObjectAt(o), obstacle);
93 ✗ CollisionPairComparision compare(colPair);
94 ✗ std::size_t nbDelPairs = 0;
95 ✗ for (std::size_t i = 0; i < cPairs_.size();) {
96 ✗ if (compare(cPairs_[i])) {
97 ✗ cPairs_.erase(cPairs_.begin() + i);
98 ✗ cRequests_.erase(cRequests_.begin() + i);
99 ✗ ++nbDelPairs;
100 } else
101 ✗ ++i;
102 }
103 ✗ if (nbDelPairs == 0) {
104 ✗ std::ostringstream oss;
105 oss << "ObstacleUser::removeObstacleFromJoint: obstacle \""
106 ✗ << obstacle->name()
107 ✗ << "\" is not registered as obstacle for joint \"" << joint->name()
108 ✗ << "\".";
109 ✗ throw std::runtime_error(oss.str());
110 ✗ } else if (nbDelPairs >= 2) {
111 hppDout(error, "obstacle " << obstacle->name() << " was registered "
112 << nbDelPairs
113 << " times as obstacle for joint "
114 << joint->name() << ".");
115 }
116 }
117 }
118 }
119
120 ✗ void ObstacleUser::filterCollisionPairs(
121 const RelativeMotion::matrix_type& matrix) {
122 // Loop over collision pairs and remove disabled ones.
123 pinocchio::JointIndex j1, j2;
124 ✗ coal::CollisionResult unused;
125 ✗ for (std::size_t i = 0; i < cPairs_.size();) {
126 ✗ const CollisionPair_t& pair = cPairs_[i];
127
128 ✗ j1 = pair.first->jointIndex();
129 ✗ j2 = pair.second->jointIndex();
130
131 ✗ switch (matrix(j1, j2)) {
132 ✗ case RelativeMotion::Parameterized:
133 hppDout(info, "Parameterized collision pairs between "
134 << pair.first->name() << " and "
135 << pair.second->name());
136 ✗ pPairs_.push_back(pair);
137 ✗ pRequests_.push_back(cRequests_[i]);
138 ✗ cPairs_.erase(cPairs_.begin() + i);
139 ✗ cRequests_.erase(cRequests_.begin() + i);
140 ✗ break;
141 ✗ case RelativeMotion::Constrained:
142 hppDout(info, "Disabling collision between " << pair.first->name()
143 << " and "
144 << pair.second->name());
145 ✗ if (pair.collide(cRequests_[i], unused) != 0) {
146 hppDout(warning,
147 "Disabling collision detection between two "
148 "bodies in collision.");
149 }
150 ✗ dPairs_.push_back(pair);
151 ✗ dRequests_.push_back(cRequests_[i]);
152 ✗ cPairs_.erase(cPairs_.begin() + i);
153 ✗ cRequests_.erase(cRequests_.begin() + i);
154 ✗ break;
155 ✗ case RelativeMotion::Unconstrained:
156 ✗ ++i;
157 ✗ break;
158 ✗ default:
159 hppDout(warning, "RelativeMotionType not understood");
160 ✗ ++i;
161 ✗ break;
162 }
163 }
164 }
165
166 ✗ void ObstacleUser::setSecurityMargins(const matrix_t& securityMatrix) {
167 ✗ if (securityMatrix.rows() != robot_->nbJoints() + 1 ||
168 ✗ securityMatrix.cols() != robot_->nbJoints() + 1) {
169 ✗ HPP_THROW(std::invalid_argument,
170 "Wrong size of security margin matrix."
171 " Expected "
172 << robot_->nbJoints() + 1 << 'x' << robot_->nbJoints() + 1
173 << ". Got " << securityMatrix.rows() << 'x'
174 << securityMatrix.cols());
175 }
176 pinocchio::JointIndex j1, j2;
177 ✗ coal::CollisionResult unused;
178 ✗ for (std::size_t i = 0; i < cPairs_.size(); ++i) {
179 ✗ const CollisionPair_t& pair = cPairs_[i];
180 ✗ j1 = pair.first->jointIndex();
181 ✗ j2 = pair.second->jointIndex();
182 ✗ cRequests_[i].security_margin = securityMatrix(j1, j2);
183 }
184 ✗ for (std::size_t i = 0; i < pPairs_.size(); ++i) {
185 ✗ const CollisionPair_t& pair = pPairs_[i];
186 ✗ j1 = pair.first->jointIndex();
187 ✗ j2 = pair.second->jointIndex();
188 ✗ pRequests_[i].security_margin = securityMatrix(j1, j2);
189 }
190 ✗ for (std::size_t i = 0; i < dPairs_.size(); ++i) {
191 ✗ const CollisionPair_t& pair = dPairs_[i];
192 ✗ j1 = pair.first->jointIndex();
193 ✗ j2 = pair.second->jointIndex();
194 ✗ dRequests_[i].security_margin = securityMatrix(j1, j2);
195 }
196 }
197
198 ✗ void ObstacleUser::setSecurityMarginBetweenBodies(const std::string& body_a,
199 const std::string& body_b,
200 const value_type& margin) {
201 ✗ auto setMargin = [&body_a, &body_b, &margin](const CollisionPairs_t& pairs,
202 ✗ CollisionRequests_t& requests) {
203 ✗ for (std::size_t i = 0; i < pairs.size(); ++i) {
204 ✗ const CollisionPair_t& pair = pairs[i];
205 ✗ if ((pair.first->name() == body_a && pair.second->name() == body_b) ||
206 ✗ (pair.first->name() == body_b && pair.second->name() == body_a)) {
207 ✗ requests[i].security_margin = margin;
208 ✗ return true;
209 }
210 }
211 ✗ return false;
212 ✗ };
213 ✗ if (setMargin(cPairs_, cRequests_) || setMargin(pPairs_, pRequests_) ||
214 ✗ setMargin(dPairs_, dRequests_))
215 ✗ return;
216 ✗ throw std::invalid_argument(
217 "Could not find a collision pair between body"
218 ✗ " " +
219 ✗ body_a + " and " + body_b);
220 }
221
222 75 void ObstacleUser::addRobotCollisionPairs() {
223 75 const pinocchio::GeomModel& model = robot_->geomModel();
224 75 const pinocchio::GeomData& data = robot_->geomData();
225
226
2/2
✓ Branch 1 taken 5192 times.
✓ Branch 2 taken 75 times.
 5267 for (std::size_t i = 0; i < model.collisionPairs.size(); ++i)
227
1/2
✓ Branch 1 taken 5192 times.
✗ Branch 2 not taken.
 5192 if (data.activeCollisionPairs[i]) {
228 CollisionObjectConstPtr_t o1(new pinocchio::CollisionObject(
229
3/6
✓ Branch 3 taken 5192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5192 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5192 times.
✗ Branch 10 not taken.
 5192 robot_, model.collisionPairs[i].first));
230 CollisionObjectConstPtr_t o2(new pinocchio::CollisionObject(
231
3/6
✓ Branch 3 taken 5192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5192 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5192 times.
✗ Branch 10 not taken.
 5192 robot_, model.collisionPairs[i].second));
232
2/4
✓ Branch 3 taken 5192 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5192 times.
✗ Branch 7 not taken.
 5192 cPairs_.push_back(CollisionPair_t(o1, o2));
233
1/2
✓ Branch 1 taken 5192 times.
✗ Branch 2 not taken.
 5192 cRequests_.push_back(defaultRequest_);
234 5192 }
235 75 }
236
237 ✗ void ObstacleUser::setRequests(const coal::CollisionRequest& r) {
238 ✗ for (std::size_t i = 0; i < cRequests_.size(); ++i) cRequests_[i] = r;
239 ✗ for (std::size_t i = 0; i < pRequests_.size(); ++i) pRequests_[i] = r;
240 ✗ for (std::size_t i = 0; i < dRequests_.size(); ++i) dRequests_[i] = r;
241 }
242 } // namespace core
243 } // namespace hpp
244