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 |