Directory: | ./ |
---|---|
File: | include/hpp/core/path.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 52 | 63 | 82.5% |
Branches: | 25 | 58 | 43.1% |
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 | 7487008 | 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 | 999233 | bool eval(ConfigurationOut_t result, const value_type& time) const { | |
136 |
1/2✓ Branch 1 taken 999233 times.
✗ Branch 2 not taken.
|
999233 | value_type s = paramAtTime(time); |
137 |
2/4✓ Branch 1 taken 999233 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 999232 times.
✗ Branch 5 not taken.
|
999233 | bool success = impl_compute(result, s); |
138 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 999232 times.
|
999232 | if (!success) return false; |
139 |
2/4✓ Branch 1 taken 999233 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 999232 times.
✗ Branch 5 not taken.
|
999232 | 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 | 27244 | 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 27262 times.
|
27244 | assert(result.size() == outputDerivativeSize()); |
174 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 27262 times.
|
27262 | assert(t0 <= t1); |
175 |
3/6✓ Branch 2 taken 27229 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27269 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27166 times.
✗ Branch 9 not taken.
|
27226 | impl_velocityBound(result, paramAtTime(std::max(t0, timeRange().first)), |
176 | 27262 | paramAtTime(std::min(t1, timeRange().second))); | |
177 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 27166 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
27166 | if (timeParam_) result *= timeParam_->derivativeBound(t0, t1); |
178 | 27166 | } | |
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 | 2813228 | size_type outputDerivativeSize() const { return outputDerivativeSize_; } | |
188 | |||
189 | /// Get interval of definition | ||
190 | 3192977 | const interval_t& timeRange() const { return timeRange_; } | |
191 | |||
192 | /// Get length of definition interval | ||
193 | 18903588 | virtual value_type length() const { | |
194 | 18903588 | 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 | 24235595 | 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 | 6659368 | const interval_t& paramRange() const { return paramRange_; } | |
218 | |||
219 | /// Get the time parameterization function | ||
220 | 1857638 | const TimeParameterizationPtr_t& timeParameterization() const { | |
221 | 1857638 | 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 | 1852919 | void timeRange(const interval_t& timeRange) { | |
284 | 1852919 | timeRange_ = timeRange; | |
285 |
2/2✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1852916 times.
|
1852919 | if (timeParam_) { |
286 | 3 | paramRange_.first = timeParam_->value(timeRange_.first); | |
287 | 3 | paramRange_.second = timeParam_->value(timeRange_.second); | |
288 | } else | ||
289 | 1852916 | paramRange_ = timeRange_; | |
290 | 1852919 | } | |
291 | |||
292 | 4749569 | value_type paramLength() const { | |
293 | 4749569 | 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 | 2144764 | value_type paramAtTime(const value_type& time) const { | |
341 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2144763 times.
|
2144764 | if (timeParam_) { |
342 | 2 | return timeParam_->value(time); | |
343 | } | ||
344 | 2144763 | 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 |