hpp-core  4.10.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 // 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 <boost/concept_check.hpp>
23 # include <hpp/util/exception.hh>
24 # include <hpp/util/serialization-fwd.hh>
25 # include <hpp/core/fwd.hh>
26 # include <hpp/core/config.hh>
28 # include <hpp/core/deprecated.hh>
31 
32 namespace hpp {
33  namespace core {
37 
62  {
63  public:
66 
68  virtual ~Path () {}
69 
71  virtual PathPtr_t copy () const = 0;
72 
77  virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const = 0;
78 
80  template <class T> boost::shared_ptr<T> as (void)
81  {
82  assert (HPP_DYNAMIC_PTR_CAST (T, weak_.lock ()));
83  return HPP_STATIC_PTR_CAST (T, weak_.lock ());
84  }
85 
87  template <class T> boost::shared_ptr<const T> as (void) const
88  {
89  assert (HPP_DYNAMIC_PTR_CAST (const T, weak_.lock ()));
90  return HPP_STATIC_PTR_CAST (const T, weak_.lock ());
91  }
92 
94 
97 
104  PathPtr_t extract (const interval_t& subInterval) const;
105 
107  PathPtr_t extract (const value_type& tmin, const value_type& tmax) const
108  {
109  return extract (std::make_pair(tmin, tmax));
110  }
111 
114  virtual PathPtr_t reverse () const;
115 
117 
120 
122  Configuration_t operator () (const value_type& time) const
124  {
125  bool unused;
126  return (*this) (time, unused);
127  }
128 
129  Configuration_t operator () (const value_type& time, bool& success) const
130  {
131  return configAtParam (paramAtTime(time), success);
132  }
133 
134  bool operator () (ConfigurationOut_t result, const value_type& time)
135  const
136  {
137  value_type s = paramAtTime (time);
138  bool success = impl_compute (result, s);
139  if (!success) return false;
140  return applyConstraints (result, s);
141  }
142 
143  Configuration_t eval (const value_type& time, bool& success) const
144  {
145  return this->operator() (time, success);
146  }
147 
148  bool eval (ConfigurationOut_t result, const value_type& time)
149  const
150  {
151  return this->operator() (result, time);
152  }
153 
155  bool at (const value_type& time, ConfigurationOut_t result) const
156  {
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 
183  void velocityBound (vectorOut_t result, const value_type& t0, const value_type& t1) const
184  {
185  assert(result.size() == outputDerivativeSize());
186  assert(t0 <= t1);
187  impl_velocityBound (result,
188  paramAtTime (std::max(t0, timeRange().first )),
189  paramAtTime (std::min(t1, timeRange().second)));
190  if (timeParam_)
191  result *= timeParam_->derivativeBound (t0, t1);
192  }
193 
196 
199  {
200  return outputSize_;
201  }
202 
205  {
206  return outputDerivativeSize_;
207  }
208 
210  const interval_t& timeRange () const
211  {
212  return timeRange_;
213  }
214 
216  virtual value_type length () const
217  {
218  return timeRange_.second - timeRange_.first;
219  }
220 
222  virtual Configuration_t initial () const = 0;
223 
225  virtual Configuration_t end () const = 0;
226 
229  {
230  return constraints_;
231  }
232 
234 
239 
244  const interval_t& paramRange () const
245  {
246  return paramRange_;
247  }
248 
251  const interval_t& tr)
252  {
253  timeParam_ = tp;
254  timeRange (tr);
255  }
256 
258 
259  protected:
262  virtual std::ostream& print (std::ostream &os) const;
263 
273  Path (const interval_t& interval, size_type outputSize,
274  size_type outputDerivativeSize,
275  const ConstraintSetPtr_t& constraints);
276 
282  Path (const interval_t& interval, size_type outputSize,
283  size_type outputDerivativeSize);
284 
286  Path (const Path& path);
287 
289  Path (const Path& path, const ConstraintSetPtr_t& constraints);
290 
294  void init (const PathWkPtr_t& self);
295 
298 
303  void constraints (const ConstraintSetPtr_t& constraint) {
304  constraints_ = constraint;
305  }
306 
308  virtual void checkPath () const;
309 
310  void timeRange (const interval_t& timeRange)
311  {
312  timeRange_ = timeRange;
313  if (timeParam_) {
314  paramRange_.first = timeParam_->value(timeRange_.first );
315  paramRange_.second = timeParam_->value(timeRange_.second);
316  } else
317  paramRange_ = timeRange_;
318  }
319 
321  {
322  return timeParam_;
323  }
324 
326  {
327  return paramRange_.second - paramRange_.first;
328  }
329 
330  Configuration_t configAtParam (const value_type& param, bool& success) const
331  {
332  Configuration_t result (outputSize ());
333  success = impl_compute (result, param);
334  if (!success) return result;
335  success = applyConstraints (result, param);
336  return result;
337  }
338 
342  virtual bool impl_compute (ConfigurationOut_t configuration,
343  value_type param) const = 0;
344 
349  virtual void impl_derivative (vectorOut_t derivative,
350  const value_type& param,
351  size_type order) const
352  {
353  (void) derivative;
354  (void) param;
355  (void) order;
356  HPP_THROW_EXCEPTION (hpp::Exception, "not implemented");
357  }
358 
362  virtual void impl_velocityBound (vectorOut_t bound,
363  const value_type& param0,
364  const value_type& param1) const
365  {
366  (void) bound;
367  (void) param0;
368  (void) param1;
369  HPP_THROW_EXCEPTION (hpp::Exception, "not implemented");
370  }
371 
373  virtual PathPtr_t impl_extract (const interval_t& paramInterval) const;
374 
375  private:
377  interval_t timeRange_;
378 
379  value_type paramAtTime (const value_type& time) const
380  {
381  if (timeParam_) {
382  return timeParam_->value (time);
383  }
384  return time;
385  }
386 
387  bool applyConstraints (ConfigurationOut_t result, const value_type& param) const;
388 
390  size_type outputSize_;
392  size_type outputDerivativeSize_;
394  ConstraintSetPtr_t constraints_;
396  TimeParameterizationPtr_t timeParam_;
398  PathWkPtr_t weak_;
399  friend std::ostream& operator<< (std::ostream& os, const Path& path);
400  friend class ExtractedPath;
401 
402  protected:
403  Path() {}
404  private:
405  HPP_SERIALIZABLE();
406  }; // class Path
407  inline std::ostream& operator<< (std::ostream& os, const Path& path)
408  {
409  return path.print (os);
410  }
412 
413  } // namespace core
414 } // namespace hpp
415 #endif // HPP_CORE_PATH_HH
boost::shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
virtual ~Path()
Destructor.
Definition: path.hh:68
size_type outputSize() const
Get size of configuration space.
Definition: path.hh:198
value_type paramLength() const
Definition: path.hh:325
Definition: basic-configuration-shooter.hh:26
size_type outputDerivativeSize() const
Get size of velocity.
Definition: path.hh:204
const interval_t & paramRange() const
Definition: path.hh:244
boost::shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
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:183
PathPtr_t extract(const value_type &tmin, const value_type &tmax) const
Definition: path.hh:107
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:310
void timeParameterization(const TimeParameterizationPtr_t &tp, const interval_t &tr)
Set the time parameterization function.
Definition: path.hh:250
virtual value_type length() const
Get length of definition interval.
Definition: path.hh:216
const ConstraintSetPtr_t & constraints() const
Get constraints the path is subject to.
Definition: path.hh:228
Configuration_t configAtParam(const value_type &param, bool &success) const
Definition: path.hh:330
boost::shared_ptr< const T > as(void) const
Static cast into a derived type.
Definition: path.hh:87
interval_t paramRange_
Interval of parameters.
Definition: path.hh:297
Path()
Definition: path.hh:403
const interval_t & timeRange() const
Get interval of definition.
Definition: path.hh:210
Configuration_t eval(const value_type &time, bool &success) const
Definition: path.hh:143
bool at(const value_type &time, ConfigurationOut_t result) const
Get the configuration at a parameter without applying the constraints.
Definition: path.hh:155
pinocchio::value_type value_type
Definition: fwd.hh:157
boost::shared_ptr< TimeParameterization > TimeParameterizationPtr_t
Definition: fwd.hh:172
virtual void impl_derivative(vectorOut_t derivative, const value_type &param, size_type order) const
Definition: path.hh:349
virtual std::ostream & print(std::ostream &os) const
void constraints(const ConstraintSetPtr_t &constraint)
Definition: path.hh:303
constraints::Implicit NumericalConstraint HPP_CORE_DEPRECATED
Definition: fwd.hh:347
const TimeParameterizationPtr_t & timeParameterization() const
Definition: path.hh:320
bool eval(ConfigurationOut_t result, const value_type &time) const
Definition: path.hh:148
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:203
virtual void impl_velocityBound(vectorOut_t bound, const value_type &param0, const value_type &param1) const
Definition: path.hh:362
std::ostream & operator<<(std::ostream &os, const Constraint &constraint)
Definition: constraint.hh:101
Definition: path.hh:61
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
boost::shared_ptr< T > as(void)
Static cast into a derived type.
Definition: path.hh:80