hpp-rbprm  4.11.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/fwd.hh>
23 #include <hpp/core/config.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 std::shared_ptr<ParabolaPath> ParabolaPathPtr_t;
34 
44 class ParabolaPath : public core::Path {
45  public:
46  typedef Path parent_t;
48  virtual ~ParabolaPath() throw() {}
49 
54  static ParabolaPathPtr_t create(const core::DevicePtr_t& device, core::ConfigurationIn_t init,
55  core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients) {
56  ParabolaPath* ptr = new ParabolaPath(device, init, end, length, coefficients);
57  ParabolaPathPtr_t shPtr(ptr);
58  ptr->init(shPtr);
59  return shPtr;
60  }
61 
68  static ParabolaPathPtr_t create(const core::DevicePtr_t& device, core::ConfigurationIn_t init,
69  core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients,
70  core::vector_t V0, core::vector_t Vimp, std::vector<std::string> initialROMnames,
71  std::vector<std::string> endROMnames) {
72  ParabolaPath* ptr =
73  new ParabolaPath(device, init, end, length, coefficients, V0, Vimp, initialROMnames, endROMnames);
74  ParabolaPathPtr_t shPtr(ptr);
75  ptr->init(shPtr);
76  return shPtr;
77  }
78 
81  static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t& path) {
82  ParabolaPath* ptr = new ParabolaPath(*path);
83  ParabolaPathPtr_t shPtr(ptr);
84  ptr->init(shPtr);
85  return shPtr;
86  }
87 
92  static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t& path, const core::ConstraintSetPtr_t& /*constraints*/) {
93  // ParabolaPath* ptr = new ParabolaPath (*path, constraints);
94  ParabolaPath* ptr = new ParabolaPath(*path);
95  ParabolaPathPtr_t shPtr(ptr);
96  ptr->init(shPtr);
97  return shPtr;
98  }
99 
104  virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
105 
110  virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t& constraints) const {
111  return createCopy(weak_.lock(), constraints);
112  }
113 
118  virtual core::PathPtr_t extract(const core::interval_t& subInterval) const throw(core::projection_error);
119 
122  virtual core::PathPtr_t reverse() const;
123 
128  void initialConfig(core::ConfigurationIn_t initial) {
129  assert(initial.size() == initial_.size());
130  initial_ = initial;
131  }
132 
137  void endConfig(core::ConfigurationIn_t end) {
138  assert(end.size() == end_.size());
139  end_ = end;
140  }
141 
143  core::DevicePtr_t device() const;
144 
146  core::Configuration_t initial() const { return initial_; }
147 
149  core::Configuration_t end() const { return end_; }
150 
152  virtual core::value_type length() const { return length_; }
153 
155  void coefficients(core::vector_t coefs) const {
156  for (size_type i = 0; i < coefs.size(); i++) coefficients_(i) = coefs(i);
157  }
158 
160  core::vector_t coefficients() const { return coefficients_; }
161 
162  virtual core::value_type computeLength(const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2) const;
163 
165  core::vector_t evaluateVelocity(const core::value_type t) const;
166 
167  core::value_type alpha_; // chosen alpha in interval
168  core::value_type alphaMin_; // min bound of alpha interval
169  core::value_type alphaMax_; // max bound of alpha interval
170  core::value_type Xtheta_;
171  core::value_type Z_;
172  core::vector_t V0_; // initial velocity
173  core::vector_t Vimp_; // final velocity
174  std::vector<std::string> initialROMnames_; // active ROM list at begining
175  std::vector<std::string> endROMnames_; // active ROM list at end
176 
177  protected:
179  virtual std::ostream& print(std::ostream& os) const {
180  os << "ParabolaPath:" << std::endl;
181  os << "interval: [ " << timeRange().first << ", " << timeRange().second << " ]" << std::endl;
182  os << "initial configuration: " << initial_.transpose() << std::endl;
183  os << "final configuration: " << end_.transpose() << std::endl;
184  return os;
185  }
187  ParabolaPath(const core::DevicePtr_t& robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end,
188  core::value_type length, core::vector_t coefficients);
189 
191  ParabolaPath(const core::DevicePtr_t& device, core::ConfigurationIn_t init, core::ConfigurationIn_t end,
192  core::value_type length, core::vector_t coefs, core::vector_t V0_, core::vector_t Vimp,
193  std::vector<std::string> initialROMnames, std::vector<std::string> endROMnames);
194 
196  ParabolaPath(const ParabolaPath& path);
197 
198  core::value_type lengthFunction(const core::value_type x) const;
199 
200  void init(ParabolaPathPtr_t self) {
201  parent_t::init(self);
202  weak_ = self;
203  }
204 
205  /*void initCopy (ParabolaPathPtr_t self)
206  {
207  parent_t::initCopy (self);
208  weak_ = self;
209  }*/
210 
216  virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const;
217 
218  private:
219  core::DevicePtr_t device_;
220  core::Configuration_t initial_;
221  core::Configuration_t end_;
222  ParabolaPathWkPtr_t weak_;
223  mutable core::vector_t coefficients_; // parabola coefficients
224  mutable core::value_type length_;
225 }; // class ParabolaPath
226 } // namespace rbprm
227 } // namespace hpp
228 #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:167
std::shared_ptr< ParabolaPath > ParabolaPathPtr_t
Definition: parabola-path.hh:33
core::Configuration_t initial() const
Get the initial configuration.
Definition: parabola-path.hh:146
void initialConfig(core::ConfigurationIn_t initial)
Definition: parabola-path.hh:128
core::vector_t Vimp_
Definition: parabola-path.hh:173
Definition: algorithm.hh:27
Definition: parabola-path.hh:44
static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t &path, const core::ConstraintSetPtr_t &)
Definition: parabola-path.hh:92
core::vector_t V0_
Definition: parabola-path.hh:172
virtual core::PathPtr_t copy() const
Definition: parabola-path.hh:104
core::value_type alphaMin_
Definition: parabola-path.hh:168
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:155
core::DevicePtr_t device() const
Return the internal robot.
virtual core::value_type length() const
Get previously computed length.
Definition: parabola-path.hh:152
HPP_PREDEF_CLASS(RbPrmFullBody)
void init(ParabolaPathPtr_t self)
Definition: parabola-path.hh:200
core::value_type Z_
Definition: parabola-path.hh:171
static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t &path)
Definition: parabola-path.hh:81
core::value_type alphaMax_
Definition: parabola-path.hh:169
core::value_type Xtheta_
Definition: parabola-path.hh:170
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:160
core::value_type lengthFunction(const core::value_type x) const
std::vector< std::string > initialROMnames_
Definition: parabola-path.hh:174
std::vector< std::string > endROMnames_
Definition: parabola-path.hh:175
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:68
virtual ~ParabolaPath()
Destructor.
Definition: parabola-path.hh:48
void endConfig(core::ConfigurationIn_t end)
Definition: parabola-path.hh:137
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: parabola-path.hh:110
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:149
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:179