| Directory: | ./ |
|---|---|
| File: | src/configuration-shooter/uniform.cc |
| Date: | 2024-08-10 11:29:48 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 13 | 17 | 76.5% |
| Branches: | 11 | 42 | 26.2% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2018 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/configuration-shooter/uniform.hh> | ||
| 31 | #include <hpp/pinocchio/joint-collection.hh> | ||
| 32 | #include <pinocchio/algorithm/joint-configuration.hpp> | ||
| 33 | |||
| 34 | namespace hpp { | ||
| 35 | namespace core { | ||
| 36 | namespace configurationShooter { | ||
| 37 | |||
| 38 | 4043 | void Uniform::impl_shoot(Configuration_t& config) const { | |
| 39 | 4043 | size_type extraDim = robot_->extraConfigSpace().dimension(); | |
| 40 | 4043 | size_type offset = robot_->configSize() - extraDim; | |
| 41 | |||
| 42 | 4043 | config.resize(robot_->configSize()); | |
| 43 |
2/4✓ Branch 4 taken 4043 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4043 times.
✗ Branch 8 not taken.
|
4043 | config.head(offset) = ::pinocchio::randomConfiguration(robot_->model()); |
| 44 | |||
| 45 |
1/2✓ Branch 0 taken 4043 times.
✗ Branch 1 not taken.
|
4043 | if (sampleExtraDOF_) { |
| 46 | // Shoot extra configuration variables | ||
| 47 |
2/2✓ Branch 0 taken 24000 times.
✓ Branch 1 taken 4043 times.
|
28043 | for (size_type i = 0; i < extraDim; ++i) { |
| 48 |
1/2✓ Branch 3 taken 24000 times.
✗ Branch 4 not taken.
|
24000 | value_type lower = robot_->extraConfigSpace().lower(i); |
| 49 |
1/2✓ Branch 3 taken 24000 times.
✗ Branch 4 not taken.
|
24000 | value_type upper = robot_->extraConfigSpace().upper(i); |
| 50 | 24000 | value_type range = upper - lower; | |
| 51 |
3/6✓ Branch 0 taken 24000 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 24000 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 24000 times.
|
24000 | if ((range < 0) || (range == std::numeric_limits<double>::infinity())) { |
| 52 | ✗ | std::ostringstream oss("Cannot uniformy sample extra config variable "); | |
| 53 | ✗ | oss << i << ". min = " << lower << ", max = " << upper << std::endl; | |
| 54 | ✗ | throw std::runtime_error(oss.str()); | |
| 55 | } | ||
| 56 |
1/2✓ Branch 2 taken 24000 times.
✗ Branch 3 not taken.
|
24000 | config[offset + i] = lower + (upper - lower) * rand() / RAND_MAX; |
| 57 | } | ||
| 58 | } else { | ||
| 59 | ✗ | config.tail(extraDim).setZero(); | |
| 60 | } | ||
| 61 | 4043 | } | |
| 62 | |||
| 63 | } // namespace configurationShooter | ||
| 64 | } // namespace core | ||
| 65 | } // namespace hpp | ||
| 66 |