hpp-rbprm  4.13.0
Implementation of RB-PRM planner using hpp.
parabola-path.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015 CNRS
3 // Authors: Mylene Campana
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_RBPRM_PARABOLA_PATH_HH
20 #define HPP_RBPRM_PARABOLA_PATH_HH
21 
22 #include <hpp/core/config.hh>
23 #include <hpp/core/fwd.hh>
24 #include <hpp/core/path.hh>
25 
26 namespace hpp {
27 namespace rbprm {
28 using core::size_type;
29 
30 // forward declaration of class
31 HPP_PREDEF_CLASS(ParabolaPath);
32 // Planner objects are manipulated only via shared pointers
33 typedef shared_ptr<ParabolaPath> ParabolaPathPtr_t;
34 
44 class ParabolaPath : public core::Path {
45  public:
46  typedef Path parent_t;
48  virtual ~ParabolaPath() {}
49 
54  static ParabolaPathPtr_t create(const core::DevicePtr_t& device,
55  core::ConfigurationIn_t init,
56  core::ConfigurationIn_t end,
57  core::value_type length,
58  core::vector_t coefficients) {
59  ParabolaPath* ptr =
60  new ParabolaPath(device, init, end, length, coefficients);
61  ParabolaPathPtr_t shPtr(ptr);
62  ptr->init(shPtr);
63  return shPtr;
64  }
65 
72  static ParabolaPathPtr_t create(const core::DevicePtr_t& device,
73  core::ConfigurationIn_t init,
74  core::ConfigurationIn_t end,
75  core::value_type length,
76  core::vector_t coefficients,
77  core::vector_t V0, core::vector_t Vimp,
78  std::vector<std::string> initialROMnames,
79  std::vector<std::string> endROMnames) {
80  ParabolaPath* ptr =
81  new ParabolaPath(device, init, end, length, coefficients, V0, Vimp,
82  initialROMnames, endROMnames);
83  ParabolaPathPtr_t shPtr(ptr);
84  ptr->init(shPtr);
85  return shPtr;
86  }
87 
90  static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t& path) {
91  ParabolaPath* ptr = new ParabolaPath(*path);
92  ParabolaPathPtr_t shPtr(ptr);
93  ptr->init(shPtr);
94  return shPtr;
95  }
96 
101  static ParabolaPathPtr_t createCopy(
102  const ParabolaPathPtr_t& path,
103  const core::ConstraintSetPtr_t& /*constraints*/) {
104  // ParabolaPath* ptr = new ParabolaPath (*path, constraints);
105  ParabolaPath* ptr = new ParabolaPath(*path);
106  ParabolaPathPtr_t shPtr(ptr);
107  ptr->init(shPtr);
108  return shPtr;
109  }
110 
115  virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
116 
121  virtual core::PathPtr_t copy(
122  const core::ConstraintSetPtr_t& constraints) const {
123  return createCopy(weak_.lock(), constraints);
124  }
125 
130  virtual core::PathPtr_t extract(const core::interval_t& subInterval) const;
131 
134  virtual core::PathPtr_t reverse() const;
135 
140  void initialConfig(core::ConfigurationIn_t initial) {
141  assert(initial.size() == initial_.size());
142  initial_ = initial;
143  }
144 
149  void endConfig(core::ConfigurationIn_t end) {
150  assert(end.size() == end_.size());
151  end_ = end;
152  }
153 
155  core::DevicePtr_t device() const;
156 
158  core::Configuration_t initial() const { return initial_; }
159 
161  core::Configuration_t end() const { return end_; }
162 
164  virtual core::value_type length() const { return length_; }
165 
167  void coefficients(core::vector_t coefs) const {
168  for (size_type i = 0; i < coefs.size(); i++) coefficients_(i) = coefs(i);
169  }
170 
172  core::vector_t coefficients() const { return coefficients_; }
173 
174  virtual core::value_type computeLength(
175  const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2) const;
176 
178  core::vector_t evaluateVelocity(const core::value_type t) const;
179 
180  core::value_type alpha_; // chosen alpha in interval
181  core::value_type alphaMin_; // min bound of alpha interval
182  core::value_type alphaMax_; // max bound of alpha interval
183  core::value_type Xtheta_;
184  core::value_type Z_;
185  core::vector_t V0_; // initial velocity
186  core::vector_t Vimp_; // final velocity
187  std::vector<std::string> initialROMnames_; // active ROM list at begining
188  std::vector<std::string> endROMnames_; // active ROM list at end
189 
190  protected:
192  virtual std::ostream& print(std::ostream& os) const {
193  os << "ParabolaPath:" << std::endl;
194  os << "interval: [ " << timeRange().first << ", " << timeRange().second
195  << " ]" << std::endl;
196  os << "initial configuration: " << initial_.transpose() << std::endl;
197  os << "final configuration: " << end_.transpose() << std::endl;
198  return os;
199  }
201  ParabolaPath(const core::DevicePtr_t& robot, core::ConfigurationIn_t init,
202  core::ConfigurationIn_t end, core::value_type length,
203  core::vector_t coefficients);
204 
206  ParabolaPath(const core::DevicePtr_t& device, core::ConfigurationIn_t init,
207  core::ConfigurationIn_t end, core::value_type length,
208  core::vector_t coefs, core::vector_t V0_, core::vector_t Vimp,
209  std::vector<std::string> initialROMnames,
210  std::vector<std::string> endROMnames);
211 
213  ParabolaPath(const ParabolaPath& path);
214 
215  core::value_type lengthFunction(const core::value_type x) const;
216 
217  void init(ParabolaPathPtr_t self) {
218  parent_t::init(self);
219  weak_ = self;
220  }
221 
222  /*void initCopy (ParabolaPathPtr_t self)
223  {
224  parent_t::initCopy (self);
225  weak_ = self;
226  }*/
227 
233  virtual bool impl_compute(core::ConfigurationOut_t result,
234  core::value_type param) const;
235 
236  private:
237  core::DevicePtr_t device_;
238  core::Configuration_t initial_;
239  core::Configuration_t end_;
240  ParabolaPathWkPtr_t weak_;
241  mutable core::vector_t coefficients_; // parabola coefficients
242  mutable core::value_type length_;
243 }; // class ParabolaPath
244 } // namespace rbprm
245 } // namespace hpp
246 #endif // HPP_CORE_PARABOLA_PATH_HH
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
core::value_type alpha_
Definition: parabola-path.hh:180
core::Configuration_t initial() const
Get the initial configuration.
Definition: parabola-path.hh:158
void initialConfig(core::ConfigurationIn_t initial)
Definition: parabola-path.hh:140
core::vector_t Vimp_
Definition: parabola-path.hh:186
Definition: algorithm.hh:26
shared_ptr< ParabolaPath > ParabolaPathPtr_t
Definition: parabola-path.hh:33
Definition: parabola-path.hh:44
static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t &path, const core::ConstraintSetPtr_t &)
Definition: parabola-path.hh:101
core::vector_t V0_
Definition: parabola-path.hh:185
virtual core::PathPtr_t copy() const
Definition: parabola-path.hh:115
core::value_type alphaMin_
Definition: parabola-path.hh:181
core::vector_t evaluateVelocity(const core::value_type t) const
Evaluate velocity vector at path abcissa t.
virtual core::PathPtr_t extract(const core::interval_t &subInterval) const
void coefficients(core::vector_t coefs) const
Set the three parabola coefficients.
Definition: parabola-path.hh:167
core::DevicePtr_t device() const
Return the internal robot.
virtual core::value_type length() const
Get previously computed length.
Definition: parabola-path.hh:164
HPP_PREDEF_CLASS(RbPrmFullBody)
void init(ParabolaPathPtr_t self)
Definition: parabola-path.hh:217
core::value_type Z_
Definition: parabola-path.hh:184
static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t &path)
Definition: parabola-path.hh:90
core::value_type alphaMax_
Definition: parabola-path.hh:182
core::value_type Xtheta_
Definition: parabola-path.hh:183
Path parent_t
Definition: parabola-path.hh:46
virtual core::PathPtr_t reverse() const
core::vector_t coefficients() const
Get path coefficients.
Definition: parabola-path.hh:172
core::value_type lengthFunction(const core::value_type x) const
std::vector< std::string > initialROMnames_
Definition: parabola-path.hh:187
std::vector< std::string > endROMnames_
Definition: parabola-path.hh:188
static ParabolaPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients, core::vector_t V0, core::vector_t Vimp, std::vector< std::string > initialROMnames, std::vector< std::string > endROMnames)
Definition: parabola-path.hh:72
virtual ~ParabolaPath()
Destructor.
Definition: parabola-path.hh:48
void endConfig(core::ConfigurationIn_t end)
Definition: parabola-path.hh:149
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: parabola-path.hh:121
ParabolaPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients)
Constructor.
core::Configuration_t end() const
Get the final configuration.
Definition: parabola-path.hh:161
static ParabolaPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients)
Definition: parabola-path.hh:54
virtual core::value_type computeLength(const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2) const
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: parabola-path.hh:192