hpp-core  4.12.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 // 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_VECTOR_HH
20 # define HPP_CORE_PATH_VECTOR_HH
21 
22 # include <hpp/pinocchio/device.hh>
23 # include <hpp/core/fwd.hh>
24 # include <hpp/core/path.hh>
25 
26 namespace hpp {
27  namespace core {
30 
33  {
34  public:
35  typedef Path parent_t;
38 
40  static PathVectorPtr_t create (size_type outputSize,
41  size_type outputDerivativeSize)
42  {
43  PathVector* ptr = new PathVector (outputSize, outputDerivativeSize);
44  PathVectorPtr_t shPtr (ptr);
45  ptr->init (shPtr);
46  return shPtr;
47  }
48 
50  static PathVectorPtr_t create (size_type outputSize,
51  size_type outputDerivativeSize,
52  const ConstraintSetPtr_t& constraint)
53  {
54  PathVector* ptr = new PathVector (outputSize, outputDerivativeSize,
55  constraint);
56  PathVectorPtr_t shPtr (ptr);
57  ptr->init (shPtr);
58  return shPtr;
59  }
60 
62  static PathVectorPtr_t createCopy (const PathVectorPtr_t& original)
63  {
64  PathVector* ptr = new PathVector (*original);
65  PathVectorPtr_t shPtr (ptr);
66  ptr->init (shPtr);
67  return shPtr;
68  }
69 
71  static PathVectorPtr_t createCopy (const PathVectorPtr_t& original,
72  const ConstraintSetPtr_t& constraints)
73  {
74  PathVector* ptr = new PathVector (*original, constraints);
75  PathVectorPtr_t shPtr (ptr);
76  ptr->init (shPtr);
77  return shPtr;
78  }
79 
81  virtual PathPtr_t copy () const
82  {
83  return createCopy (weak_.lock ());
84  }
85 
89  virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const
90  {
91  return createCopy (weak_.lock (), constraints);
92  }
93 
95  virtual ~PathVector ()
96  {
97  }
99 
101  std::size_t numberPaths () const
102  {
103  return paths_.size ();
104  }
105 
112  PathPtr_t pathAtRank (std::size_t rank) const;
113 
119  std::size_t rankAtParam (const value_type& param, value_type& localParam) const;
120 
122  void appendPath (const PathPtr_t& path);
123 
127  void concatenate (const PathVector& path) HPP_CORE_DEPRECATED;
128 
133  void concatenate (const PathVectorPtr_t& path);
134 
136  virtual Configuration_t initial () const
137  {
138  return paths_.front ()->initial ();
139  }
140 
142  virtual Configuration_t end () const
143  {
144  return paths_.back()->end ();
145  }
146 
149  void flatten (PathVectorPtr_t flattenedPath) const;
150 
152  virtual PathPtr_t reverse () const;
153 
154  protected:
156  virtual std::ostream& print (std::ostream &os) const;
158  PathVector (std::size_t outputSize, std::size_t outputDerivativeSize) :
159  parent_t (std::make_pair (0, 0), outputSize, outputDerivativeSize),
160  paths_ ()
161  {
162  }
164  PathVector (std::size_t outputSize, std::size_t outputDerivativeSize,
165  const ConstraintSetPtr_t& constraint) :
166  parent_t (std::make_pair (0, 0), outputSize, outputDerivativeSize,
167  constraint),
168  paths_ ()
169  {
170  }
172  PathVector (const PathVector& path) : parent_t (path),
173  paths_ ()
174  {
175  assert (timeRange() == path.timeRange());
176  for (Paths_t::const_iterator it = path.paths_.begin ();
177  it != path.paths_.end (); it++) {
178  paths_.push_back ((*it)->copy ());
179  }
180  }
181 
183  PathVector (const PathVector& path,
184  const ConstraintSetPtr_t& constraints) :
185  parent_t (path, constraints), paths_ ()
186  {
187  assert (timeRange() == path.timeRange());
188  for (Paths_t::const_iterator it = path.paths_.begin ();
189  it != path.paths_.end (); it++) {
190  paths_.push_back ((*it)->copy ());
191  }
192  }
193 
194  void init (PathVectorPtr_t self)
195  {
196  parent_t::init (self);
197  weak_ = self;
198  }
199  virtual bool impl_compute (ConfigurationOut_t result, value_type t) const;
201  virtual void impl_derivative (vectorOut_t result, const value_type& t,
202  size_type order) const;
205  virtual PathPtr_t impl_extract (const interval_t& subInterval) const;
206 
208  virtual void impl_velocityBound (vectorOut_t bound,
209  const value_type& param0,
210  const value_type& param1) const;
211 
212  private:
213  Paths_t paths_;
214  PathVectorWkPtr_t weak_;
215 
216  protected:
218  private:
219  HPP_SERIALIZABLE();
220  }; // class PathVector
222  } // namespace core
223 } // namespace hpp
224 
225 BOOST_CLASS_EXPORT_KEY(hpp::core::PathVector)
226 
227 #endif // HPP_CORE_PATH_VECTOR_HH
PathVector(const PathVector &path)
Copy constructor.
Definition: path-vector.hh:172
static PathVectorPtr_t createCopy(const PathVectorPtr_t &original, const ConstraintSetPtr_t &constraints)
Create instance and return shared pointer.
Definition: path-vector.hh:71
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:176
Definition: bi-rrt-planner.hh:24
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition: path-vector.hh:89
std::pair< value_type, value_type > interval_t
Definition: fwd.hh:158
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
pinocchio::size_type size_type
Definition: fwd.hh:156
Path parent_t
Definition: path-vector.hh:35
std::vector< PathPtr_t > Paths_t
Definition: fwd.hh:196
PathVector(const PathVector &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
Definition: path-vector.hh:183
std::size_t numberPaths() const
Get the number of sub paths.
Definition: path-vector.hh:101
virtual ~PathVector()
Destructor.
Definition: path-vector.hh:95
virtual Configuration_t initial() const
Get the initial configuration.
Definition: path-vector.hh:136
static PathVectorPtr_t create(size_type outputSize, size_type outputDerivativeSize, const ConstraintSetPtr_t &constraint)
Create instance and return shared pointer.
Definition: path-vector.hh:50
virtual PathPtr_t copy() const
Return a shared pointer to a copy of this.
Definition: path-vector.hh:81
const interval_t & timeRange() const
Get interval of definition.
Definition: path.hh:208
PathVector(std::size_t outputSize, std::size_t outputDerivativeSize, const ConstraintSetPtr_t &constraint)
Constructor.
Definition: path-vector.hh:164
#define HPP_CORE_DEPRECATED
Definition: deprecated.hh:32
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
pinocchio::value_type value_type
Definition: fwd.hh:157
Concatenation of several paths.
Definition: path-vector.hh:32
static PathVectorPtr_t createCopy(const PathVectorPtr_t &original)
Create instance and return shared pointer.
Definition: path-vector.hh:62
virtual Configuration_t end() const
Get the final configuration.
Definition: path-vector.hh:142
PathVector(std::size_t outputSize, std::size_t outputDerivativeSize)
Constructor.
Definition: path-vector.hh:158
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:204
void init(PathVectorPtr_t self)
Definition: path-vector.hh:194
PathVector()
Definition: path-vector.hh:217
Definition: path.hh:60
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
static PathVectorPtr_t create(size_type outputSize, size_type outputDerivativeSize)
Create instance and return shared pointer.
Definition: path-vector.hh:40
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170