19 #ifndef HPP_RBPRM_STEERING_METHOD_PARABOLA_HH
20 # define HPP_RBPRM_STEERING_METHOD_PARABOLA_HH
22 # include <hpp/util/debug.hh>
23 # include <hpp/core/steering-method.hh>
24 # include <hpp/core/weighed-distance.hh>
31 using core::value_type;
62 virtual core::SteeringMethodPtr_t
copy ()
const
64 return createCopy (weak_.lock ());
67 core::PathPtr_t operator() (core::ConfigurationIn_t q1,
68 core::ConfigurationIn_t q2)
const
70 return impl_compute (q1, q2);
74 virtual core::PathPtr_t impl_compute (core::ConfigurationIn_t q1,
75 core::ConfigurationIn_t q2)
const;
78 core::PathPtr_t compute_random_3D_path (core::ConfigurationIn_t q1,
79 core::ConfigurationIn_t q2,
81 value_type* v0)
const;
87 bool third_constraint (
bool fail,
const value_type& X,
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;
99 bool fiveth_constraint (
const core::ConfigurationIn_t q,
100 const value_type theta,
102 value_type *delta)
const;
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_),
124 void init (SteeringMethodParabolaWkPtr_t weak)
126 SteeringMethod::init (weak);
132 core::PathPtr_t compute_3D_path (core::ConfigurationIn_t q1,
133 core::ConfigurationIn_t q2)
const;
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;
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;
153 virtual value_type computeLength (
const core::ConfigurationIn_t q1,
154 const core::ConfigurationIn_t q2,
155 const vector_t coefs)
const;
159 value_type lengthFunction (
const value_type x,
const vector_t coefs)
const;
162 vector_t computeCoefficients (
const value_type alpha,
163 const value_type theta,
164 const value_type X_theta,
166 const value_type x_theta_0,
167 const value_type z_0)
const;
171 bool parabMaxHeightRespected (
const vector_t coefs,
172 const value_type x_theta_0,
173 const value_type x_theta_imp)
const;
176 value_type dichotomy (value_type a_inf, value_type a_plus,
177 std::size_t n)
const;
180 void fillROMnames (core::ConfigurationIn_t q,
181 std::vector <std::string> * ROMnames)
const;
183 core::DeviceWkPtr_t device_;
184 core::WeighedDistancePtr_t distance_;
185 SteeringMethodParabolaWkPtr_t weak_;
187 mutable value_type V0max_;
188 mutable value_type Vimpmax_;
189 mutable value_type mu_;
191 mutable std::size_t nLimit_;
192 mutable bool initialConstraint_;
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_;
200 mutable std::vector <std::string> endROMnames_;
212 #endif // HPP_RBPRM_STEERING_METHOD_PARABOLA_HH