hpp-rbprm  4.10.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 boost::shared_ptr <ParabolaPath> ParabolaPathPtr_t;
34 
35 
45  class ParabolaPath : public core::Path
46  {
47  public:
48  typedef Path parent_t;
50  virtual ~ParabolaPath () throw () {}
51 
56  static ParabolaPathPtr_t create (const core::DevicePtr_t& device,
57  core::ConfigurationIn_t init,
58  core::ConfigurationIn_t end,
59  core::value_type length,
60  core::vector_t coefficients)
61  {
63  coefficients);
64  ParabolaPathPtr_t shPtr (ptr);
65  ptr->init (shPtr);
66  return shPtr;
67  }
68 
75  static ParabolaPathPtr_t create(const core::DevicePtr_t& device,
76  core::ConfigurationIn_t init,
77  core::ConfigurationIn_t end,
78  core::value_type length,
79  core::vector_t coefficients,
80  core::vector_t V0, core::vector_t Vimp,
81  std::vector <std::string> initialROMnames,
82  std::vector <std::string> endROMnames)
83  {
85  coefficients, V0, Vimp,
86  initialROMnames, endROMnames);
87  ParabolaPathPtr_t shPtr (ptr);
88  ptr->init (shPtr);
89  return shPtr;
90  }
91 
95  {
96  ParabolaPath* ptr = new ParabolaPath (*path);
97  ParabolaPathPtr_t shPtr (ptr);
98  ptr->init (shPtr);
99  return shPtr;
100  }
101 
107  (const ParabolaPathPtr_t& path, const core::ConstraintSetPtr_t& /*constraints*/)
108  {
109  //ParabolaPath* ptr = new ParabolaPath (*path, constraints);
110  ParabolaPath* ptr = new ParabolaPath (*path);
111  ParabolaPathPtr_t shPtr (ptr);
112  ptr->init (shPtr);
113  return shPtr;
114  }
115 
120  virtual core::PathPtr_t copy () const
121  {
122  return createCopy (weak_.lock ());
123  }
124 
129  virtual core::PathPtr_t copy (const core::ConstraintSetPtr_t& constraints) const
130  {
131  return createCopy (weak_.lock (), constraints);
132  }
133 
138  virtual core::PathPtr_t extract (const core::interval_t& subInterval) const throw (core::projection_error);
139 
142  virtual core::PathPtr_t reverse () const;
143 
144 
149  void initialConfig (core::ConfigurationIn_t initial)
150  {
151  assert (initial.size () == initial_.size ());
152  initial_ = initial;
153  }
154 
159  void endConfig (core::ConfigurationIn_t end)
160  {
161  assert (end.size () == end_.size ());
162  end_ = end;
163  }
164 
166  core::DevicePtr_t device () const;
167 
169  core::Configuration_t initial () const
170  {
171  return initial_;
172  }
173 
175  core::Configuration_t end () const
176  {
177  return end_;
178  }
179 
181  virtual core::value_type length () const {
182  return length_;
183  }
184 
186  void coefficients (core::vector_t coefs) const {
187  for (size_type i = 0; i < coefs.size (); i++)
188  coefficients_(i) = coefs (i);
189  }
190 
192  core::vector_t coefficients () const {
193  return coefficients_;
194  }
195 
196  virtual core::value_type computeLength (const core::ConfigurationIn_t q1,
197  const core::ConfigurationIn_t q2) const;
198 
200  core::vector_t evaluateVelocity (const core::value_type t) const;
201 
202  core::value_type alpha_; // chosen alpha in interval
203  core::value_type alphaMin_; // min bound of alpha interval
204  core::value_type alphaMax_; // max bound of alpha interval
205  core::value_type Xtheta_;
206  core::value_type Z_;
207  core::vector_t V0_; // initial velocity
208  core::vector_t Vimp_; // final velocity
209  std::vector <std::string> initialROMnames_; // active ROM list at begining
210  std::vector <std::string> endROMnames_; // active ROM list at end
211 
212 
213  protected:
215  virtual std::ostream& print (std::ostream &os) const
216  {
217  os << "ParabolaPath:" << std::endl;
218  os << "interval: [ " << timeRange ().first << ", "
219  << timeRange ().second << " ]" << std::endl;
220  os << "initial configuration: " << initial_.transpose () << std::endl;
221  os << "final configuration: " << end_.transpose () << std::endl;
222  return os;
223  }
225  ParabolaPath (const core::DevicePtr_t& robot,
226  core::ConfigurationIn_t init,
227  core::ConfigurationIn_t end, core::value_type length,
228  core::vector_t coefficients);
229 
231  ParabolaPath (const core::DevicePtr_t& device,
232  core::ConfigurationIn_t init,
233  core::ConfigurationIn_t end,
234  core::value_type length,
235  core::vector_t coefs,
236  core::vector_t V0_, core::vector_t Vimp,
237  std::vector <std::string> initialROMnames,
238  std::vector <std::string> endROMnames);
239 
241  ParabolaPath (const ParabolaPath& path);
242 
243  core::value_type lengthFunction (const core::value_type x)const;
244 
246  {
247  parent_t::init (self);
248  weak_ = self;
249  }
250 
251  /*void initCopy (ParabolaPathPtr_t self)
252  {
253  parent_t::initCopy (self);
254  weak_ = self;
255  }*/
256 
262  virtual bool impl_compute (core::ConfigurationOut_t result,
263  core::value_type param) const;
264 
265  private:
266  core::DevicePtr_t device_;
267  core::Configuration_t initial_;
268  core::Configuration_t end_;
269  ParabolaPathWkPtr_t weak_;
270  mutable core::vector_t coefficients_; // parabola coefficients
271  mutable core::value_type length_;
272  }; // class ParabolaPath
273  } // namespace rbprm
274 } // namespace hpp
275 #endif // HPP_CORE_PARABOLA_PATH_HH
hpp::rbprm::ParabolaPath::Z_
core::value_type Z_
Definition: parabola-path.hh:206
hpp::rbprm::ParabolaPath::alphaMin_
core::value_type alphaMin_
Definition: parabola-path.hh:203
hpp::rbprm::ParabolaPath::impl_compute
virtual bool impl_compute(core::ConfigurationOut_t result, core::value_type param) const
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::rbprm::ParabolaPath::alphaMax_
core::value_type alphaMax_
Definition: parabola-path.hh:204
hpp::rbprm::ParabolaPath::device
core::DevicePtr_t device() const
Return the internal robot.
hpp::rbprm::ParabolaPath::evaluateVelocity
core::vector_t evaluateVelocity(const core::value_type t) const
Evaluate velocity vector at path abcissa t.
hpp::rbprm::ParabolaPath::V0_
core::vector_t V0_
Definition: parabola-path.hh:207
hpp::rbprm::ParabolaPath::alpha_
core::value_type alpha_
Definition: parabola-path.hh:202
hpp::rbprm::ParabolaPath::create
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:56
hpp::rbprm::ParabolaPath::initial
core::Configuration_t initial() const
Get the initial configuration.
Definition: parabola-path.hh:169
hpp::rbprm::ParabolaPath::end
core::Configuration_t end() const
Get the final configuration.
Definition: parabola-path.hh:175
hpp::rbprm::ParabolaPath::copy
virtual core::PathPtr_t copy(const core::ConstraintSetPtr_t &constraints) const
Definition: parabola-path.hh:129
hpp::rbprm::ParabolaPath::copy
virtual core::PathPtr_t copy() const
Definition: parabola-path.hh:120
hpp::rbprm::ParabolaPath::coefficients
void coefficients(core::vector_t coefs) const
Set the three parabola coefficients.
Definition: parabola-path.hh:186
hpp::rbprm::ParabolaPath::Xtheta_
core::value_type Xtheta_
Definition: parabola-path.hh:205
hpp::rbprm::ParabolaPath::createCopy
static ParabolaPathPtr_t createCopy(const ParabolaPathPtr_t &path)
Definition: parabola-path.hh:94
hpp::rbprm::ParabolaPath::coefficients
core::vector_t coefficients() const
Get path coefficients.
Definition: parabola-path.hh:192
hpp::rbprm::ParabolaPath::print
virtual std::ostream & print(std::ostream &os) const
Print path in a stream.
Definition: parabola-path.hh:215
hpp::rbprm::ParabolaPath
Definition: parabola-path.hh:45
hpp
Definition: algorithm.hh:27
hpp::rbprm::ParabolaPath::extract
virtual core::PathPtr_t extract(const core::interval_t &subInterval) const
hpp::rbprm::ParabolaPath::initialROMnames_
std::vector< std::string > initialROMnames_
Definition: parabola-path.hh:209
hpp::rbprm::ParabolaPath::Vimp_
core::vector_t Vimp_
Definition: parabola-path.hh:208
hpp::rbprm::ParabolaPath::init
void init(ParabolaPathPtr_t self)
Definition: parabola-path.hh:245
hpp::rbprm::ParabolaPath::parent_t
Path parent_t
Definition: parabola-path.hh:48
hpp::rbprm::ParabolaPath::ParabolaPath
ParabolaPath(const core::DevicePtr_t &robot, core::ConfigurationIn_t init, core::ConfigurationIn_t end, core::value_type length, core::vector_t coefficients)
Constructor.
hpp::rbprm::ParabolaPath::reverse
virtual core::PathPtr_t reverse() const
hpp::rbprm::ParabolaPathPtr_t
boost::shared_ptr< ParabolaPath > ParabolaPathPtr_t
Definition: parabola-path.hh:33
hpp::rbprm::ParabolaPath::length
virtual core::value_type length() const
Get previously computed length.
Definition: parabola-path.hh:181
hpp::rbprm::ParabolaPath::computeLength
virtual core::value_type computeLength(const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2) const
hpp::rbprm::ParabolaPath::initialConfig
void initialConfig(core::ConfigurationIn_t initial)
Definition: parabola-path.hh:149
hpp::rbprm::ParabolaPath::create
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:75
hpp::rbprm::ParabolaPath::endROMnames_
std::vector< std::string > endROMnames_
Definition: parabola-path.hh:210
hpp::rbprm::ParabolaPath::endConfig
void endConfig(core::ConfigurationIn_t end)
Definition: parabola-path.hh:159
hpp::rbprm::ParabolaPath::~ParabolaPath
virtual ~ParabolaPath()
Destructor.
Definition: parabola-path.hh:50
hpp::rbprm::ParabolaPath::lengthFunction
core::value_type lengthFunction(const core::value_type x) const