hpp-rbprm  4.12.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 std::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(const SteeringMethodParabolaPtr_t& other) {
53  SteeringMethodParabolaPtr_t shPtr(ptr);
54  ptr->init(shPtr);
55  return shPtr;
56  }
58  virtual core::SteeringMethodPtr_t copy() const { return createCopy(weak_.lock()); }
59 
60  core::PathPtr_t operator()(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const {
61  return impl_compute(q1, q2);
62  }
63 
65  virtual core::PathPtr_t impl_compute(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const;
66 
68  core::PathPtr_t compute_random_3D_path(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2, value_type* alpha0,
69  value_type* v0) const;
70 
75  bool third_constraint(bool fail, const value_type& X, const value_type& Y, const value_type alpha_imp_min,
76  const value_type alpha_imp_max, value_type* alpha_imp_sup, value_type* alpha_imp_inf,
77  const value_type n2_angle) const;
78 
83  bool fiveth_constraint(const core::ConfigurationIn_t q, const value_type theta, const int number,
84  value_type* delta) const;
85 
86  // return maximal final (or impact) velocity
87  value_type getVImpMax() { return Vimpmax_; }
88 
89  protected:
92  SteeringMethodParabola(core::ProblemConstPtr_t problem);
93 
96  : SteeringMethod(other),
97  device_(other.device_),
98  distance_(other.distance_),
99  weak_(),
100  g_(other.g_),
101  V0max_(other.V0max_),
102  Vimpmax_(other.Vimpmax_),
103  mu_(other.mu_),
104  Dalpha_(other.Dalpha_),
105  nLimit_(other.nLimit_),
106  V0_(other.V0_),
107  Vimp_(other.Vimp_) {}
108 
110  void init(SteeringMethodParabolaWkPtr_t weak) {
111  SteeringMethod::init(weak);
112  weak_ = weak;
113  }
114 
115  private:
117  core::PathPtr_t compute_3D_path(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const;
118 
123  bool second_constraint(const value_type& X, const value_type& Y, value_type* alpha_lim_plus,
124  value_type* alpha_lim_minus) const;
125 
130  bool sixth_constraint(const value_type& X, const value_type& Y, value_type* alpha_imp_plus,
131  value_type* alpha_imp_minus) const;
132 
135  virtual value_type computeLength(const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2,
136  const vector_t coefs) const;
137 
140  value_type lengthFunction(const value_type x, const vector_t coefs) const;
141 
143  vector_t computeCoefficients(const value_type alpha, const value_type theta, const value_type X_theta,
144  const value_type Z, const value_type x_theta_0, const value_type z_0) const;
145 
148  bool parabMaxHeightRespected(const vector_t coefs, const value_type x_theta_0, const value_type x_theta_imp) const;
149 
151  value_type dichotomy(value_type a_inf, value_type a_plus, std::size_t n) const;
152 
154  void fillROMnames(core::ConfigurationIn_t q, std::vector<std::string>* ROMnames) const;
155 
156  core::DeviceWkPtr_t device_;
157  core::WeighedDistancePtr_t distance_;
158  SteeringMethodParabolaWkPtr_t weak_;
159  value_type g_; // gravity constant
160  mutable value_type V0max_; // maximal initial velocity
161  mutable value_type Vimpmax_; // maximal landing velocity
162  mutable value_type mu_; // friction coefficient
163  value_type Dalpha_; // alpha increment
164  mutable std::size_t nLimit_; // number of Dichotomies applied
165  mutable bool initialConstraint_; // true if the constraint at the initial point are respected (5° for first cone, 1°
166  // and 2° constraints)
167  mutable value_type alpha_1_plus_;
168  mutable value_type alpha_1_minus_;
169  mutable value_type alpha_0_max_;
170  mutable value_type alpha_0_min_;
171  mutable core::vector_t V0_;
172  mutable core::vector_t Vimp_;
173  mutable std::vector<std::string> initialROMnames_; // active ROMs
174  mutable std::vector<std::string> endROMnames_;
175 
176  // Reminder for parabola-results = nb of fails from the following causes:
177  // [0] collision or out of configs-bounds
178  // [1] one 3D cone is not intersecting the vertical plane
179  // [2] takeoff/landing in cone not OK
180  // [3] takeoff/landing velocity bound not OK
181 
182 }; // SteeringMethodParabola
184 } // namespace rbprm
185 } // namespace hpp
186 #endif // HPP_RBPRM_STEERING_METHOD_PARABOLA_HH
value_type getVImpMax()
Definition: steering-method-parabola.hh:87
Definition: algorithm.hh:27
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:95
Definition: steering-method-parabola.hh:41
core::PathPtr_t operator()(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const
Definition: steering-method-parabola.hh:60
HPP_PREDEF_CLASS(RbPrmFullBody)
std::shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37
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:58
void init(SteeringMethodParabolaWkPtr_t weak)
Store weak pointer to itself.
Definition: steering-method-parabola.hh:110