hpp-rbprm  4.10.0
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 
20 #ifndef HPP_RBPRM_TIMED_PARABOLA_PATH_HH
21 #define HPP_RBPRM_TIMED_PARABOLA_PATH_HH
22 
23 
25 
26 
27 namespace hpp {
28  namespace rbprm {
29 
30 
31  // forward declaration of class
32  HPP_PREDEF_CLASS (TimedParabolaPath);
33  // Planner objects are manipulated only via shared pointers
34  typedef boost::shared_ptr <TimedParabolaPath> TimedParabolaPathPtr_t;
35 
36 
41  {
42  public:
45  virtual ~TimedParabolaPath () throw () {}
46 
51  static TimedParabolaPathPtr_t create (const core::DevicePtr_t& device,
52  core::ConfigurationIn_t init,
53  core::ConfigurationIn_t end,
54  ParabolaPathPtr_t parabolaPath)
55  {
56  TimedParabolaPath* ptr = new TimedParabolaPath (device, init, end, parabolaPath);
57  TimedParabolaPathPtr_t shPtr (ptr);
58  ptr->init (shPtr);
59  return shPtr;
60  }
61 
66  static TimedParabolaPathPtr_t create (const core::DevicePtr_t& device,
67  core::ConfigurationIn_t init,
68  core::ConfigurationIn_t end,
69  core::value_type length,
70  core::vector_t coefficients)
71  {
73  coefficients);
74  TimedParabolaPathPtr_t shPtr (ptr);
75  ptr->init (shPtr);
76  return shPtr;
77  }
78 
85  static TimedParabolaPathPtr_t create(const core::DevicePtr_t& device,
86  core::ConfigurationIn_t init,
87  core::ConfigurationIn_t end,
88  core::value_type length,
89  core::vector_t coefficients,
90  core::vector_t V0, core::vector_t Vimp,
91  std::vector <std::string> initialROMnames,
92  std::vector <std::string> endROMnames)
93  {
95  coefficients, V0, Vimp,
96  initialROMnames, endROMnames);
97  TimedParabolaPathPtr_t shPtr (ptr);
98  ptr->init (shPtr);
99  return shPtr;
100  }
101 
105  {
106  TimedParabolaPath* ptr = new TimedParabolaPath (*path);
107  TimedParabolaPathPtr_t shPtr (ptr);
108  ptr->init (shPtr);
109  return shPtr;
110  }
111 
117  (const TimedParabolaPathPtr_t& path, const core::ConstraintSetPtr_t& /*constraints*/)
118  {
119  //TimedParabolaPath* ptr = new TimedParabolaPath (*path, constraints);
120  TimedParabolaPath* ptr = new TimedParabolaPath (*path);
121  TimedParabolaPathPtr_t shPtr (ptr);
122  ptr->init (shPtr);
123  return shPtr;
124  }
125 
130  virtual core::PathPtr_t copy () const
131  {
132  return createCopy (weak_.lock ());
133  }
134 
139  virtual core::PathPtr_t copy (const core::ConstraintSetPtr_t& constraints) const
140  {
141  return createCopy (weak_.lock (), constraints);
142  }
143 
148  virtual core::PathPtr_t extract (const core::interval_t& subInterval) const throw (core::projection_error);
149 
152  virtual core::PathPtr_t reverse () const;
153 
154 
159  void initialConfig (core::ConfigurationIn_t initial)
160  {
161  assert (initial.size () == initial_.size ());
162  initial_ = initial;
163  }
164 
169  void endConfig (core::ConfigurationIn_t end)
170  {
171  assert (end.size () == end_.size ());
172  end_ = end;
173  }
174 
176  core::DevicePtr_t device () const;
177 
179  core::Configuration_t initial () const
180  {
181  return initial_;
182  }
183 
185  core::Configuration_t end () const
186  {
187  return end_;
188  }
189 
191  virtual core::value_type length () const {
192  return length_;
193  }
194 
195  protected :
197  TimedParabolaPath (const core::DevicePtr_t& robot,
198  core::ConfigurationIn_t init,
199  core::ConfigurationIn_t end, ParabolaPathPtr_t parabolaPath);
200 
202  TimedParabolaPath (const core::DevicePtr_t& robot,
203  core::ConfigurationIn_t init,
204  core::ConfigurationIn_t end, core::value_type length,
205  core::vector_t coefficients);
206 
208  TimedParabolaPath (const core::DevicePtr_t& robot,
209  core::ConfigurationIn_t init,
210  core::ConfigurationIn_t end,
211  core::value_type length,
212  core::vector_t coefs,
213  core::vector_t V0, core::vector_t Vimp,
214  std::vector <std::string> initialROMnames,
215  std::vector <std::string> endROMnames);
216 
218  TimedParabolaPath (const TimedParabolaPath& path);
219 
220 
222  {
223  parent_t::init (self);
224  weak_ = self;
225  }
226 
228  virtual bool impl_compute (core::ConfigurationOut_t result,
229  core::value_type t) const;
230 
231  virtual double computeTimedLength(double x_theta, double v0, double alpha0);
232  virtual double computeTimedLength(ParabolaPathPtr_t parabolaPath);
233 
235  virtual std::ostream& print (std::ostream &os) const
236  {
237  os << "TimedParabolaPath:" << std::endl;
238  os << "interval: [ " << timeRange ().first << ", "
239  << timeRange ().second << " ]" << std::endl;
240  os << "initial configuration: " << initial_.transpose () << std::endl;
241  os << "final configuration: " << end_.transpose () << std::endl;
242  return os;
243  }
244 
245  private:
246  core::DevicePtr_t device_;
247  core::Configuration_t initial_;
248  core::Configuration_t end_;
249  TimedParabolaPathWkPtr_t weak_;
250  ParabolaPathPtr_t parabolaPath_;
251  mutable core::value_type length_;
252 
253 
254  }; // class TimedParabolaPath
255  } // namespace rbprm
256 } // namespace hpp
257 
258 
259 #endif // HPP_RBPRM_TIMED_PARABOLA_PATH_HH
hpp::rbprm::TimedParabolaPath::length
virtual core::value_type length() const
Get previously computed length.
Definition: timed-parabola-path.hh:191
hpp::rbprm::TimedParabolaPath::initialConfig
void initialConfig(core::ConfigurationIn_t initial)
Definition: timed-parabola-path.hh:159
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::rbprm::TimedParabolaPath::parent_t
ParabolaPath parent_t
Definition: timed-parabola-path.hh:43
hpp::rbprm::TimedParabolaPath::copy
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: timed-parabola-path.hh:139
hpp::rbprm::TimedParabolaPath::print
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: timed-parabola-path.hh:235
hpp::rbprm::TimedParabolaPath::create
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:66
hpp::rbprm::TimedParabolaPath::extract
virtual core::PathPtr_t extract(const core::interval_t &subInterval) const
hpp::rbprm::TimedParabolaPath::copy
virtual core::PathPtr_t copy() const
Definition: timed-parabola-path.hh:130
hpp::rbprm::TimedParabolaPath::computeTimedLength
virtual double computeTimedLength(double x_theta, double v0, double alpha0)
hpp::rbprm::TimedParabolaPath::TimedParabolaPath
TimedParabolaPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, ParabolaPathPtr_t parabolaPath)
Constructor.
hpp::rbprm::TimedParabolaPath::end
core::Configuration_t end() const
Get the final configuration.
Definition: timed-parabola-path.hh:185
hpp::rbprm::TimedParabolaPath
Definition: timed-parabola-path.hh:40
hpp::rbprm::TimedParabolaPath::create
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:51
hpp::rbprm::TimedParabolaPath::init
void init(TimedParabolaPathPtr_t self)
Definition: timed-parabola-path.hh:221
hpp::rbprm::ParabolaPath::coefficients
core::vector_t coefficients() const
Get path coefficients.
Definition: parabola-path.hh:192
hpp::rbprm::TimedParabolaPath::create
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:85
hpp::rbprm::ParabolaPath
Definition: parabola-path.hh:45
hpp
Definition: algorithm.hh:27
hpp::rbprm::TimedParabolaPath::initial
core::Configuration_t initial() const
Get the initial configuration.
Definition: timed-parabola-path.hh:179
parabola-path.hh
hpp::rbprm::TimedParabolaPath::endConfig
void endConfig(core::ConfigurationIn_t end)
Definition: timed-parabola-path.hh:169
hpp::rbprm::TimedParabolaPath::device
core::DevicePtr_t device() const
Return the internal robot.
hpp::rbprm::ParabolaPath::init
void init(ParabolaPathPtr_t self)
Definition: parabola-path.hh:245
hpp::rbprm::TimedParabolaPath::~TimedParabolaPath
virtual ~TimedParabolaPath()
Destructor.
Definition: timed-parabola-path.hh:45
hpp::rbprm::TimedParabolaPath::reverse
virtual core::PathPtr_t reverse() const
hpp::rbprm::ParabolaPathPtr_t
boost::shared_ptr< ParabolaPath > ParabolaPathPtr_t
Definition: parabola-path.hh:33
hpp::rbprm::TimedParabolaPath::createCopy
static TimedParabolaPathPtr_t createCopy(const TimedParabolaPathPtr_t &path)
Definition: timed-parabola-path.hh:104
hpp::rbprm::TimedParabolaPathPtr_t
boost::shared_ptr< TimedParabolaPath > TimedParabolaPathPtr_t
Definition: timed-parabola-path.hh:34
hpp::rbprm::TimedParabolaPath::impl_compute
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type t) const
Param is the time.