Directory: | ./ |
---|---|
File: | src/joint-bound-validation.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 18 | 28 | 64.3% |
Branches: | 13 | 44 | 29.5% |
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/joint-bound-validation.hh> | ||
31 | #include <hpp/pinocchio/device.hh> | ||
32 | #include <hpp/pinocchio/joint.hh> | ||
33 | #include <pinocchio/multibody/model.hpp> | ||
34 | #include <sstream> | ||
35 | |||
36 | namespace hpp { | ||
37 | namespace core { | ||
38 | typedef pinocchio::JointConfiguration* JointConfigurationPtr_t; | ||
39 | 13 | JointBoundValidationPtr_t JointBoundValidation::create( | |
40 | const DevicePtr_t& robot) { | ||
41 |
1/2✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
|
13 | JointBoundValidation* ptr = new JointBoundValidation(robot); |
42 | 13 | return JointBoundValidationPtr_t(ptr); | |
43 | } | ||
44 | |||
45 | 32702 | bool JointBoundValidation::validate(const Configuration_t& config, | |
46 | ValidationReportPtr_t& validationReport) { | ||
47 | 32702 | const pinocchio::Model& model = robot_->model(); | |
48 | // Check whether all config param are within boundaries. | ||
49 |
2/2✓ Branch 0 taken 1078770 times.
✓ Branch 1 taken 32702 times.
|
1111472 | for (std::size_t i = 0; i < (std::size_t)model.nq; ++i) { |
50 |
2/4✓ Branch 2 taken 1078770 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1078770 times.
|
2157540 | if ((model.upperPositionLimit[i] < config[i]) || |
51 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1078770 times.
|
1078770 | (model.lowerPositionLimit[i] > config[i])) { |
52 | /// Find the joint at rank i | ||
53 | ✗ | JointPtr_t joint = robot_->getJointAtConfigRank(i); | |
54 | ✗ | assert(i >= (std::size_t)joint->rankInConfiguration()); | |
55 | ✗ | const std::size_t j = i - joint->rankInConfiguration(); | |
56 | |||
57 | JointBoundValidationReportPtr_t report(new JointBoundValidationReport( | ||
58 | ✗ | joint, j, model.lowerPositionLimit[i], model.upperPositionLimit[i], | |
59 | ✗ | config[i])); | |
60 | ✗ | validationReport = report; | |
61 | ✗ | return false; | |
62 | } | ||
63 | } | ||
64 | 32702 | const pinocchio::ExtraConfigSpace& ecs = robot_->extraConfigSpace(); | |
65 | // Check the extra config space | ||
66 | // FIXME This was introduced at the same time as the integration of Pinocchio | ||
67 | 32702 | size_type index = robot_->model().nq; | |
68 |
2/2✓ Branch 1 taken 196128 times.
✓ Branch 2 taken 32702 times.
|
228830 | for (size_type i = 0; i < ecs.dimension(); ++i) { |
69 |
1/2✓ Branch 1 taken 196128 times.
✗ Branch 2 not taken.
|
196128 | value_type lower = ecs.lower(i); |
70 |
1/2✓ Branch 1 taken 196128 times.
✗ Branch 2 not taken.
|
196128 | value_type upper = ecs.upper(i); |
71 |
1/2✓ Branch 1 taken 196128 times.
✗ Branch 2 not taken.
|
196128 | value_type value = config[index + i]; |
72 |
2/4✓ Branch 0 taken 196128 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 196128 times.
|
196128 | if (value < lower || upper < value) { |
73 | JointBoundValidationReportPtr_t report( | ||
74 | ✗ | new JointBoundValidationReport(JointPtr_t(), i, lower, upper, value)); | |
75 | ✗ | validationReport = report; | |
76 | ✗ | return false; | |
77 | } | ||
78 | } | ||
79 | 32702 | return true; | |
80 | } | ||
81 | |||
82 | 13 | JointBoundValidation::JointBoundValidation(const DevicePtr_t& robot) | |
83 | 13 | : robot_(robot) {} | |
84 | } // namespace core | ||
85 | } // namespace hpp | ||
86 |