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 |