Directory: | ./ |
---|---|
File: | src/continuous-validation/progressive.cc |
Date: | 2024-08-10 11:29:48 |
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 11506 times.
✓ Branch 1 taken 270 times.
✓ Branch 2 taken 11473 times.
✓ Branch 3 taken 33 times.
|
23552 | while (finished < 2 && valid) { |
102 |
2/4✓ Branch 2 taken 11473 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11473 times.
✗ Branch 6 not taken.
|
22946 | bool success = path->eval(q, t); |
103 | 22946 | value_type tprev = t; | |
104 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 11473 times.
|
22946 | if (!success) { |
105 | ✗ | report = PathValidationReportPtr_t(new PathValidationReport( | |
106 | ✗ | t, ValidationReportPtr_t(new ProjectionError()))); | |
107 | ✗ | valid = false; | |
108 |
3/4✓ Branch 1 taken 11473 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 44 times.
✓ Branch 4 taken 11429 times.
|
22946 | } else if (!validateConfiguration(bodyPairCollisions, q, t, interval, |
109 | pathReport)) { | ||
110 | 88 | report = pathReport; | |
111 | 88 | valid = false; | |
112 | } else { | ||
113 | 22858 | t = second(interval, reverse); | |
114 | 22858 | lastValidTime = tprev; | |
115 | } | ||
116 |
2/2✓ Branch 0 taken 440 times.
✓ Branch 1 taken 11033 times.
|
22946 | 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 |