GCC Code Coverage Report


Directory: ./
File: src/continuous-validation/progressive.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 50 55 90.9%
Branches: 26 52 50.0%

Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
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/progressive.hh>
32 #include <hpp/core/path-vector.hh>
33 #include <hpp/core/straight-path.hh>
34 #include <hpp/util/debug.hh>
35 #include <limits>
36
37 #include "continuous-validation/helper.hh"
38 #include "continuous-validation/intervals.hh"
39
40 namespace hpp {
41 namespace core {
42 namespace continuousValidation {
43 namespace {
44 typedef std::pair<pinocchio::JointIndex, pinocchio::JointIndex>
45 JointIndexPair_t;
46
47 struct JointIndexPairCompare_t {
48 bool operator()(const JointIndexPair_t& p0,
49 const JointIndexPair_t& p1) const {
50 if (p0.first < p1.first) return true;
51 if (p0.first > p1.first) return false;
52 return (p0.second < p1.second);
53 }
54 };
55
56 typedef std::set<JointIndexPair_t, JointIndexPairCompare_t> JointIndexPairSet_t;
57 } // namespace
58
59 4 ProgressivePtr_t Progressive::create(const DevicePtr_t& robot,
60 const value_type& tolerance) {
61
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
4 Progressive* ptr = new Progressive(robot, tolerance);
62 4 ProgressivePtr_t shPtr(ptr);
63
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
4 ptr->init(shPtr);
64
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
4 ptr->initialize();
65 4 return shPtr;
66 }
67
68 303 bool Progressive::validateStraightPath(IntervalValidations_t& bpc,
69 const PathPtr_t& path, bool reverse,
70 PathPtr_t& validPart,
71 PathValidationReportPtr_t& report) {
72
2/2
✓ Branch 0 taken 96 times.
✓ Branch 1 taken 207 times.
303 if (reverse)
73 96 return validateStraightPath<true>(bpc, path, validPart, report);
74 else
75 207 return validateStraightPath<false>(bpc, path, validPart, report);
76 }
77
78 template <bool reverse>
79 606 bool Progressive::validateStraightPath(
80 IntervalValidations_t& bodyPairCollisions, const PathPtr_t& path,
81 PathPtr_t& validPart, PathValidationReportPtr_t& report) {
82 // for each IntervalValidation
83 // - set path,
84 // - compute valid interval at start (end if reverse)
85 606 bool valid = true;
86 606 const interval_t& tr(path->timeRange());
87
1/2
✓ Branch 3 taken 303 times.
✗ Branch 4 not taken.
606 Configuration_t q(path->outputSize());
88 606 PathValidationReportPtr_t pathReport;
89 606 interval_t interval;
90
91
1/2
✓ Branch 1 taken 303 times.
✗ Branch 2 not taken.
606 setPath(bodyPairCollisions, path, reverse);
92
93 606 const value_type tmin = tr.first;
94 606 const value_type tmax = tr.second;
95 606 value_type lastValidTime = first(tr, reverse);
96 606 value_type t = lastValidTime;
97 606 unsigned finished = 0;
98 // If the interval is of length 0, there is only one configuration to
99 // validate.
100
2/2
✓ Branch 0 taken 100 times.
✓ Branch 1 taken 203 times.
606 if (reverse ? t <= tmin : t >= tmax) finished++;
101
4/4
✓ Branch 0 taken 12310 times.
✓ Branch 1 taken 270 times.
✓ Branch 2 taken 12277 times.
✓ Branch 3 taken 33 times.
25160 while (finished < 2 && valid) {
102
2/4
✓ Branch 2 taken 12277 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12274 times.
✗ Branch 6 not taken.
24554 bool success = path->eval(q, t);
103 24548 value_type tprev = t;
104
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 12274 times.
24548 if (!success) {
105 report = PathValidationReportPtr_t(new PathValidationReport(
106 t, ValidationReportPtr_t(new ProjectionError())));
107 valid = false;
108
3/4
✓ Branch 1 taken 12277 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 44 times.
✓ Branch 4 taken 12233 times.
24548 } else if (!validateConfiguration(bodyPairCollisions, q, t, interval,
109 pathReport)) {
110 88 report = pathReport;
111 88 valid = false;
112 } else {
113 24466 t = second(interval, reverse);
114 24466 lastValidTime = tprev;
115 }
116
2/2
✓ Branch 0 taken 440 times.
✓ Branch 1 taken 11837 times.
24554 if (reverse ? t <= tmin : t >= tmax) {
117 880 t = reverse ? tmin : tmax;
118 880 finished++;
119 }
120 }
121
2/2
✓ Branch 0 taken 259 times.
✓ Branch 1 taken 44 times.
606 if (valid) {
122 518 validPart = path;
123 518 return true;
124 } else {
125
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
88 validPart = reverse ? path->extract(lastValidTime, tmax)
126 : path->extract(tmin, lastValidTime);
127 88 return false;
128 }
129 606 }
130
131 8 Progressive::~Progressive() {}
132
133 4 void Progressive::init(const ProgressiveWkPtr_t weak) {
134
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
4 ContinuousValidation::init(weak);
135 4 weak_ = weak;
136 4 }
137
138 4 Progressive::Progressive(const DevicePtr_t& robot, const value_type& tolerance)
139 4 : ContinuousValidation(robot, tolerance), weak_() {
140
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
4 if (tolerance <= 0) {
141 throw std::runtime_error(
142 "tolerance should be positive for"
143 " progressive continuous validation.");
144 }
145 4 }
146 } // namespace continuousValidation
147 } // namespace core
148 } // namespace hpp
149