| Directory: | ./ |
|---|---|
| File: | src/joint-bound-validation.cc |
| Date: | 2024-12-13 16:14:03 |
| 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 |