GCC Code Coverage Report

Directory: ./
File: src/continuous-validation.cc
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 117 231 50.6%
Branches: 77 341 22.6%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014, 2015, 2016, 2017, 2018 CNRS
3 // Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <hpp/core/collision-path-validation-report.hh>
31 #include <hpp/core/continuous-validation.hh>
32 #include <hpp/core/continuous-validation/solid-solid-collision.hh>
33 #include <hpp/core/path-vector.hh>
34 #include <hpp/core/straight-path.hh>
35 #include <hpp/util/debug.hh>
36 #include <iterator>
37 #include <limits>
38 #include <pinocchio/multibody/geometry.hpp>
39 namespace hpp {
40 namespace core {
41 using continuousValidation::BodyPairCollision;
42 using continuousValidation::BodyPairCollisionPtr_t;
43 using continuousValidation::IntervalValidations_t;
44 using continuousValidation::SolidSolidCollision;
45
46 8 ContinuousValidation::Initialize::Initialize(ContinuousValidation &owner)
47 8 : owner_(&owner) {}
48
49 typedef std::pair<pinocchio::JointIndex, pinocchio::JointIndex>
50 JointIndexPair_t;
51 struct JointIndexPairCompare_t {
52 540 bool operator()(const JointIndexPair_t &p0,
53 const JointIndexPair_t &p1) const {
54
2/2
✓ Branch 0 taken 237 times.
✓ Branch 1 taken 303 times.
 540 if (p0.first < p1.first) return true;
55
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 294 times.
 303 if (p0.first > p1.first) return false;
56 294 return (p0.second < p1.second);
57 }
58 };
59 typedef std::map<JointIndexPair_t, BodyPairCollisionPtr_t,
60 JointIndexPairCompare_t>
61 BodyPairCollisionMap_t;
62
63 8 void ContinuousValidation::Initialize::doExecute() const {
64 8 DevicePtr_t robot = owner().robot();
65 8 const pinocchio::GeomModel &gmodel = robot->geomModel();
66
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 const pinocchio::GeomData &gdata = robot->geomData();
67 8 JointPtr_t joint1, joint2;
68 8 BodyPairCollisionMap_t bodyPairMap;
69
2/2
✓ Branch 1 taken 45 times.
✓ Branch 2 taken 8 times.
 53 for (std::size_t i = 0; i < gmodel.collisionPairs.size(); ++i) {
70
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 45 times.
 45 if (!gdata.activeCollisionPairs[i]) continue;
71
72 45 const ::pinocchio::CollisionPair &cp = gmodel.collisionPairs[i];
73 90 JointIndexPair_t jp(gmodel.geometryObjects[cp.first].parentJoint,
74 45 gmodel.geometryObjects[cp.second].parentJoint);
75
76 // Ignore pairs of bodies that are in the same joint.
77
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 45 times.
 45 if (jp.first == jp.second) continue;
78
79
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 BodyPairCollisionMap_t::iterator _bp = bodyPairMap.find(jp);
80
81
2/2
✓ Branch 2 taken 36 times.
✓ Branch 3 taken 9 times.
 45 if (_bp == bodyPairMap.end()) {
82
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 36 joint1 = Joint::create(robot, jp.first);
83
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 36 joint2 = Joint::create(robot, jp.second);
84
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
 36 if (!joint2) joint2.swap(joint1);
85
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
 36 assert(joint2);
86 continuousValidation::SolidSolidCollisionPtr_t ss(
87
1/2
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 36 SolidSolidCollision::create(joint2, joint1, owner().tolerance_));
88
1/2
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 ss->breakDistance(owner().breakDistance());
89
1/2
✓ Branch 3 taken 36 times.
✗ Branch 4 not taken.
 36 owner().addIntervalValidation(ss);
90
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 bodyPairMap[jp] = ss;
91 36 }
92 CollisionObjectConstPtr_t co1(
93
3/6
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 45 times.
✗ Branch 9 not taken.
 45 new pinocchio::CollisionObject(robot, cp.first));
94 CollisionObjectConstPtr_t co2(
95
3/6
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 45 times.
✗ Branch 9 not taken.
 45 new pinocchio::CollisionObject(robot, cp.second));
96
2/4
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
 45 bodyPairMap[jp]->addCollisionPair(co1, co2);
97 45 }
98 8 }
99
100 8 ContinuousValidation::AddObstacle::AddObstacle(ContinuousValidation &owner)
101 8 : owner_(&owner), robot_(owner.robot()) {}
102
103 5 void ContinuousValidation::AddObstacle::doExecute(
104 const CollisionObjectConstPtr_t &object) const {
105 5 DevicePtr_t robot(robot_.lock());
106
3/4
✓ Branch 2 taken 14 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 9 times.
✓ Branch 5 taken 5 times.
 14 for (size_type idx = 0; idx < robot->nbJoints(); ++idx) {
107
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 JointPtr_t joint = robot->jointAt(idx);
108
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
 9 BodyPtr_t body = joint->linkedBody();
109
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 if (body) {
110 9 ConstObjectStdVector_t objects;
111
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 objects.push_back(object);
112 continuousValidation::SolidSolidCollisionPtr_t ss(
113
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 SolidSolidCollision::create(joint, objects, owner().tolerance()));
114
1/2
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
 9 ss->breakDistance(owner().breakDistance());
115
1/2
✓ Branch 3 taken 9 times.
✗ Branch 4 not taken.
 9 owner().addIntervalValidation(ss);
116 9 }
117 9 }
118 5 }
119
120 /// Validate interval centered on a path parameter
121 /// \param intervalValidations a reference to the pair with smallest interval.
122 /// \param config Configuration at abscissa tmin on the path.
123 /// \param t parameter value in the path interval of definition
124 /// \retval interval interval validated for all validation elements
125 /// \retval report reason why the interval is not valid,
126 /// \return true if the configuration is collision free for this parameter
127 /// value, false otherwise.
128 20378 bool ContinuousValidation::validateConfiguration(
129 IntervalValidations_t &intervalValidations, const Configuration_t &config,
130 const value_type &t, interval_t &interval,
131 PathValidationReportPtr_t &report) {
132 20378 interval.first = -std::numeric_limits<value_type>::infinity();
133 20378 interval.second = std::numeric_limits<value_type>::infinity();
134
1/2
✓ Branch 1 taken 20379 times.
✗ Branch 2 not taken.
 20378 hpp::pinocchio::DeviceSync robot(robot_);
135
2/4
✓ Branch 1 taken 20379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20379 times.
✗ Branch 5 not taken.
 20379 robot.currentConfiguration(config);
136
1/2
✓ Branch 1 taken 20379 times.
✗ Branch 2 not taken.
 20379 robot.computeForwardKinematics(pinocchio::JOINT_POSITION);
137
1/2
✓ Branch 1 taken 20379 times.
✗ Branch 2 not taken.
 20379 robot.updateGeometryPlacements();
138 20379 IntervalValidations_t::iterator smallestInterval(intervalValidations.begin());
139
4/6
✓ Branch 1 taken 20379 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20378 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 95 times.
✓ Branch 7 taken 20283 times.
 20379 if (!validateIntervals(intervalValidations, t, interval, report,
140 smallestInterval, robot.d()))
141 95 return false;
142 // Put the smallest interval first so that, at next iteration,
143 // collision pairs with large interval are not computed.
144
4/4
✓ Branch 1 taken 19985 times.
✓ Branch 2 taken 298 times.
✓ Branch 3 taken 2883 times.
✓ Branch 4 taken 17102 times.
 40268 if (intervalValidations.size() > 1 &&
145
2/2
✓ Branch 2 taken 2883 times.
✓ Branch 3 taken 17400 times.
 40268 smallestInterval != intervalValidations.begin())
146 2883 std::iter_swap(intervalValidations.begin(), smallestInterval);
147 20283 return true;
148 20378 }
149
150 621 bool ContinuousValidation::validate(const PathPtr_t &path, bool reverse,
151 PathPtr_t &validPart,
152 PathValidationReportPtr_t &report) {
153
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 621 times.
 621 if (PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST(PathVector, path)) {
154 PathVectorPtr_t validPathVector =
155 ✗ PathVector::create(path->outputSize(), path->outputDerivativeSize());
156 ✗ validPart = validPathVector;
157 ✗ PathPtr_t localValidPart;
158 ✗ if (reverse) {
159 ✗ value_type param = path->length();
160 ✗ std::deque<PathPtr_t> paths;
161 ✗ for (std::size_t i = pv->numberPaths() + 1; i != 0; --i) {
162 ✗ PathPtr_t localPath(pv->pathAtRank(i - 1));
163 ✗ if (validate(localPath, reverse, localValidPart, report)) {
164 ✗ paths.push_front(localPath->copy());
165 ✗ param -= localPath->length();
166 } else {
167 ✗ report->parameter += param - localPath->length();
168 ✗ paths.push_front(localValidPart->copy());
169 ✗ for (std::deque<PathPtr_t>::const_iterator it = paths.begin();
170 ✗ it != paths.end(); ++it) {
171 ✗ validPathVector->appendPath(*it);
172 }
173 ✗ return false;
174 }
175 }
176 ✗ return true;
177 ✗ } else {
178 ✗ value_type param = 0;
179 ✗ for (std::size_t i = 0; i < pv->numberPaths(); ++i) {
180 ✗ PathPtr_t localPath(pv->pathAtRank(i));
181 ✗ if (validate(localPath, reverse, localValidPart, report)) {
182 ✗ validPathVector->appendPath(localPath->copy());
183 ✗ param += localPath->length();
184 } else {
185 ✗ report->parameter += param;
186 ✗ validPathVector->appendPath(localValidPart->copy());
187 ✗ return false;
188 }
189 }
190 ✗ return true;
191 }
192
1/2
✓ Branch 3 taken 621 times.
✗ Branch 4 not taken.
 621 }
193 // Copy list of BodyPairCollision instances in a pool for thread safety.
194 IntervalValidations_t *bpc;
195
2/2
✓ Branch 1 taken 17 times.
✓ Branch 2 taken 604 times.
 621 if (!bodyPairCollisionPool_.available()) {
196 // Add an element
197
2/4
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 17 times.
✗ Branch 7 not taken.
 17 bpc = new IntervalValidations_t(intervalValidations_.size());
198
2/2
✓ Branch 1 taken 153 times.
✓ Branch 2 taken 17 times.
 170 for (std::size_t i = 0; i < bpc->size(); ++i)
199 153 (*bpc)[i] = intervalValidations_[i]->copy();
200 17 bodyPairCollisionPool_.push_back(bpc);
201 }
202 621 bpc = bodyPairCollisionPool_.acquire();
203 621 bool ret = validateStraightPath(*bpc, path, reverse, validPart, report);
204 621 bodyPairCollisionPool_.release(bpc);
205 621 return ret;
206 }
207
208 5 void ContinuousValidation::addObstacle(
209 const CollisionObjectConstPtr_t &object) {
210 5 for (std::vector<AddObstacle>::const_iterator it(addObstacle_.begin());
211
2/2
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 5 times.
 10 it != addObstacle_.end(); ++it) {
212
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 it->doExecute(object);
213 }
214 5 }
215
216 621 void ContinuousValidation::setPath(IntervalValidations_t &intervalValidations,
217 const PathPtr_t &path, bool reverse) {
218 621 for (IntervalValidations_t::iterator itPair(intervalValidations.begin());
219
2/2
✓ Branch 3 taken 6917 times.
✓ Branch 4 taken 621 times.
 7538 itPair != intervalValidations.end(); ++itPair) {
220
1/2
✓ Branch 3 taken 6917 times.
✗ Branch 4 not taken.
 6917 (*itPair)->path(path, reverse);
221 }
222 621 }
223
224 ✗ void ContinuousValidation::removeObstacleFromJoint(
225 const JointPtr_t &joint, const CollisionObjectConstPtr_t &obstacle) {
226 ✗ assert(joint);
227 ✗ bool removed = false;
228 ✗ for (IntervalValidations_t::iterator itPair(intervalValidations_.begin());
229 ✗ itPair != intervalValidations_.end(); ++itPair) {
230 BodyPairCollisionPtr_t bpc(
231 ✗ HPP_DYNAMIC_PTR_CAST(BodyPairCollision, *itPair));
232 ✗ if (!bpc) continue;
233 // If jointA == joint and jointB is the root joint.
234 ✗ if (bpc->indexJointA() == (size_type)joint->index() &&
235 ✗ bpc->indexJointB() == 0) {
236 ✗ if (bpc->removeObjectTo_b(obstacle)) {
237 ✗ removed = true;
238 ✗ if (bpc->pairs().empty()) {
239 ✗ intervalValidations_.erase(itPair);
240 ✗ bodyPairCollisionPool_.clear();
241 }
242 }
243 }
244 }
245 ✗ if (!removed) {
246 ✗ std::ostringstream oss;
247 oss << "ContinuousValidation::removeObstacleFromJoint: obstacle \""
248 ✗ << obstacle->name() << "\" is not registered as obstacle for joint \""
249 ✗ << joint->name() << "\".";
250 ✗ throw std::runtime_error(oss.str());
251 }
252 }
253
254 ✗ void ContinuousValidation::breakDistance(value_type distance) {
255 ✗ assert(distance >= 0);
256 ✗ breakDistance_ = distance;
257
258 ✗ bodyPairCollisionPool_.clear();
259 ✗ for (IntervalValidationPtr_t &val : intervalValidations_) {
260 continuousValidation::SolidSolidCollisionPtr_t ss(
261 ✗ HPP_DYNAMIC_PTR_CAST(continuousValidation::SolidSolidCollision, val));
262 ✗ if (ss) ss->breakDistance(distance);
263 }
264 }
265
266 ✗ void ContinuousValidation::filterCollisionPairs(
267 const RelativeMotion::matrix_type &relMotion) {
268 // Loop over collision pairs and remove disabled ones.
269 size_type ia, ib;
270 ✗ for (IntervalValidations_t::iterator _colPair(intervalValidations_.begin());
271 ✗ _colPair != intervalValidations_.end();) {
272 BodyPairCollisionPtr_t bpc(
273 ✗ HPP_DYNAMIC_PTR_CAST(BodyPairCollision, *_colPair));
274 ✗ if (!bpc) continue;
275 ✗ ia = bpc->indexJointA();
276 ✗ ib = bpc->indexJointB();
277 ✗ if (ia < 0 || ib < 0) {
278 ✗ ++_colPair;
279 ✗ continue;
280 }
281 ✗ switch (relMotion(ia, ib)) {
282 ✗ case RelativeMotion::Parameterized:
283 hppDout(info, "Parameterized collision pairs treated as Constrained");
284 case RelativeMotion::Constrained:
285 hppDout(info, "Disabling collision pair " << **_colPair);
286 ✗ disabledBodyPairCollisions_.push_back(bpc);
287 ✗ _colPair = intervalValidations_.erase(_colPair);
288 ✗ break;
289 ✗ case RelativeMotion::Unconstrained:
290 ✗ ++_colPair;
291 ✗ break;
292 ✗ default:
293 hppDout(warning, "RelativeMotionType not understood");
294 ✗ ++_colPair;
295 ✗ break;
296 }
297 }
298 ✗ bodyPairCollisionPool_.clear();
299 }
300
301 ✗ void ContinuousValidation::setSecurityMargins(const matrix_t &securityMatrix) {
302 ✗ if (securityMatrix.rows() != robot_->nbJoints() + 1 ||
303 ✗ securityMatrix.cols() != robot_->nbJoints() + 1) {
304 ✗ HPP_THROW(std::invalid_argument,
305 "Wrong size of security margin matrix."
306 " Expected "
307 << robot_->nbJoints() + 1 << 'x' << robot_->nbJoints() + 1
308 << ". Got " << securityMatrix.rows() << 'x'
309 << securityMatrix.cols());
310 }
311
312 // Loop over collision pairs and remove disabled ones.
313 size_type ia, ib;
314 ✗ for (IntervalValidations_t::iterator _colPair(intervalValidations_.begin());
315 ✗ _colPair != intervalValidations_.end(); ++_colPair) {
316 BodyPairCollisionPtr_t bpc(
317 ✗ HPP_DYNAMIC_PTR_CAST(BodyPairCollision, *_colPair));
318 ✗ if (!bpc) continue;
319 ✗ ia = bpc->indexJointA();
320 ✗ ib = bpc->indexJointB();
321 ✗ value_type margin(securityMatrix(ia, ib));
322 ✗ bpc->securityMargin(margin);
323 }
324 // If the collision request is in the BodyPairCollision::Model, there is
325 // not need to clear this pool. However, it becomes required if it is
326 // moved outside, as suggested by a todo note.
327 // To avoid a hard-to-find bug when the todo is adressed, this line is
328 // kept.
329 ✗ bodyPairCollisionPool_.clear();
330 }
331
332 ✗ void ContinuousValidation::setSecurityMarginBetweenBodies(
333 const std::string &body_a, const std::string &body_b,
334 const value_type &margin) {
335 // Loop over collision pairs and remove disabled ones.
336 ✗ bool found = true;
337 ✗ for (IntervalValidations_t::iterator _colPair(intervalValidations_.begin());
338 ✗ _colPair != intervalValidations_.end(); ++_colPair) {
339 BodyPairCollisionPtr_t bpc(
340 ✗ HPP_DYNAMIC_PTR_CAST(BodyPairCollision, *_colPair));
341 ✗ if (!bpc) continue;
342 ✗ const CollisionPairs_t &prs(bpc->pairs());
343 ✗ CollisionRequests_t &requests(bpc->requests());
344 ✗ for (std::size_t i = 0; i < prs.size(); ++i) {
345 ✗ const CollisionPair &pair(prs[i]);
346 ✗ if ((pair.first->name() == body_a && pair.second->name() == body_b) ||
347 ✗ (pair.first->name() == body_b && pair.second->name() == body_a)) {
348 ✗ requests[i].security_margin = margin;
349 ✗ found = true;
350 }
351 }
352 }
353 ✗ if (!found)
354 ✗ throw std::invalid_argument(
355 "Could not find a collision pair between "
356 ✗ "body " +
357 ✗ body_a + " and " + body_b);
358 // If the collision request is in the BodyPairCollision::Model, there is
359 // not need to clear this pool. However, it becomes required if it is
360 // moved outside, as suggested by a todo note.
361 // To avoid a hard-to-find bug when the todo is adressed, this line is
362 // kept.
363 ✗ bodyPairCollisionPool_.clear();
364 }
365
366 template <>
367 8 void ContinuousValidation::add<ContinuousValidation::AddObstacle>(
368 const AddObstacle &delegate) {
369 8 addObstacle_.push_back(delegate);
370 8 }
371
372 template <>
373 ✗ void ContinuousValidation::reset<ContinuousValidation::AddObstacle>() {
374 ✗ addObstacle_.clear();
375 }
376
377 template <>
378 8 void ContinuousValidation::add<ContinuousValidation::Initialize>(
379 const Initialize &delegate) {
380 8 initialize_.push_back(delegate);
381 8 }
382
383 template <>
384 ✗ void ContinuousValidation::reset<ContinuousValidation::Initialize>() {
385 ✗ initialize_.clear();
386 }
387
388 8 void ContinuousValidation::init(ContinuousValidationWkPtr_t weak) {
389 8 weak_ = weak;
390 8 }
391
392 45 void ContinuousValidation::addIntervalValidation(
393 const IntervalValidationPtr_t &intervalValidation) {
394 45 intervalValidations_.push_back(intervalValidation);
395 45 bodyPairCollisionPool_.clear();
396 45 }
397
398 8 void ContinuousValidation::initialize() {
399 8 for (std::vector<Initialize>::const_iterator it(initialize_.begin());
400
2/2
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 8 times.
 16 it != initialize_.end(); ++it) {
401
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 8 it->doExecute();
402 }
403 8 }
404
405 8 ContinuousValidation::~ContinuousValidation() {}
406
407 8 ContinuousValidation::ContinuousValidation(const DevicePtr_t &robot,
408 8 const value_type &tolerance)
409 8 : robot_(robot),
410 8 tolerance_(tolerance),
411 8 breakDistance_(1e-2),
412 8 intervalValidations_(),
413
1/2
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 16 weak_() {
414
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
 8 if (tolerance < 0) {
415 ✗ throw std::runtime_error("tolerance should be non-negative.");
416 }
417
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 add<Initialize>(Initialize(*this));
418
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 add<AddObstacle>(AddObstacle(*this));
419 8 }
420 } // namespace core
421 } // namespace hpp
422