GCC Code Coverage Report

Directory: ./
File: src/continuous-validation/body-pair-collision.cc
Date: 2024-12-13 16:14:03
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 249337 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 16211 times.
✓ Branch 1 taken 233126 times.
 249337 if (valid_) {
52 16211 interval = path_->timeRange();
53
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16211 times.
 16211 assert(interval.second > interval.first);
54 16211 return true;
55 }
56 233135 continuous_interval iclInterval(interval.first, interval.second,
57 233126 icl::interval_bounds::closed());
58
3/4
✓ Branch 1 taken 233187 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 144510 times.
✓ Branch 4 taken 88677 times.
 233149 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 144510 times.
 144510 assert(interval.second > interval.first);
62 144510 return true;
63 }
64
65 value_type distanceLowerBound;
66
3/4
✓ Branch 1 taken 88637 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 95 times.
✓ Branch 4 taken 88542 times.
 88677 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 88556 times.
 88542 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 88579 times.
✗ Branch 2 not taken.
 88556 halfLengthDist = collisionFreeInterval(t, distanceLowerBound, Vm);
80
1/2
✓ Branch 0 taken 88583 times.
✗ Branch 1 not taken.
 88579 if (Vm != 0) {
81 88583 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 88572 times.
 88587 assert(!std::isnan(halfLengthDist));
87
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 88577 times.
 88572 assert(!std::isnan(halfLengthTol));
88 88577 interval.first = t - (halfLengthDist + halfLengthTol);
89 88577 interval.second = t + (halfLengthDist + halfLengthTol);
90
1/2
✓ Branch 3 taken 88551 times.
✗ Branch 4 not taken.
 88577 validInterval_.insert(continuous_interval(interval.first, interval.second,
91 icl::interval_bounds::closed()));
92 // Check if the whole path is valid.
93 88550 iclInterval =
94 88551 continuous_interval(path_->timeRange().first, path_->timeRange().second,
95 icl::interval_bounds::closed());
96
3/4
✓ Branch 1 taken 88565 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6063 times.
✓ Branch 4 taken 82502 times.
 88550 if (icl::contains(validInterval_, iclInterval)) valid_ = true;
97
6/10
✓ Branch 0 taken 2205 times.
✓ Branch 1 taken 86360 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 86360 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 88565 assert(interval.second > interval.first || path()->length() == 0);
98 88560 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 88536 value_type BodyPairCollision::collisionFreeInterval(
120 const value_type& t, const value_type& distanceLowerBound,
121 value_type& maxVelocity) const {
122 88536 constexpr int Nrefine = 2;
123 value_type T[1 + 2 * Nrefine], Vm[1 + 2 * Nrefine];
124 value_type tm, tM;
125 88536 Vm[0] = maxVelocity = maximalVelocity_;
126 88536 T[0] = distanceLowerBound / maxVelocity;
127
2/2
✓ Branch 0 taken 76777 times.
✓ Branch 1 taken 11759 times.
 88536 if (!refine_) {
128 76777 return T[0];
129 } else {
130 11759 tm = t - T[0];
131 11759 tM = t + T[0];
132 11759 bool leftIsValid = (tm < path_->timeRange().first);
133 11767 bool rightIsValid = (tM > path_->timeRange().second);
134
3/4
✓ Branch 0 taken 165 times.
✓ Branch 1 taken 11618 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 165 times.
 11783 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 47120 times.
✓ Branch 1 taken 11728 times.
 58848 for (int i = 0; i < 2 * Nrefine; ++i) {
159
2/2
✓ Branch 0 taken 660 times.
✓ Branch 1 taken 46460 times.
 47120 tm = t - (leftIsValid ? 0 : T[i]);
160
2/2
✓ Branch 0 taken 916 times.
✓ Branch 1 taken 46204 times.
 47120 tM = t + (rightIsValid ? 0 : T[i]);
161
2/4
✓ Branch 2 taken 47239 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 46197 times.
✗ Branch 6 not taken.
 47120 path_->velocityBound(Vb_, tm, tM);
162
1/2
✓ Branch 1 taken 47053 times.
✗ Branch 2 not taken.
 46197 Vm[i + 1] = computeMaximalVelocity(Vb_);
163 47053 T[i + 1] = distanceLowerBound / Vm[i + 1];
164
2/2
✓ Branch 0 taken 23594 times.
✓ Branch 1 taken 23459 times.
 47053 if (i % 2 == 1) {
165
3/4
✓ Branch 0 taken 23605 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 23602 times.
✓ Branch 3 taken 3 times.
 23594 assert(T[i - 1] <= T[i + 1] && T[i + 1] <= T[i]);
166
2/4
✓ Branch 0 taken 23604 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 23606 times.
✗ Branch 3 not taken.
 23602 assert(Vm[i - 1] >= Vm[i + 1] && Vm[i + 1] >= Vm[i]);
167 }
168 }
169 11728 constexpr int k = 2 * Nrefine;
170 11728 maxVelocity = Vm[k];
171 11728 return T[k];
172 }
173 }
174
175 88660 bool BodyPairCollision::computeDistanceLowerBound(
176 value_type& distanceLowerBound, ValidationReportPtr_t& report,
177 const pinocchio::DeviceData& data) {
178 using std::numeric_limits;
179 88660 distanceLowerBound = numeric_limits<value_type>::infinity();
180 88646 const CollisionPairs_t& prs(pairs());
181 88660 CollisionRequests_t& rqsts(requests());
182
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 88635 times.
 88674 assert(rqsts.size() == prs.size());
183
2/2
✓ Branch 1 taken 112466 times.
✓ Branch 2 taken 88555 times.
 201092 for (std::size_t i = 0; i < prs.size(); ++i) {
184
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 112521 times.
 112466 assert(rqsts[i].enable_distance_lower_bound == true);
185
1/2
✓ Branch 1 taken 112562 times.
✗ Branch 2 not taken.
 112521 coal::CollisionResult result;
186
1/2
✓ Branch 3 taken 112471 times.
✗ Branch 4 not taken.
 112562 prs[i].collide(data, rqsts[i], result);
187 // Get result
188
2/2
✓ Branch 1 taken 52 times.
✓ Branch 2 taken 112411 times.
 112471 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.
 52 setReport(report, result, prs[i]);
190 95 return false;
191 }
192
2/2
✓ Branch 0 taken 88553 times.
✓ Branch 1 taken 23858 times.
 112411 if (result.distance_lower_bound < distanceLowerBound) {
193 88553 distanceLowerBound = result.distance_lower_bound;
194
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 88553 times.
 88553 assert(distanceLowerBound > 0);
195 }
196
2/2
✓ Branch 1 taken 112457 times.
✓ Branch 2 taken 89 times.
 112506 }
197 88555 return true;
198 }
199 } // namespace continuousValidation
200 } // namespace core
201 } // namespace hpp
202