hpp-core  4.12.0
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 // This file is part of hpp-core
6 // hpp-core is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-core is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_CORE_PATH_HH
20 # define HPP_CORE_PATH_HH
21 
22 # include <hpp/util/exception.hh>
23 # include <hpp/util/serialization-fwd.hh>
24 # include <hpp/core/fwd.hh>
25 # include <hpp/core/config.hh>
27 # include <hpp/core/deprecated.hh>
30 
31 namespace hpp {
32  namespace core {
36 
61  {
62  public:
65 
67  virtual ~Path () {}
68 
70  virtual PathPtr_t copy () const = 0;
71 
76  virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const = 0;
77 
79  template <class T> shared_ptr<T> as (void)
80  {
81  assert (HPP_DYNAMIC_PTR_CAST (T, weak_.lock ()));
82  return HPP_STATIC_PTR_CAST (T, weak_.lock ());
83  }
84 
86  template <class T> shared_ptr<const T> as (void) const
87  {
88  assert (HPP_DYNAMIC_PTR_CAST (const T, weak_.lock ()));
89  return HPP_STATIC_PTR_CAST (const T, weak_.lock ());
90  }
91 
93 
96 
103  PathPtr_t extract (const interval_t& subInterval) const;
104 
106  PathPtr_t extract (const value_type& tmin, const value_type& tmax) const
107  {
108  return extract (std::make_pair(tmin, tmax));
109  }
110 
113  virtual PathPtr_t reverse () const;
114 
116 
119 
122  Configuration_t operator () (const value_type& time, bool& success) const
124  {
125  return eval (time, success);
126  }
127 
130  bool operator () (ConfigurationOut_t result, const value_type& time)
131  const
132  {
133  return eval(result, time);
134  }
135 
137  Configuration_t eval (const value_type& time, bool& success) const
138  {
139  return configAtParam (paramAtTime(time), success);
140  }
141 
143  bool eval (ConfigurationOut_t result, const value_type& time)
144  const
145  {
146  value_type s = paramAtTime (time);
147  bool success = impl_compute (result, s);
148  if (!success) return false;
149  return applyConstraints (result, s);
150  }
151 
153  bool at (const value_type& time, ConfigurationOut_t result) const
154  {
155  return impl_compute (result, paramAtTime(time));
156  }
157 
167  void derivative (vectorOut_t result, const value_type& time,
168  size_type order) const;
169 
181  void velocityBound (vectorOut_t result, const value_type& t0, const value_type& t1) const
182  {
183  assert(result.size() == outputDerivativeSize());
184  assert(t0 <= t1);
185  impl_velocityBound (result,
186  paramAtTime (std::max(t0, timeRange().first )),
187  paramAtTime (std::min(t1, timeRange().second)));
188  if (timeParam_)
189  result *= timeParam_->derivativeBound (t0, t1);
190  }
191 
194 
197  {
198  return outputSize_;
199  }
200 
203  {
204  return outputDerivativeSize_;
205  }
206 
208  const interval_t& timeRange () const
209  {
210  return timeRange_;
211  }
212 
214  virtual value_type length () const
215  {
216  return timeRange_.second - timeRange_.first;
217  }
218 
220  virtual Configuration_t initial () const = 0;
221 
223  virtual Configuration_t end () const = 0;
224 
227  {
228  return constraints_;
229  }
230 
232 
237 
242  const interval_t& paramRange () const
243  {
244  return paramRange_;
245  }
246 
249  const interval_t& tr)
250  {
251  timeParam_ = tp;
252  timeRange (tr);
253  }
254 
256 
257  protected:
260  virtual std::ostream& print (std::ostream &os) const;
261 
271  Path (const interval_t& interval, size_type outputSize,
272  size_type outputDerivativeSize,
273  const ConstraintSetPtr_t& constraints);
274 
280  Path (const interval_t& interval, size_type outputSize,
281  size_type outputDerivativeSize);
282 
284  Path (const Path& path);
285 
287  Path (const Path& path, const ConstraintSetPtr_t& constraints);
288 
292  void init (const PathWkPtr_t& self);
293 
296 
301  void constraints (const ConstraintSetPtr_t& constraint) {
302  constraints_ = constraint;
303  }
304 
306  virtual void checkPath () const;
307 
308  void timeRange (const interval_t& timeRange)
309  {
310  timeRange_ = timeRange;
311  if (timeParam_) {
312  paramRange_.first = timeParam_->value(timeRange_.first );
313  paramRange_.second = timeParam_->value(timeRange_.second);
314  } else
315  paramRange_ = timeRange_;
316  }
317 
319  {
320  return timeParam_;
321  }
322 
324  {
325  return paramRange_.second - paramRange_.first;
326  }
327 
328  Configuration_t configAtParam (const value_type& param, bool& success) const
329  {
330  Configuration_t result (outputSize ());
331  success = impl_compute (result, param);
332  if (!success) return result;
333  success = applyConstraints (result, param);
334  return result;
335  }
336 
340  virtual bool impl_compute (ConfigurationOut_t configuration,
341  value_type param) const = 0;
342 
347  virtual void impl_derivative (vectorOut_t derivative,
348  const value_type& param,
349  size_type order) const
350  {
351  (void) derivative;
352  (void) param;
353  (void) order;
354  HPP_THROW_EXCEPTION (hpp::Exception, "not implemented");
355  }
356 
360  virtual void impl_velocityBound (vectorOut_t bound,
361  const value_type& param0,
362  const value_type& param1) const
363  {
364  (void) bound;
365  (void) param0;
366  (void) param1;
367  HPP_THROW_EXCEPTION (hpp::Exception, "not implemented");
368  }
369 
371  virtual PathPtr_t impl_extract (const interval_t& paramInterval) const;
372 
373  private:
375  interval_t timeRange_;
376 
377  value_type paramAtTime (const value_type& time) const
378  {
379  if (timeParam_) {
380  return timeParam_->value (time);
381  }
382  return time;
383  }
384 
385  bool applyConstraints (ConfigurationOut_t result, const value_type& param) const;
386 
388  size_type outputSize_;
390  size_type outputDerivativeSize_;
392  ConstraintSetPtr_t constraints_;
394  TimeParameterizationPtr_t timeParam_;
396  PathWkPtr_t weak_;
397  friend std::ostream& operator<< (std::ostream& os, const Path& path);
398  friend class ExtractedPath;
399 
400  protected:
401  Path() {}
402  private:
403  HPP_SERIALIZABLE();
404  }; // class Path
405  inline std::ostream& operator<< (std::ostream& os, const Path& path)
406  {
407  return path.print (os);
408  }
410 
411  } // namespace core
412 } // namespace hpp
413 #endif // HPP_CORE_PATH_HH
shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:172
virtual ~Path()
Destructor.
Definition: path.hh:67
size_type outputSize() const
Get size of configuration space.
Definition: path.hh:196
value_type paramLength() const
Definition: path.hh:323
Definition: bi-rrt-planner.hh:24
size_type outputDerivativeSize() const
Get size of velocity.
Definition: path.hh:202
const interval_t & paramRange() const
Definition: path.hh:242
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:158
void velocityBound(vectorOut_t result, const value_type &t0, const value_type &t1) const
Definition: path.hh:181
PathPtr_t extract(const value_type &tmin, const value_type &tmax) const
Definition: path.hh:106
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
pinocchio::size_type size_type
Definition: fwd.hh:156
void timeRange(const interval_t &timeRange)
Definition: path.hh:308
void timeParameterization(const TimeParameterizationPtr_t &tp, const interval_t &tr)
Set the time parameterization function.
Definition: path.hh:248
shared_ptr< T > as(void)
Static cast into a derived type.
Definition: path.hh:79
virtual value_type length() const
Get length of definition interval.
Definition: path.hh:214
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition: path.hh:226
Configuration_t configAtParam(const value_type &param, bool &success) const
Definition: path.hh:328
interval_t paramRange_
Interval of parameters.
Definition: path.hh:295
Path()
Definition: path.hh:401
const interval_t & timeRange() const
Get interval of definition.
Definition: path.hh:208
Configuration_t eval(const value_type &time, bool &success) const
Configuration at time.
Definition: path.hh:137
#define HPP_CORE_DEPRECATED
Definition: deprecated.hh:32
bool at(const value_type &time, ConfigurationOut_t result) const
Get the configuration at a parameter without applying the constraints.
Definition: path.hh:153
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
shared_ptr< const T > as(void) const
Static cast into a derived type.
Definition: path.hh:86
pinocchio::value_type value_type
Definition: fwd.hh:157
virtual void impl_derivative(vectorOut_t derivative, const value_type &param, size_type order) const
Definition: path.hh:347
virtual std::ostream & print(std::ostream &os) const
void constraints(const ConstraintSetPtr_t &constraint)
Definition: path.hh:301
const TimeParameterizationPtr_t & timeParameterization() const
Definition: path.hh:318
bool eval(ConfigurationOut_t result, const value_type &time) const
Configuration at time.
Definition: path.hh:143
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:204
virtual void impl_velocityBound(vectorOut_t bound, const value_type &param0, const value_type &param1) const
Definition: path.hh:360
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition: constraint.hh:101
Definition: path.hh:60
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170