hpp-core  4.10.1
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 createCopy (const PathVectorPtr_t& original)
51  {
52  PathVector* ptr = new PathVector (*original);
53  PathVectorPtr_t shPtr (ptr);
54  ptr->init (shPtr);
55  return shPtr;
56  }
57 
59  static PathVectorPtr_t createCopy (const PathVectorPtr_t& original,
60  const ConstraintSetPtr_t& constraints)
61  {
62  PathVector* ptr = new PathVector (*original, constraints);
63  PathVectorPtr_t shPtr (ptr);
64  ptr->init (shPtr);
65  return shPtr;
66  }
67 
69  virtual PathPtr_t copy () const
70  {
71  return createCopy (weak_.lock ());
72  }
73 
77  virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const
78  {
79  return createCopy (weak_.lock (), constraints);
80  }
81 
83  virtual ~PathVector ()
84  {
85  }
87 
89  std::size_t numberPaths () const
90  {
91  return paths_.size ();
92  }
93 
100  PathPtr_t pathAtRank (std::size_t rank) const;
101 
107  std::size_t rankAtParam (const value_type& param, value_type& localParam) const;
108 
110  void appendPath (const PathPtr_t& path);
111 
115  void concatenate (const PathVector& path) HPP_CORE_DEPRECATED;
116 
121  void concatenate (const PathVectorPtr_t& path);
122 
124  virtual Configuration_t initial () const
125  {
126  return paths_.front ()->initial ();
127  }
128 
130  virtual Configuration_t end () const
131  {
132  return paths_.back()->end ();
133  }
134 
137  void flatten (PathVectorPtr_t flattenedPath) const;
138 
139  protected:
141  virtual std::ostream& print (std::ostream &os) const;
143  PathVector (std::size_t outputSize, std::size_t outputDerivativeSize) :
144  parent_t (std::make_pair (0, 0), outputSize, outputDerivativeSize),
145  paths_ ()
146  {
147  }
149  PathVector (const PathVector& path) : parent_t (path),
150  paths_ ()
151  {
152  assert (timeRange() == path.timeRange());
153  for (Paths_t::const_iterator it = path.paths_.begin ();
154  it != path.paths_.end (); it++) {
155  paths_.push_back ((*it)->copy ());
156  }
157  }
158 
160  PathVector (const PathVector& path,
161  const ConstraintSetPtr_t& constraints) :
162  parent_t (path, constraints), paths_ ()
163  {
164  assert (timeRange() == path.timeRange());
165  for (Paths_t::const_iterator it = path.paths_.begin ();
166  it != path.paths_.end (); it++) {
167  paths_.push_back ((*it)->copy ());
168  }
169  }
170 
171  void init (PathVectorPtr_t self)
172  {
173  parent_t::init (self);
174  weak_ = self;
175  }
176  virtual bool impl_compute (ConfigurationOut_t result, value_type t) const;
178  virtual void impl_derivative (vectorOut_t result, const value_type& t,
179  size_type order) const;
182  virtual PathPtr_t impl_extract (const interval_t& subInterval) const;
183 
184  private:
185  Paths_t paths_;
186  PathVectorWkPtr_t weak_;
187 
188  protected:
190  private:
191  HPP_SERIALIZABLE();
192  }; // class PathVector
194  } // namespace core
195 } // namespace hpp
196 #endif // HPP_CORE_PATH_VECTOR_HH
boost::shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
PathVector(const PathVector &path)
Copy constructor.
Definition: path-vector.hh:149
static PathVectorPtr_t createCopy(const PathVectorPtr_t &original, const ConstraintSetPtr_t &constraints)
Create instance and return shared pointer.
Definition: path-vector.hh:59
Definition: basic-configuration-shooter.hh:26
virtual PathPtr_t copy(const ConstraintSetPtr_t &constraints) const
Definition: path-vector.hh:77
boost::shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
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:195
PathVector(const PathVector &path, const ConstraintSetPtr_t &constraints)
Copy constructor with constraints.
Definition: path-vector.hh:160
std::size_t numberPaths() const
Get the number of sub paths.
Definition: path-vector.hh:89
virtual ~PathVector()
Destructor.
Definition: path-vector.hh:83
virtual Configuration_t initial() const
Get the initial configuration.
Definition: path-vector.hh:124
virtual PathPtr_t copy() const
Return a shared pointer to a copy of this.
Definition: path-vector.hh:69
const interval_t & timeRange() const
Get interval of definition.
Definition: path.hh:210
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:50
boost::shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:176
constraints::Implicit NumericalConstraint HPP_CORE_DEPRECATED
Definition: fwd.hh:347
virtual Configuration_t end() const
Get the final configuration.
Definition: path-vector.hh:130
PathVector(std::size_t outputSize, std::size_t outputDerivativeSize)
Constructor.
Definition: path-vector.hh:143
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:203
void init(PathVectorPtr_t self)
Definition: path-vector.hh:171
PathVector()
Definition: path-vector.hh:189
Definition: path.hh:61
#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