hpp-core  4.15.1
Implement basic classes for canonical path planning for kinematic chains.
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>
35 #include <hpp/core/deprecated.hh>
36 #include <hpp/core/fwd.hh>
39 #include <hpp/util/exception.hh>
40 #include <hpp/util/serialization-fwd.hh>
41 
42 namespace hpp {
43 namespace 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
381 inline 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
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:189
virtual ~Path()
Destructor.
Definition: path.hh:77
size_type outputSize() const
Get size of configuration space.
Definition: path.hh:197
value_type paramLength() const
Definition: path.hh:304
Definition: bi-rrt-planner.hh:35
size_type outputDerivativeSize() const
Get size of velocity.
Definition: path.hh:200
const interval_t & paramRange() const
Definition: path.hh:230
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:175
void velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
Definition: path.hh:184
PathPtr_t extract(const value_type &tmin, const value_type &tmax) const
Definition: path.hh:116
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:108
pinocchio::size_type size_type
Definition: fwd.hh:173
void timeRange(const interval_t &timeRange)
Definition: path.hh:291
void timeParameterization(const TimeParameterizationPtr_t &tp, const interval_t &tr)
Set the time parameterization function.
Definition: path.hh:233
shared_ptr< T > as(void)
Static cast into a derived type.
Definition: path.hh:90
virtual value_type length() const
Get length of definition interval.
Definition: path.hh:206
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition: path.hh:217
Configuration_t configAtParam(const value_type &param, bool &success) const
Definition: path.hh:308
interval_t paramRange_
Interval of parameters.
Definition: path.hh:278
Path()
Definition: path.hh:376
const interval_t & timeRange() const
Get interval of definition.
Definition: path.hh:203
Configuration_t eval(const value_type &time, bool &success) const
Configuration at time.
Definition: path.hh:143
#define HPP_CORE_DEPRECATED
Definition: deprecated.hh:37
bool at(const value_type &time, ConfigurationOut_t result) const
Get the configuration at a parameter without applying the constraints.
Definition: path.hh:156
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
HPP_CORE_DEPRECATED Configuration_t operator()(const value_type &time, bool &success) const
Definition: path.hh:131
shared_ptr< const T > as(void) const
Static cast into a derived type.
Definition: path.hh:97
pinocchio::value_type value_type
Definition: fwd.hh:174
virtual void impl_derivative(vectorOut_t derivative, const value_type &param, size_type order) const
Definition: path.hh:326
bool operator()(ConfigurationOut_t result, const value_type &time) const
Definition: path.hh:138
virtual std::ostream & print(std::ostream &os) const
void constraints(const ConstraintSetPtr_t &constraint)
Definition: path.hh:284
const TimeParameterizationPtr_t & timeParameterization() const
Definition: path.hh:300
bool eval(ConfigurationOut_t result, const value_type &time) const
Configuration at time.
Definition: path.hh:148
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:222
virtual void impl_velocityBound(vectorOut_t bound, const value_type &param0, const value_type &param1) const
Definition: path.hh:337
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition: constraint.hh:99
Definition: path.hh:71
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:106
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:187