GCC Code Coverage Report

Directory: ./
File: include/hpp/core/continuous-validation/interval-validation.hh
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 16 18 88.9%
Branches: 5 14 35.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014,2015,2016,2018 CNRS
3 // Authors: Florent Lamiraux, Joseph Mirabel, Diane Bury
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 #ifndef HPP_CORE_CONTINUOUS_VALIDATION_INTERVAL_VALIDATION_HH
31 #define HPP_CORE_CONTINUOUS_VALIDATION_INTERVAL_VALIDATION_HH
32
33 #include <coal/collision.h>
34 #include <coal/collision_data.h>
35
36 #include <boost/icl/continuous_interval.hpp>
37 #include <boost/icl/interval_set.hpp>
38 #include <hpp/core/deprecated.hh>
39 #include <hpp/core/fwd.hh>
40 #include <hpp/pinocchio/body.hh>
41 #include <hpp/pinocchio/collision-object.hh>
42 #include <hpp/pinocchio/joint.hh>
43 #include <iterator>
44 #include <limits>
45
46 namespace hpp {
47 namespace core {
48 namespace continuousValidation {
49 /// Validation of a parameter interval of a path
50 ///
51 /// During path planning, some criteria need to be checked to be
52 /// valid over the whole interval of definition of a path.
53 /// The most common criterion is the absence of collision (implemented
54 /// by derived class BodyPairCollision), but some other criteria might
55 /// need to be checked. For instance the tension of the cables of a
56 /// parallel cable driven robot need to remain in an interval.
57 ///
58 /// This class provides a common interface for continuous validation
59 /// through method \link IntervalValidation::validateConfiguration
60 /// validateConfiguration \endlink.
61 ///
62 /// A \link IntervalValidation::tolerance tolerance \endlink
63 /// may be provided at construction. An interval will be
64 /// considered as valid if the criterion is violated by less than the
65 /// tolerance. This parameter interpretation is left to the
66 /// specialization designers.
67 class IntervalValidation {
68 public:
69 /// Validate an interval for a given criterion
70 /// \param t center of the interval to validate
71 /// \param interval over which the criterion should be checked,
72 /// \retval interval part of the input interval that is valid,
73 /// \retval report report in case of non validity of the configuration
74 /// at parameter t
75 /// \param data data resulting from forward kinematics computed at
76 /// parameter t.
77 virtual bool validateConfiguration(const value_type &t, interval_t &interval,
78 ValidationReportPtr_t &report,
79 const pinocchio::DeviceData &data) = 0;
80
81 /// Set path to validate
82 /// \param path path to validate,
83 /// \param reverse whether path is validated from end to beginning.
84 6917 void path(const PathPtr_t &path, bool reverse) {
85 6917 path_ = path;
86 6917 reverse_ = reverse;
87 6917 valid_ = false;
88
1/2
✓ Branch 2 taken 6917 times.
✗ Branch 3 not taken.
 6917 validInterval_ = interval_set();
89 6917 setupPath();
90 6917 }
91
92 /// Get path
93 246119 PathConstPtr_t path() const { return path_; }
94
95 value_type tolerance() const { return tolerance_; }
96
97 virtual std::string name() const = 0;
98 virtual std::ostream &print(std::ostream &os) const = 0;
99 virtual IntervalValidationPtr_t copy() const = 0;
100
101 protected:
102 typedef boost::icl::continuous_interval<value_type> continuous_interval;
103 typedef boost::icl::interval_set<value_type> interval_set;
104 PathPtr_t path_;
105 value_type tolerance_;
106 bool reverse_;
107 bool refine_;
108 bool valid_;
109 interval_set validInterval_;
110 /// Constructor of interval validation element
111 ///
112 /// \param tolerance allowed penetration should be positive
113 49 IntervalValidation(value_type tolerance)
114
1/2
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
 49 : tolerance_(tolerance), reverse_(false), refine_(true) {
115
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 49 times.
 49 if (tolerance < 0) {
116 ✗ throw std::runtime_error("tolerance should be non-negative.");
117 }
118 49 }
119
120 153 IntervalValidation(const IntervalValidation &other)
121
1/2
✓ Branch 2 taken 153 times.
✗ Branch 3 not taken.
 153 : tolerance_(other.tolerance_), refine_(true) {
122
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 153 times.
 153 if (tolerance_ < 0) {
123 ✗ throw std::runtime_error("tolerance should be non-negative.");
124 }
125 153 }
126
127 private:
128 virtual void setupPath() = 0;
129 }; // class IntervalValidation
130
131 inline std::ostream &operator<<(std::ostream &os, const IntervalValidation &b) {
132 return b.print(os);
133 }
134 } // namespace continuousValidation
135 } // namespace core
136 } // namespace hpp
137 #endif // HPP_CORE_CONTINUOUS_VALIDATION_INTERVAL_VALIDATION_HH
138