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;
44 static SteeringMethodParabolaPtr_t
create(core::ProblemConstPtr_t problem) {
46 SteeringMethodParabolaPtr_t shPtr(ptr);
51 static SteeringMethodParabolaPtr_t
createCopy(
const SteeringMethodParabolaPtr_t& other) {
53 SteeringMethodParabolaPtr_t shPtr(ptr);
58 virtual core::SteeringMethodPtr_t
copy()
const {
return createCopy(weak_.lock()); }
60 core::PathPtr_t
operator()(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2)
const {
61 return impl_compute(q1, q2);
65 virtual core::PathPtr_t impl_compute(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2)
const;
68 core::PathPtr_t compute_random_3D_path(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2, value_type* alpha0,
69 value_type* v0)
const;
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;
83 bool fiveth_constraint(
const core::ConfigurationIn_t q,
const value_type theta,
const int number,
84 value_type* delta)
const;
96 : SteeringMethod(other),
97 device_(other.device_),
98 distance_(other.distance_),
101 V0max_(other.V0max_),
102 Vimpmax_(other.Vimpmax_),
104 Dalpha_(other.Dalpha_),
105 nLimit_(other.nLimit_),
107 Vimp_(other.Vimp_) {}
110 void init(SteeringMethodParabolaWkPtr_t weak) {
111 SteeringMethod::init(weak);
117 core::PathPtr_t compute_3D_path(core::ConfigurationIn_t q1, core::ConfigurationIn_t q2)
const;
123 bool second_constraint(
const value_type& X,
const value_type& Y, value_type* alpha_lim_plus,
124 value_type* alpha_lim_minus)
const;
130 bool sixth_constraint(
const value_type& X,
const value_type& Y, value_type* alpha_imp_plus,
131 value_type* alpha_imp_minus)
const;
135 virtual value_type computeLength(
const core::ConfigurationIn_t q1,
const core::ConfigurationIn_t q2,
136 const vector_t coefs)
const;
140 value_type lengthFunction(
const value_type x,
const vector_t coefs)
const;
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;
148 bool parabMaxHeightRespected(
const vector_t coefs,
const value_type x_theta_0,
const value_type x_theta_imp)
const;
151 value_type dichotomy(value_type a_inf, value_type a_plus, std::size_t n)
const;
154 void fillROMnames(core::ConfigurationIn_t q, std::vector<std::string>* ROMnames)
const;
156 core::DeviceWkPtr_t device_;
157 core::WeighedDistancePtr_t distance_;
158 SteeringMethodParabolaWkPtr_t weak_;
160 mutable value_type V0max_;
161 mutable value_type Vimpmax_;
162 mutable value_type mu_;
164 mutable std::size_t nLimit_;
165 mutable bool initialConstraint_;
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_;
174 mutable std::vector<std::string> endROMnames_;
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