GCC Code Coverage Report

Directory: ./
File: include/hpp/core/interpolated-path.hh
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 13 33 39.4%
Branches: 11 46 23.9%
Line Branch Exec Source
1 // Copyright (c) 2015 CNRS
2 // Authors: Joseph Mirabel
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #ifndef HPP_CORE_INTERPOLATED_PATH_HH
30 #define HPP_CORE_INTERPOLATED_PATH_HH
31
32 #include <hpp/core/config.hh>
33 #include <hpp/core/fwd.hh>
34 #include <hpp/core/path.hh>
35
36 namespace hpp {
37 namespace core {
38 /// \addtogroup path
39 /// \{
40
41 /// Piecewise linear interpolation between two configurations
42 ///
43 /// This type of path is the return type of PathProjector algorithms.
44 ///
45 /// Degrees of freedom are interpolated depending on the type of
46 /// \link hpp::pinocchio::Joint joint \endlink
47 /// they parameterize:
48 /// \li linear interpolation for translation joints, bounded rotation
49 /// joints, and translation part of freeflyer joints,
50 /// \li angular interpolation for unbounded rotation joints,
51 /// \li constant angular velocity for SO(3) part of freeflyer joints.
52 class HPP_CORE_DLLAPI InterpolatedPath : public Path {
53 public:
54 typedef std::pair<const value_type, Configuration_t> InterpolationPoint_t;
55 typedef std::map<value_type, Configuration_t, std::less<value_type>,
56 Eigen::aligned_allocator<InterpolationPoint_t> >
57 InterpolationPoints_t;
58 typedef Path parent_t;
59
60 /// Destructor
61 424 virtual ~InterpolatedPath() {}
62
63 /// Create instance and return shared pointer
64 /// \param device Robot corresponding to configurations
65 /// \param init, end Start and end configurations of the path
66 /// \param timeRange interval of definition
67 static InterpolatedPathPtr_t create(const DevicePtr_t& device,
68 ConfigurationIn_t init,
69 ConfigurationIn_t end,
70 interval_t timeRange) {
71 InterpolatedPath* ptr = new InterpolatedPath(device, init, end, timeRange);
72 InterpolatedPathPtr_t shPtr(ptr);
73 ptr->init(shPtr);
74 return shPtr;
75 }
76
77 /// Create instance and return shared pointer
78 /// \param device Robot corresponding to configurations
79 /// \param init, end Start and end configurations of the path
80 /// \param timeRange interval of definition
81 /// \param constraints the path is subject to
82 106 static InterpolatedPathPtr_t create(const DevicePtr_t& device,
83 ConfigurationIn_t init,
84 ConfigurationIn_t end,
85 interval_t timeRange,
86 ConstraintSetPtr_t constraints) {
87 InterpolatedPath* ptr =
88
3/6
✓ Branch 2 taken 106 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 106 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 106 times.
✗ Branch 10 not taken.
 106 new InterpolatedPath(device, init, end, timeRange, constraints);
89 106 InterpolatedPathPtr_t shPtr(ptr);
90
1/2
✓ Branch 2 taken 106 times.
✗ Branch 3 not taken.
 106 ptr->init(shPtr);
91 106 return shPtr;
92 }
93
94 /// Create instance and return shared pointer
95 /// \param device Robot corresponding to configurations
96 /// \param init, end Start and end configurations of the path
97 /// \param length Distance between the configurations.
98 static InterpolatedPathPtr_t create(const DevicePtr_t& device,
99 ConfigurationIn_t init,
100 ConfigurationIn_t end,
101 value_type length) {
102 return create(device, init, end, interval_t(0, length));
103 }
104
105 /// Create instance and return shared pointer
106 /// \param device Robot corresponding to configurations
107 /// \param init, end Start and end configurations of the path
108 /// \param length Distance between the configurations.
109 /// \param constraints the path is subject to
110 106 static InterpolatedPathPtr_t create(const DevicePtr_t& device,
111 ConfigurationIn_t init,
112 ConfigurationIn_t end, value_type length,
113 ConstraintSetPtr_t constraints) {
114
3/6
✓ Branch 3 taken 106 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 106 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 106 times.
✗ Branch 10 not taken.
 106 return create(device, init, end, interval_t(0, length), constraints);
115 }
116
117 /// Create copy and return shared pointer
118 /// \param path path to copy
119 ✗ static InterpolatedPathPtr_t createCopy(const InterpolatedPathPtr_t& path) {
120 ✗ InterpolatedPath* ptr = new InterpolatedPath(*path);
121 ✗ InterpolatedPathPtr_t shPtr(ptr);
122 ✗ ptr->initCopy(shPtr);
123 ✗ return shPtr;
124 }
125
126 /// Create an interpolation of this path
127 /// \param path path to interpolate
128 /// \param nbSamples number of samples between the initial and end
129 /// configuration
130 /// \note it is assume that the constraints are constant along the path
131 static InterpolatedPathPtr_t create(const PathPtr_t& path,
132 const DevicePtr_t& device,
133 const std::size_t& nbSamples);
134
135 /// Create copy and return shared pointer
136 /// \param path path to copy
137 /// \param constraints the path is subject to
138 ✗ static InterpolatedPathPtr_t createCopy(
139 const InterpolatedPathPtr_t& path,
140 const ConstraintSetPtr_t& constraints) {
141 ✗ InterpolatedPath* ptr = new InterpolatedPath(*path, constraints);
142 ✗ InterpolatedPathPtr_t shPtr(ptr);
143 ✗ ptr->initCopy(shPtr);
144 ✗ return shPtr;
145 }
146
147 /// Return a shared pointer to this
148 ///
149 /// As StaightPath are immutable, and refered to by shared pointers,
150 /// they do not need to be copied.
151 ✗ virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
152
153 /// Return a shared pointer to a copy of this and set constraints
154 ///
155 /// \param constraints constraints to apply to the copy
156 /// \pre *this should not have constraints.
157 ✗ virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
158 ✗ return createCopy(weak_.lock(), constraints);
159 }
160
161 virtual PathPtr_t reverse() const;
162
163 /// Return the internal robot.
164 DevicePtr_t device() const;
165
166 /// Insert interpolation point
167 3018 void insert(const value_type& time, ConfigurationIn_t config) {
168
1/2
✓ Branch 2 taken 3018 times.
✗ Branch 3 not taken.
 3018 configs_.insert(InterpolationPoint_t(time, config));
169 3018 }
170
171 /// Get the initial configuration
172
1/2
✓ Branch 3 taken 324 times.
✗ Branch 4 not taken.
 648 Configuration_t initial() const { return configs_.begin()->second; }
173
174 /// Get the final configuration
175
2/4
✓ Branch 2 taken 314 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 314 times.
✗ Branch 6 not taken.
 628 Configuration_t end() const { return configs_.rbegin()->second; }
176
177 ✗ const InterpolationPoints_t& interpolationPoints() const { return configs_; }
178
179 protected:
180 /// Print path in a stream
181 ✗ virtual std::ostream& print(std::ostream& os) const {
182 ✗ os << "InterpolatedPath:" << std::endl;
183 ✗ Path::print(os);
184 ✗ os << "initial configuration: " << initial().transpose() << std::endl;
185 ✗ os << "final configuration: " << end().transpose() << std::endl;
186 ✗ return os;
187 }
188
189 /// Constructor
190 InterpolatedPath(const DevicePtr_t& robot, ConfigurationIn_t init,
191 ConfigurationIn_t end, interval_t timeRange);
192
193 /// Constructor with constraints
194 InterpolatedPath(const DevicePtr_t& robot, ConfigurationIn_t init,
195 ConfigurationIn_t end, interval_t timeRange,
196 ConstraintSetPtr_t constraints);
197
198 /// DIscretization of a given path.
199 InterpolatedPath(const PathPtr_t& path, const DevicePtr_t& device,
200 const std::size_t& nbSamples);
201
202 /// Copy constructor
203 InterpolatedPath(const InterpolatedPath& path);
204
205 /// Copy constructor with constraints
206 InterpolatedPath(const InterpolatedPath& path,
207 const ConstraintSetPtr_t& constraints);
208
209 void init(InterpolatedPathPtr_t self);
210
211 void initCopy(InterpolatedPathPtr_t self);
212
213 virtual bool impl_compute(ConfigurationOut_t result, value_type param) const;
214 /// Virtual implementation of derivative
215 virtual void impl_derivative(vectorOut_t result, const value_type& t,
216 size_type order) const;
217 virtual void impl_velocityBound(vectorOut_t result, const value_type& t0,
218 const value_type& t1) const;
219
220 /// Extraction/Reversion of a sub-path
221 /// See Path::extract
222 PathPtr_t impl_extract(const interval_t& subInterval) const;
223
224 private:
225 DevicePtr_t device_;
226 InterpolationPoints_t configs_;
227 InterpolatedPathWkPtr_t weak_;
228 }; // class InterpolatedPath
229 /// \}
230 } // namespace core
231 } // namespace hpp
232 #endif // HPP_CORE_INTERPOLATED_PATH_HH
233