hpp-rbprm  4.13.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/core/steering-method.hh>
23 #include <hpp/core/weighed-distance.hh>
24 #include <hpp/util/debug.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 shared_ptr<SteeringMethodParabola> SteeringMethodParabolaPtr_t;
38 
41 class HPP_CORE_DLLAPI SteeringMethodParabola : public core::SteeringMethod {
42  public:
44  static SteeringMethodParabolaPtr_t create(core::ProblemConstPtr_t problem) {
46  SteeringMethodParabolaPtr_t shPtr(ptr);
47  ptr->init(shPtr);
48  return shPtr;
49  }
51  static SteeringMethodParabolaPtr_t createCopy(
52  const SteeringMethodParabolaPtr_t& other) {
54  SteeringMethodParabolaPtr_t shPtr(ptr);
55  ptr->init(shPtr);
56  return shPtr;
57  }
59  virtual core::SteeringMethodPtr_t copy() const {
60  return createCopy(weak_.lock());
61  }
62 
63  core::PathPtr_t operator()(core::ConfigurationIn_t q1,
64  core::ConfigurationIn_t q2) const {
65  return impl_compute(q1, q2);
66  }
67 
69  virtual core::PathPtr_t impl_compute(core::ConfigurationIn_t q1,
70  core::ConfigurationIn_t q2) const;
71 
73  core::PathPtr_t compute_random_3D_path(core::ConfigurationIn_t q1,
74  core::ConfigurationIn_t q2,
75  value_type* alpha0,
76  value_type* v0) const;
77 
82  bool third_constraint(bool fail, const value_type& X, const value_type& Y,
83  const value_type alpha_imp_min,
84  const value_type alpha_imp_max,
85  value_type* alpha_imp_sup, value_type* alpha_imp_inf,
86  const value_type n2_angle) const;
87 
92  bool fiveth_constraint(const core::ConfigurationIn_t q,
93  const value_type theta, const int number,
94  value_type* delta) const;
95 
96  // return maximal final (or impact) velocity
97  value_type getVImpMax() { return Vimpmax_; }
98 
99  protected:
102  SteeringMethodParabola(core::ProblemConstPtr_t problem);
103 
106  : SteeringMethod(other),
107  device_(other.device_),
108  distance_(other.distance_),
109  weak_(),
110  g_(other.g_),
111  V0max_(other.V0max_),
112  Vimpmax_(other.Vimpmax_),
113  mu_(other.mu_),
114  Dalpha_(other.Dalpha_),
115  nLimit_(other.nLimit_),
116  V0_(other.V0_),
117  Vimp_(other.Vimp_) {}
118 
120  void init(SteeringMethodParabolaWkPtr_t weak) {
121  SteeringMethod::init(weak);
122  weak_ = weak;
123  }
124 
125  private:
127  core::PathPtr_t compute_3D_path(core::ConfigurationIn_t q1,
128  core::ConfigurationIn_t q2) const;
129 
134  bool second_constraint(const value_type& X, const value_type& Y,
135  value_type* alpha_lim_plus,
136  value_type* alpha_lim_minus) const;
137 
142  bool sixth_constraint(const value_type& X, const value_type& Y,
143  value_type* alpha_imp_plus,
144  value_type* alpha_imp_minus) const;
145 
148  virtual value_type computeLength(const core::ConfigurationIn_t q1,
149  const core::ConfigurationIn_t q2,
150  const vector_t coefs) const;
151 
154  value_type lengthFunction(const value_type x, const vector_t coefs) const;
155 
157  vector_t computeCoefficients(const value_type alpha, const value_type theta,
158  const value_type X_theta, const value_type Z,
159  const value_type x_theta_0,
160  const value_type z_0) const;
161 
164  bool parabMaxHeightRespected(const vector_t coefs, const value_type x_theta_0,
165  const value_type x_theta_imp) const;
166 
168  value_type dichotomy(value_type a_inf, value_type a_plus,
169  std::size_t n) const;
170 
172  void fillROMnames(core::ConfigurationIn_t q,
173  std::vector<std::string>* ROMnames) const;
174 
175  core::DeviceWkPtr_t device_;
176  core::WeighedDistancePtr_t distance_;
177  SteeringMethodParabolaWkPtr_t weak_;
178  value_type g_; // gravity constant
179  mutable value_type V0max_; // maximal initial velocity
180  mutable value_type Vimpmax_; // maximal landing velocity
181  mutable value_type mu_; // friction coefficient
182  value_type Dalpha_; // alpha increment
183  mutable std::size_t nLimit_; // number of Dichotomies applied
184  mutable bool initialConstraint_; // true if the constraint at the initial
185  // point are respected (5° for first cone,
186  // 1° and 2° constraints)
187  mutable value_type alpha_1_plus_;
188  mutable value_type alpha_1_minus_;
189  mutable value_type alpha_0_max_;
190  mutable value_type alpha_0_min_;
191  mutable core::vector_t V0_;
192  mutable core::vector_t Vimp_;
193  mutable std::vector<std::string> initialROMnames_; // active ROMs
194  mutable std::vector<std::string> endROMnames_;
195 
196  // Reminder for parabola-results = nb of fails from the following causes:
197  // [0] collision or out of configs-bounds
198  // [1] one 3D cone is not intersecting the vertical plane
199  // [2] takeoff/landing in cone not OK
200  // [3] takeoff/landing velocity bound not OK
201 
202 }; // SteeringMethodParabola
204 } // namespace rbprm
205 } // namespace hpp
206 #endif // HPP_RBPRM_STEERING_METHOD_PARABOLA_HH
value_type getVImpMax()
Definition: steering-method-parabola.hh:97
Definition: algorithm.hh:26
static SteeringMethodParabolaPtr_t createCopy(const SteeringMethodParabolaPtr_t &other)
Copy instance and return shared pointer.
Definition: steering-method-parabola.hh:51
SteeringMethodParabola(const SteeringMethodParabola &other)
Copy constructor.
Definition: steering-method-parabola.hh:105
Definition: steering-method-parabola.hh:41
core::PathPtr_t operator()(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const
Definition: steering-method-parabola.hh:63
HPP_PREDEF_CLASS(RbPrmFullBody)
static SteeringMethodParabolaPtr_t create(core::ProblemConstPtr_t problem)
Create instance and return shared pointer.
Definition: steering-method-parabola.hh:44
virtual core::SteeringMethodPtr_t copy() const
Copy instance and return shared pointer.
Definition: steering-method-parabola.hh:59
void init(SteeringMethodParabolaWkPtr_t weak)
Store weak pointer to itself.
Definition: steering-method-parabola.hh:120
shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37