| 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 |