GCC Code Coverage Report

Directory: ./
File: include/hpp/core/path.hh
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 51 63 81.0%
Branches: 24 58 41.4%
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 #ifndef HPP_CORE_PATH_HH
31 #define HPP_CORE_PATH_HH
32
33 #include <hpp/core/config.hh>
34 #include <hpp/core/constraint-set.hh>
35 #include <hpp/core/deprecated.hh>
36 #include <hpp/core/fwd.hh>
37 #include <hpp/core/projection-error.hh>
38 #include <hpp/core/time-parameterization.hh>
39 #include <hpp/util/exception.hh>
40 #include <hpp/util/serialization-fwd.hh>
41
42 namespace hpp {
43 namespace core {
44 /// \addtogroup path Path
45 /// Path abstraction, implementation and decorators
46 /// \{
47
48 /** Abstraction of paths: mapping from time to configuration space
49 *
50 * A path \f$ p \f$ is defined by:
51 * \f{eqnarray*}{
52 * p : [t_0, t_1] &\to & \mathcal{C} \\
53 * t &\mapsto & constraints.apply( q(t) )
54 * \f}
55 * where
56 * \li \f$ [t_0, t_1] \f$ is given by \ref timeRange
57 * \li \f$ q(t) \f$ is the child class implementation of \ref impl_compute
58 * \li `constraints.apply` corresponds to calling Constraint::apply to
59 * \ref Path::constraints "constraints"
60 *
61 * Optionally, it is possible to time-parameterize the path with a function
62 * \f$ s \f$. By default, \f$ s \f$ is the identity.
63 * The model becomes:
64 * \f{eqnarray*}{
65 * p : [t_0, t_1] &\to & \mathcal{C} \\
66 * t &\mapsto & constraints.apply( q(s(t)) )
67 * \f}
68 * where \f$ s \f$ is the \ref timeParameterization, from
69 * \ref timeRange to \ref paramRange.
70 */
71 class HPP_CORE_DLLAPI Path {
72 public:
73 /// \name Construction, destruction, copy
74 /// \{
75
76 /// Destructor
77 7487026 virtual ~Path() {}
78
79 /// Return a shared pointer to a copy of this
80 virtual PathPtr_t copy() const = 0;
81
82 /// Return a shared pointer to a copy of this and set constraints
83 ///
84 /// \param constraints constraints to apply to the copy
85 /// \pre *this should not have constraints.
86 virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const = 0;
87
88 /// Static cast into a derived type
89 template <class T>
90 ✗ shared_ptr<T> as(void) {
91 ✗ assert(HPP_DYNAMIC_PTR_CAST(T, weak_.lock()));
92 ✗ return HPP_STATIC_PTR_CAST(T, weak_.lock());
93 }
94
95 /// Static cast into a derived type
96 template <class T>
97 shared_ptr<const T> as(void) const {
98 assert(HPP_DYNAMIC_PTR_CAST(const T, weak_.lock()));
99 return HPP_STATIC_PTR_CAST(const T, weak_.lock());
100 }
101
102 /// \}
103
104 /// \name Extraction/Reversion of a sub-path
105 /// \{
106
107 /// \param subInterval interval of definition of the extract path
108 /// If upper bound of subInterval is smaller than lower bound,
109 /// result is reversed.
110 /// \exception projection_error is thrown when an end configuration of
111 /// the returned path could not be computed
112 /// due to projection failure.
113 PathPtr_t extract(const interval_t& subInterval) const;
114
115 /// \copydoc Path::extract(const interval_t&) const
116 178 PathPtr_t extract(const value_type& tmin, const value_type& tmax) const {
117
1/2
✓ Branch 2 taken 178 times.
✗ Branch 3 not taken.
 356 return extract(std::make_pair(tmin, tmax));
118 }
119
120 /// Reversion of a path
121 /// \return a new path that is this one reversed.
122 virtual PathPtr_t reverse() const;
123
124 /// \}
125
126 /// \name Evalutation of the path
127 /// \{
128
129 /// Configuration at time
130 1090431 Configuration_t eval(const value_type& time, bool& success) const {
131
1/2
✓ Branch 2 taken 1090431 times.
✗ Branch 3 not taken.
 2180862 return configAtParam(paramAtTime(time), success);
132 }
133
134 /// Configuration at time
135 1000010 bool eval(ConfigurationOut_t result, const value_type& time) const {
136
1/2
✓ Branch 1 taken 1000010 times.
✗ Branch 2 not taken.
 1000010 value_type s = paramAtTime(time);
137
2/4
✓ Branch 1 taken 1000010 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000005 times.
✗ Branch 5 not taken.
 1000010 bool success = impl_compute(result, s);
138
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1000005 times.
 1000005 if (!success) return false;
139
2/4
✓ Branch 1 taken 1000010 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000008 times.
✗ Branch 5 not taken.
 1000005 return applyConstraints(result, s);
140 }
141
142 /// Get the configuration at a parameter without applying the constraints.
143 660 bool at(const value_type& time, ConfigurationOut_t result) const {
144
1/2
✓ Branch 3 taken 660 times.
✗ Branch 4 not taken.
 660 return impl_compute(result, paramAtTime(time));
145 }
146
147 /// Get derivative with respect to parameter at given parameter
148 /// \param time value of the time in the definition interval,
149 /// \param order order of the derivative
150 /// \retval result derivative. Should be allocated and of correct size.
151 /// \warning the method is not implemented in this class and throws if
152 /// called without being implemented in the derived class.
153 /// \note unless otherwise stated, this method is not compatible with
154 /// constraints. The derivative of the non-constrained path will
155 /// be computed.
156 void derivative(vectorOut_t result, const value_type& time,
157 size_type order) const;
158
159 /// Get an upper bound of the velocity on a sub-interval.
160 /// The result is a coefficient-wise.
161 ///
162 /// \param t0 begin of the interval
163 /// \param t1 end of the interval
164 /// \retval result maximal derivative on the sub-interval. Should be allocated
165 /// and of correct size. \warning the method is not implemented in this class
166 /// and throws if
167 /// called without being implemented in the derived class.
168 /// \note unless otherwise stated, this method is not compatible with
169 /// constraints. The derivative of the non-constrained path will
170 /// be computed.
171 52600 void velocityBound(vectorOut_t result, const value_type& t0,
172 const value_type& t1) const {
173
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 52627 times.
 52600 assert(result.size() == outputDerivativeSize());
174
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 52627 times.
 52627 assert(t0 <= t1);
175
3/6
✓ Branch 2 taken 52700 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 52794 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 51676 times.
✗ Branch 9 not taken.
 52560 impl_velocityBound(result, paramAtTime(std::max(t0, timeRange().first)),
176 52627 paramAtTime(std::min(t1, timeRange().second)));
177
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 51716 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 51676 if (timeParam_) result *= timeParam_->derivativeBound(t0, t1);
178 51716 }
179
180 /// \name Path definition
181 /// \{
182
183 /// Get size of configuration space
184 2018399 size_type outputSize() const { return outputSize_; }
185
186 /// Get size of velocity
187 2838560 size_type outputDerivativeSize() const { return outputDerivativeSize_; }
188
189 /// Get interval of definition
190 3268799 const interval_t& timeRange() const { return timeRange_; }
191
192 /// Get length of definition interval
193 18963598 virtual value_type length() const {
194 18963598 return timeRange_.second - timeRange_.first;
195 }
196
197 /// Get the initial configuration
198 virtual Configuration_t initial() const = 0;
199
200 /// Get the final configuration
201 virtual Configuration_t end() const = 0;
202
203 /// Get constraints the path is subject to
204 24235619 const ConstraintSetPtr_t& constraints() const { return constraints_; }
205
206 /// \}
207
208 /// \name Time parameterizarion
209 /// Time parameterization is handled by this class so child classes need
210 /// not to handle it.
211 /// \{
212
213 /// Get interval of parameters.
214 /// \return the result of applying the \ref timeParameterization to
215 /// \ref timeRange().
216 /// \note The time parameterization defaults to identity.
217 6712150 const interval_t& paramRange() const { return paramRange_; }
218
219 /// Get the time parameterization function
220 1857642 const TimeParameterizationPtr_t& timeParameterization() const {
221 1857642 return timeParam_;
222 }
223
224 /// Set the time parameterization function
225 3 void timeParameterization(const TimeParameterizationPtr_t& tp,
226 const interval_t& tr) {
227 3 timeParam_ = tp;
228 3 timeRange(tr);
229 3 }
230
231 /// \}
232
233 protected:
234 /// Print interval of definition (and of parameters if relevant)
235 /// in a stream
236 virtual std::ostream& print(std::ostream& os) const;
237
238 /// Constructor
239 /// \param interval interval of definition of the path,
240 /// \param outputSize size of the output configuration,
241 /// \param outputDerivativeSize number of degrees of freedom of the
242 /// underlying robot
243 /// \param constraints constraints the set is subject to,
244 /// constraints are solved at each evaluation of the output
245 /// configuration.
246 /// \note Constraints are copied.
247 Path(const interval_t& interval, size_type outputSize,
248 size_type outputDerivativeSize, const ConstraintSetPtr_t& constraints);
249
250 /// Constructor
251 /// \param interval interval of definition of the path,
252 /// \param outputSize size of the output configuration,
253 /// \param outputDerivativeSize number of degrees of freedom of the
254 /// underlying robot
255 Path(const interval_t& interval, size_type outputSize,
256 size_type outputDerivativeSize);
257
258 /// Copy constructor
259 Path(const Path& path);
260
261 /// Copy constructor with constraints
262 Path(const Path& path, const ConstraintSetPtr_t& constraints);
263
264 /// Store weak pointer to itself
265 ///
266 /// should be called at construction of derived class instances
267 void init(const PathWkPtr_t& self);
268
269 /// Interval of parameters
270 interval_t paramRange_;
271
272 /// Set the constraints
273 /// \warning this method is protected for child classes that need to
274 /// initialize themselves before being sure that the initial and
275 /// end configuration satisfy the constraints
276 ✗ void constraints(const ConstraintSetPtr_t& constraint) {
277 ✗ constraints_ = constraint;
278 }
279
280 /// Should be called by child classes after having init.
281 virtual void checkPath() const;
282
283 1852923 void timeRange(const interval_t& timeRange) {
284 1852923 timeRange_ = timeRange;
285
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1852920 times.
 1852923 if (timeParam_) {
286 3 paramRange_.first = timeParam_->value(timeRange_.first);
287 3 paramRange_.second = timeParam_->value(timeRange_.second);
288 } else
289 1852920 paramRange_ = timeRange_;
290 1852923 }
291
292 4826515 value_type paramLength() const {
293 4826515 return paramRange_.second - paramRange_.first;
294 }
295
296 1090889 Configuration_t configAtParam(const value_type& param, bool& success) const {
297
1/2
✓ Branch 2 taken 1090889 times.
✗ Branch 3 not taken.
 1090889 Configuration_t result(outputSize());
298
2/4
✓ Branch 1 taken 1090889 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1090889 times.
✗ Branch 5 not taken.
 1090889 success = impl_compute(result, param);
299
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1090889 times.
 1090889 if (!success) return result;
300
2/4
✓ Branch 1 taken 1090889 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1090889 times.
✗ Branch 5 not taken.
 1090889 success = applyConstraints(result, param);
301 1090889 return result;
302 }
303
304 /// \brief Function evaluation without applying constraints
305 ///
306 /// \return true if everything went good.
307 virtual bool impl_compute(ConfigurationOut_t configuration,
308 value_type param) const = 0;
309
310 /// Virtual implementation of \ref derivative
311 /// \param param parameter within \ref paramRange
312 /// \param order order of derivation.
313 /// \retval derivative
314 ✗ virtual void impl_derivative(vectorOut_t derivative, const value_type& param,
315 size_type order) const {
316 (void)derivative;
317 (void)param;
318 (void)order;
319 ✗ HPP_THROW_EXCEPTION(hpp::Exception, "not implemented");
320 }
321
322 /// Virtual implementation of \ref velocityBound
323 /// \param param0, param1 interval of parameter
324 /// \retval bound
325 ✗ virtual void impl_velocityBound(vectorOut_t bound, const value_type& param0,
326 const value_type& param1) const {
327 (void)bound;
328 (void)param0;
329 (void)param1;
330 ✗ HPP_THROW_EXCEPTION(hpp::Exception, "not implemented");
331 }
332
333 /// Virtual implementation of \ref extract
334 virtual PathPtr_t impl_extract(const interval_t& paramInterval) const;
335
336 private:
337 /// Interval of definition
338 interval_t timeRange_;
339
340 2196189 value_type paramAtTime(const value_type& time) const {
341
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2196286 times.
 2196189 if (timeParam_) {
342 ✗ return timeParam_->value(time);
343 }
344 2196286 return time;
345 }
346
347 bool applyConstraints(ConfigurationOut_t result,
348 const value_type& param) const;
349
350 /// Size of the configuration space
351 size_type outputSize_;
352 /// Number of degrees of freedom of the robot
353 size_type outputDerivativeSize_;
354 /// Constraints that apply to the robot
355 ConstraintSetPtr_t constraints_;
356 /// Time parameterization
357 TimeParameterizationPtr_t timeParam_;
358 /// Weak pointer to itself
359 PathWkPtr_t weak_;
360 friend std::ostream& operator<<(std::ostream& os, const Path& path);
361 friend class ExtractedPath;
362
363 protected:
364 15 Path() {}
365
366 private:
367 HPP_SERIALIZABLE();
368 }; // class Path
369 ✗ inline std::ostream& operator<<(std::ostream& os, const Path& path) {
370 ✗ return path.print(os);
371 }
372 /// \}
373
374 } // namespace core
375 } // namespace hpp
376 #endif // HPP_CORE_PATH_HH
377