GCC Code Coverage Report

Directory: ./
File: src/continuous-validation/body-pair-collision.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 80 83 96.4%
Branches: 71 106 67.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 <coal/collision.h>
30 #include <coal/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 244010 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 16257 times.
✓ Branch 1 taken 227753 times.
 244010 if (valid_) {
52 16257 interval = path_->timeRange();
53
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16257 times.
 16257 assert(interval.second > interval.first);
54 16257 return true;
55 }
56 227774 continuous_interval iclInterval(interval.first, interval.second,
57 227753 icl::interval_bounds::closed());
58
3/4
✓ Branch 1 taken 227798 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 142531 times.
✓ Branch 4 taken 85267 times.
 227776 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 142531 times.
 142531 assert(interval.second > interval.first);
62 142531 return true;
63 }
64
65 value_type distanceLowerBound;
66
3/4
✓ Branch 1 taken 85234 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 95 times.
✓ Branch 4 taken 85139 times.
 85267 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 85149 times.
 85139 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 85169 times.
✗ Branch 2 not taken.
 85149 halfLengthDist = collisionFreeInterval(t, distanceLowerBound, Vm);
80
1/2
✓ Branch 0 taken 85169 times.
✗ Branch 1 not taken.
 85169 if (Vm != 0) {
81 85169 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 85163 times.
 85170 assert(!std::isnan(halfLengthDist));
87
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 85164 times.
 85163 assert(!std::isnan(halfLengthTol));
88 85164 interval.first = t - (halfLengthDist + halfLengthTol);
89 85164 interval.second = t + (halfLengthDist + halfLengthTol);
90
1/2
✓ Branch 3 taken 85145 times.
✗ Branch 4 not taken.
 85164 validInterval_.insert(continuous_interval(interval.first, interval.second,
91 icl::interval_bounds::closed()));
92 // Check if the whole path is valid.
93 85165 iclInterval =
94 85145 continuous_interval(path_->timeRange().first, path_->timeRange().second,
95 icl::interval_bounds::closed());
96
3/4
✓ Branch 1 taken 85164 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6063 times.
✓ Branch 4 taken 79101 times.
 85165 if (icl::contains(validInterval_, iclInterval)) valid_ = true;
97
6/10
✓ Branch 0 taken 2205 times.
✓ Branch 1 taken 82959 times.
✓ Branch 5 taken 2205 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 2205 times.
✓ Branch 10 taken 2205 times.
✓ Branch 11 taken 82959 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 85164 assert(interval.second > interval.first || path()->length() == 0);
98 85157 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 85132 value_type BodyPairCollision::collisionFreeInterval(
120 const value_type& t, const value_type& distanceLowerBound,
121 value_type& maxVelocity) const {
122 85132 constexpr int Nrefine = 2;
123 value_type T[1 + 2 * Nrefine], Vm[1 + 2 * Nrefine];
124 value_type tm, tM;
125 85132 Vm[0] = maxVelocity = maximalVelocity_;
126 85132 T[0] = distanceLowerBound / maxVelocity;
127
2/2
✓ Branch 0 taken 76777 times.
✓ Branch 1 taken 8355 times.
 85132 if (!refine_) {
128 76777 return T[0];
129 } else {
130 8355 tm = t - T[0];
131 8355 tM = t + T[0];
132 8355 bool leftIsValid = (tm < path_->timeRange().first);
133 8361 bool rightIsValid = (tM > path_->timeRange().second);
134
3/4
✓ Branch 0 taken 160 times.
✓ Branch 1 taken 8224 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 160 times.
 8384 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 33540 times.
✓ Branch 1 taken 8386 times.
 41926 for (int i = 0; i < 2 * Nrefine; ++i) {
159
2/2
✓ Branch 0 taken 640 times.
✓ Branch 1 taken 32900 times.
 33540 tm = t - (leftIsValid ? 0 : T[i]);
160
2/2
✓ Branch 0 taken 1017 times.
✓ Branch 1 taken 32523 times.
 33540 tM = t + (rightIsValid ? 0 : T[i]);
161
2/4
✓ Branch 2 taken 33603 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 33037 times.
✗ Branch 6 not taken.
 33540 path_->velocityBound(Vb_, tm, tM);
162
1/2
✓ Branch 1 taken 33529 times.
✗ Branch 2 not taken.
 33037 Vm[i + 1] = computeMaximalVelocity(Vb_);
163 33529 T[i + 1] = distanceLowerBound / Vm[i + 1];
164
2/2
✓ Branch 0 taken 16783 times.
✓ Branch 1 taken 16746 times.
 33529 if (i % 2 == 1) {
165
2/4
✓ Branch 0 taken 16788 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 16793 times.
✗ Branch 3 not taken.
 16783 assert(T[i - 1] <= T[i + 1] && T[i + 1] <= T[i]);
166
3/4
✓ Branch 0 taken 16792 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 16796 times.
✗ Branch 3 not taken.
 16793 assert(Vm[i - 1] >= Vm[i + 1] && Vm[i + 1] >= Vm[i]);
167 }
168 }
169 8386 constexpr int k = 2 * Nrefine;
170 8386 maxVelocity = Vm[k];
171 8386 return T[k];
172 }
173 }
174
175 85263 bool BodyPairCollision::computeDistanceLowerBound(
176 value_type& distanceLowerBound, ValidationReportPtr_t& report,
177 const pinocchio::DeviceData& data) {
178 using std::numeric_limits;
179 85263 distanceLowerBound = numeric_limits<value_type>::infinity();
180 85255 const CollisionPairs_t& prs(pairs());
181 85257 CollisionRequests_t& rqsts(requests());
182
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 85252 times.
 85270 assert(rqsts.size() == prs.size());
183
2/2
✓ Branch 1 taken 108237 times.
✓ Branch 2 taken 85157 times.
 193407 for (std::size_t i = 0; i < prs.size(); ++i) {
184
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 108263 times.
 108237 assert(rqsts[i].enable_distance_lower_bound == true);
185
1/2
✓ Branch 1 taken 108290 times.
✗ Branch 2 not taken.
 108263 coal::CollisionResult result;
186
1/2
✓ Branch 3 taken 108230 times.
✗ Branch 4 not taken.
 108290 prs[i].collide(data, rqsts[i], result);
187 // Get result
188
2/2
✓ Branch 1 taken 69 times.
✓ Branch 2 taken 108150 times.
 108230 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.
 69 setReport(report, result, prs[i]);
190 95 return false;
191 }
192
2/2
✓ Branch 0 taken 85158 times.
✓ Branch 1 taken 22992 times.
 108150 if (result.distance_lower_bound < distanceLowerBound) {
193 85158 distanceLowerBound = result.distance_lower_bound;
194
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 85158 times.
 85158 assert(distanceLowerBound > 0);
195 }
196
2/2
✓ Branch 1 taken 108155 times.
✓ Branch 2 taken 91 times.
 108245 }
197 85157 return true;
198 }
199 } // namespace continuousValidation
200 } // namespace core
201 } // namespace hpp
202