hpp-core 4.15.1
Implement basic classes for canonical path planning for kinematic chains.
Loading...
Searching...
No Matches
path.hh
Go to the documentation of this file.
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>
36#include <hpp/core/fwd.hh>
39#include <hpp/util/exception.hh>
40#include <hpp/util/serialization-fwd.hh>
41
42namespace hpp {
43namespace core {
47
72 public:
75
77 virtual ~Path() {}
78
80 virtual PathPtr_t copy() const = 0;
81
86 virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const = 0;
87
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
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
103
106
113 PathPtr_t extract(const interval_t& subInterval) const;
114
116 PathPtr_t extract(const value_type& tmin, const value_type& tmax) const {
117 return extract(std::make_pair(tmin, tmax));
118 }
119
122 virtual PathPtr_t reverse() const;
123
125
128
132 bool& success) const {
133 return eval(time, success);
134 }
135
138 bool operator()(ConfigurationOut_t result, const value_type& time) const {
139 return eval(result, time);
140 }
141
143 Configuration_t eval(const value_type& time, bool& success) const {
144 return configAtParam(paramAtTime(time), success);
145 }
146
148 bool eval(ConfigurationOut_t result, const value_type& time) const {
149 value_type s = paramAtTime(time);
150 bool success = impl_compute(result, s);
151 if (!success) return false;
152 return applyConstraints(result, s);
153 }
154
156 bool at(const value_type& time, ConfigurationOut_t result) const {
157 return impl_compute(result, paramAtTime(time));
158 }
159
169 void derivative(vectorOut_t result, const value_type& time,
170 size_type order) const;
171
184 void velocityBound(vectorOut_t result, const value_type& t0,
185 const value_type& t1) const {
186 assert(result.size() == outputDerivativeSize());
187 assert(t0 <= t1);
188 impl_velocityBound(result, paramAtTime(std::max(t0, timeRange().first)),
189 paramAtTime(std::min(t1, timeRange().second)));
190 if (timeParam_) result *= timeParam_->derivativeBound(t0, t1);
191 }
192
195
197 size_type outputSize() const { return outputSize_; }
198
200 size_type outputDerivativeSize() const { return outputDerivativeSize_; }
201
203 const interval_t& timeRange() const { return timeRange_; }
204
206 virtual value_type length() const {
207 return timeRange_.second - timeRange_.first;
208 }
209
211 virtual Configuration_t initial() const = 0;
212
214 virtual Configuration_t end() const = 0;
215
217 const ConstraintSetPtr_t& constraints() const { return constraints_; }
218
220
225
230 const interval_t& paramRange() const { return paramRange_; }
231
234 const interval_t& tr) {
235 timeParam_ = tp;
236 timeRange(tr);
237 }
238
240
241 protected:
244 virtual std::ostream& print(std::ostream& os) const;
245
255 Path(const interval_t& interval, size_type outputSize,
256 size_type outputDerivativeSize, const ConstraintSetPtr_t& constraints);
257
263 Path(const interval_t& interval, size_type outputSize,
264 size_type outputDerivativeSize);
265
267 Path(const Path& path);
268
270 Path(const Path& path, const ConstraintSetPtr_t& constraints);
271
275 void init(const PathWkPtr_t& self);
276
279
284 void constraints(const ConstraintSetPtr_t& constraint) {
285 constraints_ = constraint;
286 }
287
289 virtual void checkPath() const;
290
291 void timeRange(const interval_t& timeRange) {
292 timeRange_ = timeRange;
293 if (timeParam_) {
294 paramRange_.first = timeParam_->value(timeRange_.first);
295 paramRange_.second = timeParam_->value(timeRange_.second);
296 } else
297 paramRange_ = timeRange_;
298 }
299
301 return timeParam_;
302 }
303
305 return paramRange_.second - paramRange_.first;
306 }
307
308 Configuration_t configAtParam(const value_type& param, bool& success) const {
309 Configuration_t result(outputSize());
310 success = impl_compute(result, param);
311 if (!success) return result;
312 success = applyConstraints(result, param);
313 return result;
314 }
315
319 virtual bool impl_compute(ConfigurationOut_t configuration,
320 value_type param) const = 0;
321
326 virtual void impl_derivative(vectorOut_t derivative, const value_type& param,
327 size_type order) const {
328 (void)derivative;
329 (void)param;
330 (void)order;
331 HPP_THROW_EXCEPTION(hpp::Exception, "not implemented");
332 }
333
337 virtual void impl_velocityBound(vectorOut_t bound, const value_type& param0,
338 const value_type& param1) const {
339 (void)bound;
340 (void)param0;
341 (void)param1;
342 HPP_THROW_EXCEPTION(hpp::Exception, "not implemented");
343 }
344
346 virtual PathPtr_t impl_extract(const interval_t& paramInterval) const;
347
348 private:
350 interval_t timeRange_;
351
352 value_type paramAtTime(const value_type& time) const {
353 if (timeParam_) {
354 return timeParam_->value(time);
355 }
356 return time;
357 }
358
359 bool applyConstraints(ConfigurationOut_t result,
360 const value_type& param) const;
361
363 size_type outputSize_;
365 size_type outputDerivativeSize_;
367 ConstraintSetPtr_t constraints_;
369 TimeParameterizationPtr_t timeParam_;
371 PathWkPtr_t weak_;
372 friend std::ostream& operator<<(std::ostream& os, const Path& path);
373 friend class ExtractedPath;
374
375 protected:
376 Path() {}
377
378 private:
379 HPP_SERIALIZABLE();
380}; // class Path
381inline std::ostream& operator<<(std::ostream& os, const Path& path) {
382 return path.print(os);
383}
385
386} // namespace core
387} // namespace hpp
388#endif // HPP_CORE_PATH_HH
Definition: path.hh:71
Configuration_t eval(const value_type &time, bool &success) const
Configuration at time.
Definition: path.hh:143
HPP_CORE_DEPRECATED Configuration_t operator()(const value_type &time, bool &success) const
Definition: path.hh:131
size_type outputSize() const
Get size of configuration space.
Definition: path.hh:197
void init(const PathWkPtr_t &self)
virtual std::ostream & print(std::ostream &os) const
value_type paramLength() const
Definition: path.hh:304
size_type outputDerivativeSize() const
Get size of velocity.
Definition: path.hh:200
virtual ~Path()
Destructor.
Definition: path.hh:77
const interval_t & paramRange() const
Definition: path.hh:230
virtual void impl_derivative(vectorOut_t derivative, const value_type &param, size_type order) const
Definition: path.hh:326
PathPtr_t extract(const interval_t &subInterval) const
shared_ptr< T > as(void)
Static cast into a derived type.
Definition: path.hh:90
virtual bool impl_compute(ConfigurationOut_t configuration, value_type param) const =0
Function evaluation without applying constraints.
void timeParameterization(const TimeParameterizationPtr_t &tp, const interval_t &tr)
Set the time parameterization function.
Definition: path.hh:233
shared_ptr< const T > as(void) const
Static cast into a derived type.
Definition: path.hh:97
bool operator()(ConfigurationOut_t result, const value_type &time) const
Definition: path.hh:138
virtual PathPtr_t impl_extract(const interval_t &paramInterval) const
Virtual implementation of extract.
Path(const interval_t &interval, size_type outputSize, size_type outputDerivativeSize)
bool at(const value_type &time, ConfigurationOut_t result) const
Get the configuration at a parameter without applying the constraints.
Definition: path.hh:156
interval_t paramRange_
Interval of parameters.
Definition: path.hh:278
virtual PathPtr_t copy() const =0
Return a shared pointer to a copy of this.
PathPtr_t extract(const value_type &tmin, const value_type &tmax) const
Definition: path.hh:116
virtual PathPtr_t reverse() const
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const =0
const TimeParameterizationPtr_t & timeParameterization() const
Definition: path.hh:300
void constraints(const ConstraintSetPtr_t &constraint)
Definition: path.hh:284
void velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
Definition: path.hh:184
const interval_t & timeRange() const
Get interval of definition.
Definition: path.hh:203
void derivative(vectorOut_t result, const value_type &time, size_type order) const
void timeRange(const interval_t &timeRange)
Definition: path.hh:291
Path(const Path &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
Path()
Definition: path.hh:376
bool eval(ConfigurationOut_t result, const value_type &time) const
Configuration at time.
Definition: path.hh:148
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition: path.hh:217
virtual void impl_velocityBound(vectorOut_t bound, const value_type &param0, const value_type &param1) const
Definition: path.hh:337
Path(const Path &path)
Copy constructor.
virtual Configuration_t end() const =0
Get the final configuration.
virtual value_type length() const
Get length of definition interval.
Definition: path.hh:206
Configuration_t configAtParam(const value_type &param, bool &success) const
Definition: path.hh:308
virtual void checkPath() const
Should be called by child classes after having init.
Path(const interval_t &interval, size_type outputSize, size_type outputDerivativeSize, const ConstraintSetPtr_t &constraints)
virtual Configuration_t initial() const =0
Get the initial configuration.
#define HPP_CORE_DLLAPI
Definition: config.hh:64
#define HPP_CORE_DEPRECATED
Definition: deprecated.hh:37
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition: constraint.hh:99
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:189
pinocchio::value_type value_type
Definition: fwd.hh:174
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:222
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:108
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:175
pinocchio::size_type size_type
Definition: fwd.hh:173
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:106
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187
Definition: bi-rrt-planner.hh:35