Directory: | ./ |
---|---|
File: | src/continuous-validation/body-pair-collision.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 80 | 83 | 96.4% |
Branches: | 70 | 106 | 66.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // Copyright (c) 2014,2015,2016,2018 CNRS | ||
2 | // Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury | ||
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 <hpp/fcl/collision.h> | ||
30 | #include <hpp/fcl/collision_data.h> | ||
31 | |||
32 | #include <hpp/core/continuous-validation/body-pair-collision.hh> | ||
33 | #include <hpp/core/path.hh> | ||
34 | #include <hpp/core/straight-path.hh> // To enable dynamic casting (needs inheritance). | ||
35 | #include <hpp/pinocchio/body.hh> | ||
36 | #include <hpp/pinocchio/collision-object.hh> | ||
37 | #include <limits> | ||
38 | #include <pinocchio/spatial/fcl-pinocchio-conversions.hpp> | ||
39 | |||
40 | namespace hpp { | ||
41 | namespace core { | ||
42 | namespace continuousValidation { | ||
43 | using ::pinocchio::toFclTransform3f; | ||
44 | |||
45 | 239701 | bool BodyPairCollision::validateConfiguration( | |
46 | const value_type& t, interval_t& interval, ValidationReportPtr_t& report, | ||
47 | const pinocchio::DeviceData& data) { | ||
48 | namespace icl = boost::icl; | ||
49 | using std::numeric_limits; | ||
50 | |||
51 |
2/2✓ Branch 0 taken 16204 times.
✓ Branch 1 taken 223497 times.
|
239701 | if (valid_) { |
52 | 16204 | interval = path_->timeRange(); | |
53 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 16204 times.
|
16204 | assert(interval.second > interval.first); |
54 | 16204 | return true; | |
55 | } | ||
56 | 223499 | continuous_interval iclInterval(interval.first, interval.second, | |
57 | 223497 | icl::interval_bounds::closed()); | |
58 |
3/4✓ Branch 1 taken 223508 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 141189 times.
✓ Branch 4 taken 82319 times.
|
223500 | if (icl::contains(validInterval_, iclInterval)) { |
59 | // TODO interval could probably be enlarge using validInterval_ | ||
60 | // interval = validInterval_; | ||
61 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 141189 times.
|
141189 | assert(interval.second > interval.first); |
62 | 141189 | return true; | |
63 | } | ||
64 | |||
65 | value_type distanceLowerBound; | ||
66 |
3/4✓ Branch 1 taken 82300 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 95 times.
✓ Branch 4 taken 82205 times.
|
82319 | if (!computeDistanceLowerBound(distanceLowerBound, report, data)) { |
67 | 95 | return false; | |
68 | } | ||
69 | |||
70 | value_type halfLengthDist, halfLengthTol; | ||
71 | /// \todo A finer bound could be computed when path is an | ||
72 | /// InterpolatedPath using the maximal velocity on each | ||
73 | /// subinterval | ||
74 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 82209 times.
|
82205 | if (distanceLowerBound == numeric_limits<value_type>::infinity()) { |
75 | ✗ | halfLengthDist = numeric_limits<value_type>::infinity(); | |
76 | ✗ | halfLengthTol = 0; | |
77 | } else { | ||
78 | value_type Vm; | ||
79 |
1/2✓ Branch 1 taken 82216 times.
✗ Branch 2 not taken.
|
82209 | halfLengthDist = collisionFreeInterval(t, distanceLowerBound, Vm); |
80 |
1/2✓ Branch 0 taken 82216 times.
✗ Branch 1 not taken.
|
82216 | if (Vm != 0) { |
81 | 82216 | halfLengthTol = 2 * tolerance_ / Vm; | |
82 | } else { | ||
83 | ✗ | halfLengthTol = numeric_limits<value_type>::infinity(); | |
84 | } | ||
85 | } | ||
86 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 82216 times.
|
82216 | assert(!std::isnan(halfLengthDist)); |
87 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 82219 times.
|
82216 | assert(!std::isnan(halfLengthTol)); |
88 | 82219 | interval.first = t - (halfLengthDist + halfLengthTol); | |
89 | 82219 | interval.second = t + (halfLengthDist + halfLengthTol); | |
90 |
1/2✓ Branch 3 taken 82219 times.
✗ Branch 4 not taken.
|
82219 | validInterval_.insert(continuous_interval(interval.first, interval.second, |
91 | icl::interval_bounds::closed())); | ||
92 | // Check if the whole path is valid. | ||
93 | 82220 | iclInterval = | |
94 | 82219 | continuous_interval(path_->timeRange().first, path_->timeRange().second, | |
95 | icl::interval_bounds::closed()); | ||
96 |
3/4✓ Branch 1 taken 82220 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6061 times.
✓ Branch 4 taken 76159 times.
|
82220 | if (icl::contains(validInterval_, iclInterval)) valid_ = true; |
97 |
6/10✓ Branch 0 taken 2203 times.
✓ Branch 1 taken 80017 times.
✓ Branch 5 taken 2203 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 2203 times.
✓ Branch 10 taken 2203 times.
✓ Branch 11 taken 80017 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
82220 | assert(interval.second > interval.first || path()->length() == 0); |
98 | 82217 | return true; | |
99 | } | ||
100 | |||
101 | 6917 | void BodyPairCollision::setupPath() { | |
102 |
2/2✓ Branch 3 taken 6647 times.
✓ Branch 4 taken 270 times.
|
6917 | if (HPP_DYNAMIC_PTR_CAST(StraightPath, path_)) |
103 | 6647 | refine_ = false; | |
104 | else | ||
105 | 270 | refine_ = true; | |
106 |
1/2✓ Branch 3 taken 6917 times.
✗ Branch 4 not taken.
|
6917 | Vb_ = vector_t(path_->outputDerivativeSize()); |
107 | 6917 | value_type t0 = path_->timeRange().first; | |
108 | 6917 | value_type t1 = path_->timeRange().second; | |
109 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6917 times.
|
6917 | assert(t1 >= t0); |
110 |
2/2✓ Branch 0 taken 2400 times.
✓ Branch 1 taken 4517 times.
|
6917 | if (t1 - t0 == 0) { |
111 | 2400 | maximalVelocity_ = std::numeric_limits<value_type>::infinity(); | |
112 | 2400 | refine_ = false; | |
113 | } else { | ||
114 |
2/4✓ Branch 2 taken 4517 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4517 times.
✗ Branch 6 not taken.
|
4517 | path_->velocityBound(Vb_, t0, t1); |
115 |
1/2✓ Branch 1 taken 4517 times.
✗ Branch 2 not taken.
|
4517 | maximalVelocity_ = computeMaximalVelocity(Vb_); |
116 | } | ||
117 | 6917 | } | |
118 | |||
119 | 82202 | value_type BodyPairCollision::collisionFreeInterval( | |
120 | const value_type& t, const value_type& distanceLowerBound, | ||
121 | value_type& maxVelocity) const { | ||
122 | 82202 | constexpr int Nrefine = 2; | |
123 | value_type T[1 + 2 * Nrefine], Vm[1 + 2 * Nrefine]; | ||
124 | value_type tm, tM; | ||
125 | 82202 | Vm[0] = maxVelocity = maximalVelocity_; | |
126 | 82202 | T[0] = distanceLowerBound / maxVelocity; | |
127 |
2/2✓ Branch 0 taken 76792 times.
✓ Branch 1 taken 5410 times.
|
82202 | if (!refine_) { |
128 | 76792 | return T[0]; | |
129 | } else { | ||
130 | 5410 | tm = t - T[0]; | |
131 | 5410 | tM = t + T[0]; | |
132 | 5410 | bool leftIsValid = (tm < path_->timeRange().first); | |
133 | 5417 | bool rightIsValid = (tM > path_->timeRange().second); | |
134 |
3/4✓ Branch 0 taken 150 times.
✓ Branch 1 taken 5276 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 150 times.
|
5426 | if (leftIsValid && rightIsValid) return T[0]; |
135 | |||
136 | // Refinement step | ||
137 | // Make Nrefine times the following actions | ||
138 | // | ||
139 | // 1. Recompute the maximal velocity on interval T[2*i+0] -> Vm[2*i+1] | ||
140 | // 2. T[2*i+1] <- distanceLowerBound / Vm[2*i+1] | ||
141 | // 3. Recompute the maximal velocity on interval T[2*i+1] -> Vm[2*i+2] | ||
142 | // 4. T[2*i+2] <- distanceLowerBound / Vm[2*i+1] | ||
143 | // | ||
144 | // The following inequalities hold: | ||
145 | // - T [2*i+1] >= T [2*i+0], | ||
146 | // - Vm[2*i+1] <= Vm[2*i], | ||
147 | // - T [2*i+2] <= T [2*i+1], | ||
148 | // - Vm[2*i+2] >= Vm[2*i+1] | ||
149 | // since | ||
150 | // - the maximal velocity over an interval is increasing with the | ||
151 | // size of the interval and | ||
152 | // - distanceLowerBound / Vm is decreasing when Vm increases. | ||
153 | // | ||
154 | // Note that steps 3 and 4 are compacted into one loop in the lines | ||
155 | // below. Performing those steps an even number of times ensures that | ||
156 | // the maximal velocity used to compute the result interval T[2*i+2] | ||
157 | // is evaluated on an superset of T[2*i+2]. | ||
158 |
2/2✓ Branch 0 taken 21688 times.
✓ Branch 1 taken 5420 times.
|
27108 | for (int i = 0; i < 2 * Nrefine; ++i) { |
159 |
2/2✓ Branch 0 taken 600 times.
✓ Branch 1 taken 21088 times.
|
21688 | tm = t - (leftIsValid ? 0 : T[i]); |
160 |
2/2✓ Branch 0 taken 1064 times.
✓ Branch 1 taken 20624 times.
|
21688 | tM = t + (rightIsValid ? 0 : T[i]); |
161 |
2/4✓ Branch 2 taken 21732 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21627 times.
✗ Branch 6 not taken.
|
21688 | path_->velocityBound(Vb_, tm, tM); |
162 |
1/2✓ Branch 1 taken 21671 times.
✗ Branch 2 not taken.
|
21627 | Vm[i + 1] = computeMaximalVelocity(Vb_); |
163 | 21671 | T[i + 1] = distanceLowerBound / Vm[i + 1]; | |
164 |
2/2✓ Branch 0 taken 10849 times.
✓ Branch 1 taken 10822 times.
|
21671 | if (i % 2 == 1) { |
165 |
2/4✓ Branch 0 taken 10850 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 10852 times.
✗ Branch 3 not taken.
|
10849 | assert(T[i - 1] <= T[i + 1] && T[i + 1] <= T[i]); |
166 |
2/4✓ Branch 0 taken 10855 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 10860 times.
✗ Branch 3 not taken.
|
10852 | assert(Vm[i - 1] >= Vm[i + 1] && Vm[i + 1] >= Vm[i]); |
167 | } | ||
168 | } | ||
169 | 5420 | constexpr int k = 2 * Nrefine; | |
170 | 5420 | maxVelocity = Vm[k]; | |
171 | 5420 | return T[k]; | |
172 | } | ||
173 | } | ||
174 | |||
175 | 82312 | bool BodyPairCollision::computeDistanceLowerBound( | |
176 | value_type& distanceLowerBound, ValidationReportPtr_t& report, | ||
177 | const pinocchio::DeviceData& data) { | ||
178 | using std::numeric_limits; | ||
179 | 82312 | distanceLowerBound = numeric_limits<value_type>::infinity(); | |
180 | 82310 | const CollisionPairs_t& prs(pairs()); | |
181 | 82309 | CollisionRequests_t& rqsts(requests()); | |
182 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 82305 times.
|
82318 | assert(rqsts.size() == prs.size()); |
183 |
2/2✓ Branch 1 taken 104509 times.
✓ Branch 2 taken 82204 times.
|
186720 | for (std::size_t i = 0; i < prs.size(); ++i) { |
184 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 104518 times.
|
104509 | assert(rqsts[i].enable_distance_lower_bound == true); |
185 |
1/2✓ Branch 1 taken 104534 times.
✗ Branch 2 not taken.
|
104518 | fcl::CollisionResult result; |
186 |
1/2✓ Branch 3 taken 104509 times.
✗ Branch 4 not taken.
|
104534 | prs[i].collide(data, rqsts[i], result); |
187 | // Get result | ||
188 |
2/2✓ Branch 1 taken 91 times.
✓ Branch 2 taken 104411 times.
|
104509 | if (result.isCollision()) { |
189 |
3/6✓ Branch 2 taken 95 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 95 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 95 times.
✗ Branch 9 not taken.
|
91 | setReport(report, result, prs[i]); |
190 | 95 | return false; | |
191 | } | ||
192 |
2/2✓ Branch 0 taken 82235 times.
✓ Branch 1 taken 22176 times.
|
104411 | if (result.distance_lower_bound < distanceLowerBound) { |
193 | 82235 | distanceLowerBound = result.distance_lower_bound; | |
194 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 82235 times.
|
82235 | assert(distanceLowerBound > 0); |
195 | } | ||
196 |
2/2✓ Branch 1 taken 104415 times.
✓ Branch 2 taken 92 times.
|
104506 | } |
197 | 82204 | return true; | |
198 | } | ||
199 | } // namespace continuousValidation | ||
200 | } // namespace core | ||
201 | } // namespace hpp | ||
202 |