hpp-core  4.13.0
Implement basic classes for canonical path planning for kinematic chains.
path-vector.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_VECTOR_HH
31 #define HPP_CORE_PATH_VECTOR_HH
32 
33 #include <hpp/core/fwd.hh>
34 #include <hpp/core/path.hh>
35 #include <hpp/pinocchio/device.hh>
36 
37 namespace hpp {
38 namespace core {
41 
44  public:
45  typedef Path parent_t;
48 
50  static PathVectorPtr_t create(size_type outputSize,
51  size_type outputDerivativeSize) {
52  PathVector* ptr = new PathVector(outputSize, outputDerivativeSize);
53  PathVectorPtr_t shPtr(ptr);
54  ptr->init(shPtr);
55  return shPtr;
56  }
57 
59  static PathVectorPtr_t create(size_type outputSize,
60  size_type outputDerivativeSize,
61  const ConstraintSetPtr_t& constraint) {
62  PathVector* ptr =
63  new PathVector(outputSize, outputDerivativeSize, constraint);
64  PathVectorPtr_t shPtr(ptr);
65  ptr->init(shPtr);
66  return shPtr;
67  }
68 
70  static PathVectorPtr_t createCopy(const PathVectorPtr_t& original) {
71  PathVector* ptr = new PathVector(*original);
72  PathVectorPtr_t shPtr(ptr);
73  ptr->init(shPtr);
74  return shPtr;
75  }
76 
78  static PathVectorPtr_t createCopy(const PathVectorPtr_t& original,
79  const ConstraintSetPtr_t& constraints) {
80  PathVector* ptr = new PathVector(*original, constraints);
81  PathVectorPtr_t shPtr(ptr);
82  ptr->init(shPtr);
83  return shPtr;
84  }
85 
87  virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
88 
92  virtual PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
93  return createCopy(weak_.lock(), constraints);
94  }
95 
97  virtual ~PathVector() {}
99 
101  std::size_t numberPaths() const { return paths_.size(); }
102 
109  PathPtr_t pathAtRank(std::size_t rank) const;
110 
116  std::size_t rankAtParam(const value_type& param,
117  value_type& localParam) const;
118 
120  void appendPath(const PathPtr_t& path);
121 
125  void concatenate(const PathVector& path) HPP_CORE_DEPRECATED;
126 
131  void concatenate(const PathVectorPtr_t& path);
132 
134  virtual Configuration_t initial() const { return paths_.front()->initial(); }
135 
137  virtual Configuration_t end() const { return paths_.back()->end(); }
138 
141  void flatten(PathVectorPtr_t flattenedPath) const;
142 
144  virtual PathPtr_t reverse() const;
145 
146  protected:
148  virtual std::ostream& print(std::ostream& os) const;
150  PathVector(std::size_t outputSize, std::size_t outputDerivativeSize)
151  : parent_t(std::make_pair(0, 0), outputSize, outputDerivativeSize),
152  paths_() {}
154  PathVector(std::size_t outputSize, std::size_t outputDerivativeSize,
155  const ConstraintSetPtr_t& constraint)
156  : parent_t(std::make_pair(0, 0), outputSize, outputDerivativeSize,
157  constraint),
158  paths_() {}
160  PathVector(const PathVector& path) : parent_t(path), paths_() {
161  assert(timeRange() == path.timeRange());
162  for (Paths_t::const_iterator it = path.paths_.begin();
163  it != path.paths_.end(); it++) {
164  paths_.push_back((*it)->copy());
165  }
166  }
167 
169  PathVector(const PathVector& path, const ConstraintSetPtr_t& constraints)
170  : parent_t(path, constraints), paths_() {
171  assert(timeRange() == path.timeRange());
172  for (Paths_t::const_iterator it = path.paths_.begin();
173  it != path.paths_.end(); it++) {
174  paths_.push_back((*it)->copy());
175  }
176  }
177 
178  void init(PathVectorPtr_t self) {
179  parent_t::init(self);
180  weak_ = self;
181  }
182  virtual bool impl_compute(ConfigurationOut_t result, value_type t) const;
184  virtual void impl_derivative(vectorOut_t result, const value_type& t,
185  size_type order) const;
188  virtual PathPtr_t impl_extract(const interval_t& subInterval) const;
189 
191  virtual void impl_velocityBound(vectorOut_t bound, const value_type& param0,
192  const value_type& param1) const;
193 
194  private:
195  Paths_t paths_;
196  PathVectorWkPtr_t weak_;
197 
198  protected:
200 
201  private:
202  HPP_SERIALIZABLE();
203 }; // class PathVector
205 } // namespace core
206 } // namespace hpp
207 
208 BOOST_CLASS_EXPORT_KEY(hpp::core::PathVector)
209 
210 #endif // HPP_CORE_PATH_VECTOR_HH
PathVector(const PathVector &path)
Copy constructor.
Definition: path-vector.hh:160
static PathVectorPtr_t createCopy(const PathVectorPtr_t &original, const ConstraintSetPtr_t &constraints)
Create instance and return shared pointer.
Definition: path-vector.hh:78
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:182
Definition: bi-rrt-planner.hh:35
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition: path-vector.hh:92
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:164
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:107
pinocchio::size_type size_type
Definition: fwd.hh:162
Path parent_t
Definition: path-vector.hh:45
std::vector< PathPtr_t > Paths_t
Definition: fwd.hh:203
PathVector(const PathVector &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
Definition: path-vector.hh:169
std::size_t numberPaths() const
Get the number of sub paths.
Definition: path-vector.hh:101
virtual ~PathVector()
Destructor.
Definition: path-vector.hh:97
virtual Configuration_t initial() const
Get the initial configuration.
Definition: path-vector.hh:134
static PathVectorPtr_t create(size_type outputSize, size_type outputDerivativeSize, const ConstraintSetPtr_t &constraint)
Create instance and return shared pointer.
Definition: path-vector.hh:59
virtual PathPtr_t copy() const
Return a shared pointer to a copy of this.
Definition: path-vector.hh:87
const interval_t & timeRange() const
Get interval of definition.
Definition: path.hh:203
PathVector(std::size_t outputSize, std::size_t outputDerivativeSize, const ConstraintSetPtr_t &constraint)
Constructor.
Definition: path-vector.hh:154
#define HPP_CORE_DEPRECATED
Definition: deprecated.hh:32
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:119
pinocchio::value_type value_type
Definition: fwd.hh:163
Concatenation of several paths.
Definition: path-vector.hh:43
static PathVectorPtr_t createCopy(const PathVectorPtr_t &original)
Create instance and return shared pointer.
Definition: path-vector.hh:70
virtual Configuration_t end() const
Get the final configuration.
Definition: path-vector.hh:137
PathVector(std::size_t outputSize, std::size_t outputDerivativeSize)
Constructor.
Definition: path-vector.hh:150
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:211
void init(PathVectorPtr_t self)
Definition: path-vector.hh:178
PathVector()
Definition: path-vector.hh:199
Definition: path.hh:71
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:105
static PathVectorPtr_t create(size_type outputSize, size_type outputDerivativeSize)
Create instance and return shared pointer.
Definition: path-vector.hh:50
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:176