hpp-rbprm  4.15.1
Implementation of RB-PRM planner using hpp.
timed-parabola-path.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017 CNRS
3 // Authors: Pierre Fernbach
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_TIMED_PARABOLA_PATH_HH
20 #define HPP_RBPRM_TIMED_PARABOLA_PATH_HH
21 
23 
24 namespace hpp {
25 namespace rbprm {
26 
27 // forward declaration of class
28 HPP_PREDEF_CLASS(TimedParabolaPath);
29 // Planner objects are manipulated only via shared pointers
30 typedef shared_ptr<TimedParabolaPath> TimedParabolaPathPtr_t;
31 
36  public:
39  virtual ~TimedParabolaPath() {}
40 
45  static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
46  core::ConfigurationIn_t init,
47  core::ConfigurationIn_t end,
48  ParabolaPathPtr_t parabolaPath) {
49  TimedParabolaPath* ptr =
50  new TimedParabolaPath(device, init, end, parabolaPath);
51  TimedParabolaPathPtr_t shPtr(ptr);
52  ptr->init(shPtr);
53  return shPtr;
54  }
55 
60  static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
61  core::ConfigurationIn_t init,
62  core::ConfigurationIn_t end,
63  core::value_type length,
64  core::vector_t coefficients) {
65  TimedParabolaPath* ptr =
66  new TimedParabolaPath(device, init, end, length, coefficients);
67  TimedParabolaPathPtr_t shPtr(ptr);
68  ptr->init(shPtr);
69  return shPtr;
70  }
71 
78  static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
79  core::ConfigurationIn_t init,
80  core::ConfigurationIn_t end,
81  core::value_type length,
82  core::vector_t coefficients,
83  core::vector_t V0, core::vector_t Vimp,
84  std::vector<std::string> initialROMnames,
85  std::vector<std::string> endROMnames) {
86  TimedParabolaPath* ptr =
87  new TimedParabolaPath(device, init, end, length, coefficients, V0, Vimp,
88  initialROMnames, endROMnames);
89  TimedParabolaPathPtr_t shPtr(ptr);
90  ptr->init(shPtr);
91  return shPtr;
92  }
93 
96  static TimedParabolaPathPtr_t createCopy(const TimedParabolaPathPtr_t& path) {
97  TimedParabolaPath* ptr = new TimedParabolaPath(*path);
98  TimedParabolaPathPtr_t shPtr(ptr);
99  ptr->init(shPtr);
100  return shPtr;
101  }
102 
107  static TimedParabolaPathPtr_t createCopy(
108  const TimedParabolaPathPtr_t& path,
109  const core::ConstraintSetPtr_t& /*constraints*/) {
110  // TimedParabolaPath* ptr = new TimedParabolaPath (*path, constraints);
111  TimedParabolaPath* ptr = new TimedParabolaPath(*path);
112  TimedParabolaPathPtr_t shPtr(ptr);
113  ptr->init(shPtr);
114  return shPtr;
115  }
116 
121  virtual core::PathPtr_t copy() const { return createCopy(weak_.lock()); }
122 
127  virtual core::PathPtr_t copy(
128  const core::ConstraintSetPtr_t& constraints) const {
129  return createCopy(weak_.lock(), constraints);
130  }
131 
136  virtual core::PathPtr_t extract(const core::interval_t& subInterval) const;
137 
140  virtual core::PathPtr_t reverse() const;
141 
146  void initialConfig(core::ConfigurationIn_t initial) {
147  assert(initial.size() == initial_.size());
148  initial_ = initial;
149  }
150 
155  void endConfig(core::ConfigurationIn_t end) {
156  assert(end.size() == end_.size());
157  end_ = end;
158  }
159 
161  core::DevicePtr_t device() const;
162 
164  core::Configuration_t initial() const { return initial_; }
165 
167  core::Configuration_t end() const { return end_; }
168 
170  virtual core::value_type length() const { return length_; }
171 
172  protected:
174  TimedParabolaPath(const core::DevicePtr_t& robot,
175  core::ConfigurationIn_t init, core::ConfigurationIn_t end,
176  ParabolaPathPtr_t parabolaPath);
177 
179  TimedParabolaPath(const core::DevicePtr_t& robot,
180  core::ConfigurationIn_t init, core::ConfigurationIn_t end,
181  core::value_type length, core::vector_t coefficients);
182 
184  TimedParabolaPath(const core::DevicePtr_t& robot,
185  core::ConfigurationIn_t init, core::ConfigurationIn_t end,
186  core::value_type length, core::vector_t coefs,
187  core::vector_t V0, core::vector_t Vimp,
188  std::vector<std::string> initialROMnames,
189  std::vector<std::string> endROMnames);
190 
193 
194  void init(TimedParabolaPathPtr_t self) {
195  parent_t::init(self);
196  weak_ = self;
197  }
198 
200  virtual bool impl_compute(core::ConfigurationOut_t result,
201  core::value_type t) const;
202 
203  virtual double computeTimedLength(double x_theta, double v0, double alpha0);
204  virtual double computeTimedLength(ParabolaPathPtr_t parabolaPath);
205 
207  virtual std::ostream& print(std::ostream& os) const {
208  os << "TimedParabolaPath:" << std::endl;
209  os << "interval: [ " << timeRange().first << ", " << timeRange().second
210  << " ]" << std::endl;
211  os << "initial configuration: " << initial_.transpose() << std::endl;
212  os << "final configuration: " << end_.transpose() << std::endl;
213  return os;
214  }
215 
216  private:
217  core::DevicePtr_t device_;
218  core::Configuration_t initial_;
219  core::Configuration_t end_;
220  TimedParabolaPathWkPtr_t weak_;
221  ParabolaPathPtr_t parabolaPath_;
222  mutable core::value_type length_;
223 
224 }; // class TimedParabolaPath
225 } // namespace rbprm
226 } // namespace hpp
227 
228 #endif // HPP_RBPRM_TIMED_PARABOLA_PATH_HH
void init(TimedParabolaPathPtr_t self)
Definition: timed-parabola-path.hh:194
static TimedParabolaPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, ParabolaPathPtr_t parabolaPath)
Definition: timed-parabola-path.hh:45
static TimedParabolaPathPtr_t create(const core::DevicePtr_t &device, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients)
Definition: timed-parabola-path.hh:60
core::Configuration_t end() const
Get the final configuration.
Definition: timed-parabola-path.hh:167
shared_ptr< TimedParabolaPath > TimedParabolaPathPtr_t
Definition: timed-parabola-path.hh:30
Definition: algorithm.hh:26
core::Configuration_t initial() const
Get the initial configuration.
Definition: timed-parabola-path.hh:164
shared_ptr< ParabolaPath > ParabolaPathPtr_t
Definition: parabola-path.hh:33
Definition: parabola-path.hh:44
static TimedParabolaPathPtr_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: timed-parabola-path.hh:78
TimedParabolaPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, ParabolaPathPtr_t parabolaPath)
Constructor.
void initialConfig(core::ConfigurationIn_t initial)
Definition: timed-parabola-path.hh:146
static TimedParabolaPathPtr_t createCopy(const TimedParabolaPathPtr_t &path)
Definition: timed-parabola-path.hh:96
virtual core::PathPtr_t copy() const
Definition: timed-parabola-path.hh:121
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: timed-parabola-path.hh:127
HPP_PREDEF_CLASS(RbPrmFullBody)
void init(ParabolaPathPtr_t self)
Definition: parabola-path.hh:217
void endConfig(core::ConfigurationIn_t end)
Definition: timed-parabola-path.hh:155
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: timed-parabola-path.hh:207
Definition: timed-parabola-path.hh:35
virtual double computeTimedLength(double x_theta, double v0, double alpha0)
virtual core::PathPtr_t extract(const core::interval_t &subInterval) const
static TimedParabolaPathPtr_t createCopy(const TimedParabolaPathPtr_t &path, const core::ConstraintSetPtr_t &)
Definition: timed-parabola-path.hh:107
core::DevicePtr_t device() const
Return the internal robot.
virtual ~TimedParabolaPath()
Destructor.
Definition: timed-parabola-path.hh:39
core::vector_t coefficients() const
Get path coefficients.
Definition: parabola-path.hh:172
virtual core::PathPtr_t reverse() const
virtual core::value_type length() const
Get previously computed length.
Definition: timed-parabola-path.hh:170
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type t) const
Param is the time.
ParabolaPath parent_t
Definition: timed-parabola-path.hh:37