19#ifndef HPP_RBPRM_STEERING_METHOD_PARABOLA_HH
20#define HPP_RBPRM_STEERING_METHOD_PARABOLA_HH
22#include <hpp/core/steering-method.hh>
23#include <hpp/core/weighed-distance.hh>
24#include <hpp/util/debug.hh>
31using core::value_type;
59 virtual core::SteeringMethodPtr_t
copy()
const {
60 return createCopy(weak_.lock());
64 core::ConfigurationIn_t q2)
const {
65 return impl_compute(q1, q2);
70 core::ConfigurationIn_t q2)
const;
74 core::ConfigurationIn_t q2,
76 value_type* v0)
const;
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;
93 const value_type theta,
const int number,
94 value_type* delta)
const;
106 : SteeringMethod(other),
107 device_(other.device_),
108 distance_(other.distance_),
111 V0max_(other.V0max_),
112 Vimpmax_(other.Vimpmax_),
114 Dalpha_(other.Dalpha_),
115 nLimit_(other.nLimit_),
117 Vimp_(other.Vimp_) {}
120 void init(SteeringMethodParabolaWkPtr_t weak) {
121 SteeringMethod::init(weak);
127 core::PathPtr_t compute_3D_path(core::ConfigurationIn_t q1,
128 core::ConfigurationIn_t q2)
const;
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;
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;
148 virtual value_type computeLength(
const core::ConfigurationIn_t q1,
149 const core::ConfigurationIn_t q2,
150 const vector_t coefs)
const;
154 value_type lengthFunction(
const value_type x,
const vector_t coefs)
const;
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;
164 bool parabMaxHeightRespected(
const vector_t coefs,
const value_type x_theta_0,
165 const value_type x_theta_imp)
const;
168 value_type dichotomy(value_type a_inf, value_type a_plus,
169 std::size_t n)
const;
172 void fillROMnames(core::ConfigurationIn_t q,
173 std::vector<std::string>* ROMnames)
const;
175 core::DeviceWkPtr_t device_;
176 core::WeighedDistancePtr_t distance_;
177 SteeringMethodParabolaWkPtr_t weak_;
179 mutable value_type V0max_;
180 mutable value_type Vimpmax_;
181 mutable value_type mu_;
183 mutable std::size_t nLimit_;
184 mutable bool initialConstraint_;
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_;
194 mutable std::vector<std::string> endROMnames_;
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