GCC Code Coverage Report

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