hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
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
26namespace hpp {
27namespace rbprm {
30
31using core::value_type;
32using core::vector_t;
33
34// forward declaration of class
36// Planner objects are manipulated only via shared pointers
37typedef shared_ptr<SteeringMethodParabola> SteeringMethodParabolaPtr_t;
38
41class HPP_CORE_DLLAPI SteeringMethodParabola : public core::SteeringMethod {
42 public:
44 static SteeringMethodParabolaPtr_t create(core::ProblemConstPtr_t problem) {
47 ptr->init(shPtr);
48 return shPtr;
49 }
52 const SteeringMethodParabolaPtr_t& other) {
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
Definition: steering-method-parabola.hh:41
bool third_constraint(bool fail, const value_type &X, const value_type &Y, const value_type alpha_imp_min, const value_type alpha_imp_max, value_type *alpha_imp_sup, value_type *alpha_imp_inf, const value_type n2_angle) const
bool fiveth_constraint(const core::ConfigurationIn_t q, const value_type theta, const int number, value_type *delta) const
core::PathPtr_t compute_random_3D_path(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2, value_type *alpha0, value_type *v0) const
Compute a random parabola in direction of q1->q2.
SteeringMethodParabola(core::ProblemConstPtr_t problem)
static SteeringMethodParabolaPtr_t create(core::ProblemConstPtr_t problem)
Create instance and return shared pointer.
Definition: steering-method-parabola.hh:44
virtual core::PathPtr_t impl_compute(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const
create a path between two configurations
static SteeringMethodParabolaPtr_t createCopy(const SteeringMethodParabolaPtr_t &other)
Copy instance and return shared pointer.
Definition: steering-method-parabola.hh:51
value_type getVImpMax()
Definition: steering-method-parabola.hh:97
void init(SteeringMethodParabolaWkPtr_t weak)
Store weak pointer to itself.
Definition: steering-method-parabola.hh:120
SteeringMethodParabola(const SteeringMethodParabola &other)
Copy constructor.
Definition: steering-method-parabola.hh:105
core::PathPtr_t operator()(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const
Definition: steering-method-parabola.hh:63
virtual core::SteeringMethodPtr_t copy() const
Copy instance and return shared pointer.
Definition: steering-method-parabola.hh:59
shared_ptr< SteeringMethodParabola > SteeringMethodParabolaPtr_t
Definition: steering-method-parabola.hh:37
HPP_PREDEF_CLASS(RbPrmFullBody)
Definition: algorithm.hh:26