hpp-rbprm  4.10.0
Implementation of RB-PRM planner using hpp.
steering-method-parabola.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_STEERING_METHOD_PARABOLA_HH
20 # define HPP_RBPRM_STEERING_METHOD_PARABOLA_HH
21 
22 # include <hpp/util/debug.hh>
23 # include <hpp/core/steering-method.hh>
24 # include <hpp/core/weighed-distance.hh>
25 
26 namespace hpp {
27  namespace rbprm {
30 
31  using core::value_type;
32  using core::vector_t;
33 
34  // forward declaration of class
35  HPP_PREDEF_CLASS (SteeringMethodParabola);
36  // Planner objects are manipulated only via shared pointers
37  typedef boost::shared_ptr <SteeringMethodParabola> SteeringMethodParabolaPtr_t;
38 
41  class HPP_CORE_DLLAPI SteeringMethodParabola : public core::SteeringMethod
42  {
43  public:
45  static SteeringMethodParabolaPtr_t create (const core::Problem& problem)
46  {
48  SteeringMethodParabolaPtr_t shPtr (ptr);
49  ptr->init (shPtr);
50  return shPtr;
51  }
53  static SteeringMethodParabolaPtr_t createCopy
55  {
57  SteeringMethodParabolaPtr_t shPtr (ptr);
58  ptr->init (shPtr);
59  return shPtr;
60  }
62  virtual core::SteeringMethodPtr_t copy () const
63  {
64  return createCopy (weak_.lock ());
65  }
66 
67  core::PathPtr_t operator() (core::ConfigurationIn_t q1,
68  core::ConfigurationIn_t q2) const
69  {
70  return impl_compute (q1, q2);
71  }
72 
74  virtual core::PathPtr_t impl_compute (core::ConfigurationIn_t q1,
75  core::ConfigurationIn_t q2) const;
76 
78  core::PathPtr_t compute_random_3D_path (core::ConfigurationIn_t q1,
79  core::ConfigurationIn_t q2,
80  value_type* alpha0,
81  value_type* v0) const;
82 
87  bool third_constraint (bool fail, const value_type& X,
88  const value_type& Y,
89  const value_type alpha_imp_min,
90  const value_type alpha_imp_max,
91  value_type *alpha_imp_sup,
92  value_type *alpha_imp_inf,
93  const value_type n2_angle) const;
94 
99  bool fiveth_constraint (const core::ConfigurationIn_t q,
100  const value_type theta,
101  const int number,
102  value_type *delta) const;
103 
104  // return maximal final (or impact) velocity
105  value_type getVImpMax() {return Vimpmax_;}
106 
107  protected:
110  SteeringMethodParabola (const core::Problem& problem);
111 
114  SteeringMethod (other),
115  device_ (other.device_),
116  distance_ (other.distance_), weak_ (), g_(other.g_),
117  V0max_ (other.V0max_), Vimpmax_ (other.Vimpmax_),mu_ (other.mu_),
118  Dalpha_ (other.Dalpha_), nLimit_ (other.nLimit_), V0_ (other.V0_),
119  Vimp_ (other.Vimp_)
120  {
121  }
122 
124  void init (SteeringMethodParabolaWkPtr_t weak)
125  {
126  SteeringMethod::init (weak);
127  weak_ = weak;
128  }
129 
130  private:
132  core::PathPtr_t compute_3D_path (core::ConfigurationIn_t q1,
133  core::ConfigurationIn_t q2) const;
134 
139  bool second_constraint (const value_type& X, const value_type& Y,
140  value_type *alpha_lim_plus,
141  value_type *alpha_lim_minus) const;
142 
147  bool sixth_constraint (const value_type& X, const value_type& Y,
148  value_type *alpha_imp_plus,
149  value_type *alpha_imp_minus) const;
150 
153  virtual value_type computeLength (const core::ConfigurationIn_t q1,
154  const core::ConfigurationIn_t q2,
155  const vector_t coefs) const;
156 
159  value_type lengthFunction (const value_type x, const vector_t coefs) const;
160 
162  vector_t computeCoefficients (const value_type alpha,
163  const value_type theta,
164  const value_type X_theta,
165  const value_type Z,
166  const value_type x_theta_0,
167  const value_type z_0) const;
168 
171  bool parabMaxHeightRespected (const vector_t coefs,
172  const value_type x_theta_0,
173  const value_type x_theta_imp) const;
174 
176  value_type dichotomy (value_type a_inf, value_type a_plus,
177  std::size_t n) const;
178 
180  void fillROMnames (core::ConfigurationIn_t q,
181  std::vector <std::string> * ROMnames) const;
182 
183  core::DeviceWkPtr_t device_;
184  core::WeighedDistancePtr_t distance_;
185  SteeringMethodParabolaWkPtr_t weak_;
186  value_type g_; // gravity constant
187  mutable value_type V0max_; // maximal initial velocity
188  mutable value_type Vimpmax_; // maximal landing velocity
189  mutable value_type mu_; // friction coefficient
190  value_type Dalpha_; // alpha increment
191  mutable std::size_t nLimit_; // number of Dichotomies applied
192  mutable bool initialConstraint_; // true if the constraint at the initial point are respected (5° for first cone, 1° and 2° constraints)
193  mutable value_type alpha_1_plus_;
194  mutable value_type alpha_1_minus_;
195  mutable value_type alpha_0_max_;
196  mutable value_type alpha_0_min_;
197  mutable core::vector_t V0_;
198  mutable core::vector_t Vimp_;
199  mutable std::vector <std::string> initialROMnames_; // active ROMs
200  mutable std::vector <std::string> endROMnames_;
201 
202  // Reminder for parabola-results = nb of fails from the following causes:
203  // [0] collision or out of configs-bounds
204  // [1] one 3D cone is not intersecting the vertical plane
205  // [2] takeoff/landing in cone not OK
206  // [3] takeoff/landing velocity bound not OK
207 
208  }; // SteeringMethodParabola
210  } // namespace rbprm
211 } // namespace hpp
212 #endif // HPP_RBPRM_STEERING_METHOD_PARABOLA_HH
hpp::rbprm::SteeringMethodParabola::create
static SteeringMethodParabolaPtr_t create(const core::Problem &problem)
Create instance and return shared pointer.
Definition: steering-method-parabola.hh:45
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::rbprm::SteeringMethodParabola::getVImpMax
value_type getVImpMax()
Definition: steering-method-parabola.hh:105
hpp::rbprm::SteeringMethodParabolaPtr_t
boost::shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37
hpp::rbprm::SteeringMethodParabola::SteeringMethodParabola
SteeringMethodParabola(const SteeringMethodParabola &other)
Copy constructor.
Definition: steering-method-parabola.hh:113
hpp
Definition: algorithm.hh:27
hpp::rbprm::SteeringMethodParabola::copy
virtual core::SteeringMethodPtr_t copy() const
Copy instance and return shared pointer.
Definition: steering-method-parabola.hh:62
hpp::rbprm::SteeringMethodParabola::init
void init(SteeringMethodParabolaWkPtr_t weak)
Store weak pointer to itself.
Definition: steering-method-parabola.hh:124
hpp::rbprm::SteeringMethodParabola
Definition: steering-method-parabola.hh:41