1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2016 CNRS |
3 |
|
|
// Authors: Mylene Campana, Pierre Fernbach |
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 |
|
|
#include <hpp/core/config-validations.hh> |
20 |
|
|
#include <hpp/core/path-validation.hh> |
21 |
|
|
#include <hpp/core/path-vector.hh> |
22 |
|
|
#include <hpp/core/problem.hh> |
23 |
|
|
#include <hpp/pinocchio/configuration.hh> |
24 |
|
|
#include <hpp/rbprm/planner/steering-method-parabola.hh> |
25 |
|
|
#include <hpp/rbprm/planner/timed-parabola-path.hh> |
26 |
|
|
#include <hpp/rbprm/rbprm-device.hh> |
27 |
|
|
#include <hpp/rbprm/rbprm-path-validation.hh> |
28 |
|
|
#include <hpp/rbprm/rbprm-validation-report.hh> |
29 |
|
|
#include <hpp/util/debug.hh> |
30 |
|
|
|
31 |
|
|
namespace hpp { |
32 |
|
|
namespace rbprm { |
33 |
|
|
using core::interval_t; |
34 |
|
|
using core::value_type; |
35 |
|
|
using core::vector_t; |
36 |
|
|
using pinocchio::size_type; |
37 |
|
|
|
38 |
|
24 |
SteeringMethodParabola::SteeringMethodParabola(core::ProblemConstPtr_t problem) |
39 |
|
|
: SteeringMethod(problem), |
40 |
|
24 |
device_(problem->robot()), |
41 |
|
|
distance_(core::WeighedDistance::create(problem->robot())), |
42 |
|
|
weak_(), |
43 |
|
|
g_(9.81), |
44 |
|
|
V0max_(1.), |
45 |
|
|
Vimpmax_(1.), |
46 |
|
|
mu_(0.5), |
47 |
|
|
Dalpha_(0.001), |
48 |
|
|
nLimit_(6), |
49 |
|
|
initialConstraint_(true), |
50 |
|
|
V0_(vector_t(3)), |
51 |
✓✗✓✗
✓✗ |
48 |
Vimp_(vector_t(3)) { |
52 |
|
|
hppDout(notice, "Constructor steering-method-parabola"); |
53 |
✓✗✓✗
|
48 |
V0max_ = problem->getParameter(std::string("Kinodynamic/velocityBound")) |
54 |
✓✗ |
24 |
.floatValue(); |
55 |
|
24 |
Vimpmax_ = V0max_; |
56 |
|
24 |
} |
57 |
|
|
|
58 |
|
|
core::PathPtr_t SteeringMethodParabola::impl_compute( |
59 |
|
|
core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const { |
60 |
|
|
hppDout(info, "q_init: " << hpp::pinocchio::displayConfig(q1)); |
61 |
|
|
hppDout(info, "q_goal: " << hpp::pinocchio::displayConfig(q2)); |
62 |
|
|
hppDout(info, "g_: " << g_ << " , mu_: " << mu_ << " , V0max: " << V0max_ |
63 |
|
|
<< " , Vimpmax: " << Vimpmax_); |
64 |
|
|
|
65 |
|
|
core::PathPtr_t pp = compute_3D_path(q1, q2); |
66 |
|
|
return pp; |
67 |
|
|
} |
68 |
|
|
|
69 |
|
|
core::PathPtr_t SteeringMethodParabola::compute_3D_path( |
70 |
|
|
core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const { |
71 |
|
|
std::vector<std::string> filter; |
72 |
|
|
core::PathPtr_t validPart; |
73 |
|
|
const core::PathValidationPtr_t pathValidation(problem()->pathValidation()); |
74 |
|
|
RbPrmPathValidationPtr_t rbPathValidation = |
75 |
|
|
dynamic_pointer_cast<RbPrmPathValidation>(pathValidation); |
76 |
|
|
pinocchio::RbPrmDevicePtr_t rbDevice = |
77 |
|
|
dynamic_pointer_cast<pinocchio::RbPrmDevice>(device_.lock()); |
78 |
|
|
core::PathValidationReportPtr_t pathReport; |
79 |
|
|
if (!rbDevice) hppDout(error, "Device cannot be cast"); |
80 |
|
|
if (!rbPathValidation) hppDout(error, "PathValidation cannot be cast"); |
81 |
|
|
|
82 |
|
|
/* Define some constants */ |
83 |
|
|
const size_type index = |
84 |
|
|
device_.lock()->configSize() - |
85 |
|
|
device_.lock()->extraConfigSpace().dimension(); // ecs index |
86 |
|
|
const value_type x_0 = q1(0); |
87 |
|
|
const value_type y_0 = q1(1); |
88 |
|
|
const value_type z_0 = q1(2); |
89 |
|
|
const value_type x_imp = q2(0); |
90 |
|
|
const value_type y_imp = q2(1); |
91 |
|
|
const value_type z_imp = q2(2); |
92 |
|
|
const value_type X = x_imp - x_0; |
93 |
|
|
const value_type Y = y_imp - y_0; |
94 |
|
|
const value_type Z = z_imp - z_0; |
95 |
|
|
const value_type theta = atan2(Y, X); |
96 |
|
|
const value_type x_theta_0 = cos(theta) * x_0 + sin(theta) * y_0; |
97 |
|
|
const value_type x_theta_imp = cos(theta) * x_imp + sin(theta) * y_imp; |
98 |
|
|
const value_type X_theta = X * cos(theta) + Y * sin(theta); |
99 |
|
|
|
100 |
|
|
hppDout(info, "x_0: " << x_0); |
101 |
|
|
hppDout(info, "y_0: " << y_0); |
102 |
|
|
hppDout(info, "z_0: " << z_0); |
103 |
|
|
hppDout(info, "x_imp: " << x_imp); |
104 |
|
|
hppDout(info, "y_imp: " << y_imp); |
105 |
|
|
hppDout(info, "z_imp: " << z_imp); |
106 |
|
|
hppDout(info, "X: " << X); |
107 |
|
|
hppDout(info, "Y: " << Y); |
108 |
|
|
hppDout(info, "Z: " << Z); |
109 |
|
|
hppDout(info, "theta: " << theta); |
110 |
|
|
hppDout(info, "x_theta_0: " << x_theta_0); |
111 |
|
|
hppDout(info, "x_theta_imp: " << x_theta_imp); |
112 |
|
|
hppDout(info, "X_theta: " << X_theta); |
113 |
|
|
hppDout(info, "phi: " << atan(mu_)); // phi |
114 |
|
|
|
115 |
|
|
/* 5th constraint: first cone */ |
116 |
|
|
|
117 |
|
|
/* Remove friction check (testing) |
118 |
|
|
//value_type delta1,delta2; |
119 |
|
|
if (1000 * (q1 (index) * q1 (index) + q1 (index+1) * q1 (index+1)) |
120 |
|
|
> q1 (index+2) * q1 (index+2)) { // cone 1 not vertical |
121 |
|
|
if (!fiveth_constraint (q1, theta, 1, &delta1)) { |
122 |
|
|
hppDout (info, "plane_theta not intersecting first cone"); |
123 |
|
|
initialConstraint_ = false; |
124 |
|
|
// problem.parabolaResults_ [1] ++; |
125 |
|
|
return core::PathPtr_t (); |
126 |
|
|
} |
127 |
|
|
} |
128 |
|
|
else { // cone 1 "very" vertical |
129 |
|
|
delta1 = phi; |
130 |
|
|
} |
131 |
|
|
hppDout (info, "delta1: " << delta1); |
132 |
|
|
|
133 |
|
|
// 5th constraint: second cone // |
134 |
|
|
if (1000 * (q2 (index) * q2 (index) + q2 (index+1) * q2 (index+1)) |
135 |
|
|
> q2 (index+2) * q2 (index+2)) { // cone 1 not vertical |
136 |
|
|
if (!fiveth_constraint (q2, theta, 2, &delta2)) { |
137 |
|
|
hppDout (info, "plane_theta not intersecting second cone"); |
138 |
|
|
// problem.parabolaResults_ [1] ++; |
139 |
|
|
return core::PathPtr_t (); |
140 |
|
|
} |
141 |
|
|
} |
142 |
|
|
else { // cone 2 "very" vertical |
143 |
|
|
delta2 = phi; |
144 |
|
|
} |
145 |
|
|
hppDout (info, "delta2: " << delta2); |
146 |
|
|
|
147 |
|
|
// Definition of gamma_theta angles // |
148 |
|
|
const value_type n1_angle = atan2(q1 (index+2), cos(theta)*q1 (index) + |
149 |
|
|
sin(theta)*q1 (index+1)); |
150 |
|
|
const value_type n2_angle = atan2(q2 (index+2), cos(theta)*q2 (index) + |
151 |
|
|
sin(theta)*q2 (index+1)); |
152 |
|
|
hppDout (info, "n1_angle: " << n1_angle); |
153 |
|
|
hppDout (info, "n2_angle: " << n2_angle); |
154 |
|
|
|
155 |
|
|
// Only for demo without friction : |
156 |
|
|
//delta1 = 100.; |
157 |
|
|
//delta2 = 100.; |
158 |
|
|
|
159 |
|
|
const value_type alpha_0_min = n1_angle - delta1; |
160 |
|
|
const value_type alpha_0_max = n1_angle + delta1; |
161 |
|
|
alpha_0_min_ = alpha_0_min; alpha_0_max_ = alpha_0_max; |
162 |
|
|
hppDout (info, "alpha_0_min: " << alpha_0_min); |
163 |
|
|
hppDout (info, "alpha_0_max: " << alpha_0_max); |
164 |
|
|
|
165 |
|
|
|
166 |
|
|
|
167 |
|
|
value_type alpha_imp_min = n2_angle - M_PI - delta2; |
168 |
|
|
value_type alpha_imp_max = n2_angle - M_PI + delta2; |
169 |
|
|
if (n2_angle < 0) { |
170 |
|
|
alpha_imp_min = n2_angle + M_PI - delta2; |
171 |
|
|
alpha_imp_max = n2_angle + M_PI + delta2; |
172 |
|
|
} |
173 |
|
|
|
174 |
|
|
*/ // Commented in order to remove non friction test (testing) |
175 |
|
|
|
176 |
|
|
value_type alpha_inf4; |
177 |
|
|
alpha_inf4 = atan(Z / X_theta); |
178 |
|
|
hppDout(info, "alpha_inf4: " << alpha_inf4); |
179 |
|
|
|
180 |
|
|
// Ajout pour test sans friction : |
181 |
|
|
const value_type alpha_0_min = -2 * M_PI; |
182 |
|
|
const value_type alpha_0_max = 2 * M_PI; |
183 |
|
|
value_type alpha_imp_min = -2 * M_PI; |
184 |
|
|
value_type alpha_imp_max = 2 * M_PI; |
185 |
|
|
const value_type n2_angle = 1.5; |
186 |
|
|
// ########### ^ a enlever ^ ############ // |
187 |
|
|
|
188 |
|
|
hppDout(info, "alpha_imp_min: " << alpha_imp_min); |
189 |
|
|
hppDout(info, "alpha_imp_max: " << alpha_imp_max); |
190 |
|
|
|
191 |
|
|
value_type alpha_lim_plus; |
192 |
|
|
value_type alpha_lim_minus; |
193 |
|
|
bool fail = second_constraint(X_theta, Z, &alpha_lim_plus, &alpha_lim_minus); |
194 |
|
|
if (fail) { |
195 |
|
|
hppDout(info, "failed to apply 2nd constraint"); |
196 |
|
|
// problem.parabolaResults_ [3] ++; |
197 |
|
|
return core::PathPtr_t(); |
198 |
|
|
} |
199 |
|
|
|
200 |
|
|
hppDout(info, "alpha_lim_plus: " << alpha_lim_plus); |
201 |
|
|
hppDout(info, "alpha_lim_minus: " << alpha_lim_minus); |
202 |
|
|
|
203 |
|
|
value_type alpha_imp_plus; |
204 |
|
|
value_type alpha_imp_minus; |
205 |
|
|
bool fail6 = sixth_constraint(X_theta, Z, &alpha_imp_plus, &alpha_imp_minus); |
206 |
|
|
if (fail6) { |
207 |
|
|
hppDout(info, "failed to apply 6th constraint"); |
208 |
|
|
// problem.parabolaResults_ [3] ++; |
209 |
|
|
return core::PathPtr_t(); |
210 |
|
|
} |
211 |
|
|
|
212 |
|
|
hppDout(info, "alpha_imp_plus: " << alpha_imp_plus); |
213 |
|
|
hppDout(info, "alpha_imp_minus: " << alpha_imp_minus); |
214 |
|
|
|
215 |
|
|
value_type alpha_imp_inf; |
216 |
|
|
value_type alpha_imp_sup; |
217 |
|
|
// commented (Pierre) : test without friction |
218 |
|
|
bool fail3 = third_constraint(fail, X_theta, Z, alpha_imp_min, alpha_imp_max, |
219 |
|
|
&alpha_imp_sup, &alpha_imp_inf, n2_angle); |
220 |
|
|
|
221 |
|
|
if (fail3) { |
222 |
|
|
hppDout(info, "failed to apply 3rd constraint"); |
223 |
|
|
// problem.parabolaResults_ [2] ++; |
224 |
|
|
return core::PathPtr_t(); |
225 |
|
|
} |
226 |
|
|
|
227 |
|
|
hppDout(info, "alpha_imp_inf: " << alpha_imp_inf); |
228 |
|
|
hppDout(info, "alpha_imp_sup: " << alpha_imp_sup); |
229 |
|
|
|
230 |
|
|
value_type alpha_inf_bound = 0; |
231 |
|
|
value_type alpha_sup_bound = 0; |
232 |
|
|
|
233 |
|
|
/* Define alpha_0 interval satisfying constraints */ |
234 |
|
|
if (n2_angle > 0) { |
235 |
|
|
alpha_lim_minus = std::max(alpha_lim_minus, alpha_imp_minus); |
236 |
|
|
alpha_inf_bound = std::max(std::max(alpha_imp_inf, alpha_lim_minus), |
237 |
|
|
std::max(alpha_0_min, alpha_inf4 + Dalpha_)); |
238 |
|
|
|
239 |
|
|
if (alpha_imp_min < -M_PI / 2) { |
240 |
|
|
alpha_lim_plus = std::min(alpha_lim_plus, alpha_imp_plus); |
241 |
|
|
alpha_sup_bound = |
242 |
|
|
std::min(alpha_0_max, std::min(alpha_lim_plus, M_PI / 2)); |
243 |
|
|
} else { // alpha_imp_sup is worth |
244 |
|
|
alpha_lim_plus = std::min(alpha_lim_plus, alpha_imp_plus); |
245 |
|
|
alpha_sup_bound = std::min(std::min(alpha_0_max, M_PI / 2), |
246 |
|
|
std::min(alpha_lim_plus, alpha_imp_sup)); |
247 |
|
|
} |
248 |
|
|
} else { // down-oriented cone |
249 |
|
|
if (alpha_imp_max < M_PI / 2) { |
250 |
|
|
alpha_lim_minus = std::max(alpha_lim_minus, alpha_imp_minus); |
251 |
|
|
alpha_inf_bound = std::max(std::max(alpha_imp_inf, alpha_lim_minus), |
252 |
|
|
std::max(alpha_0_min, alpha_inf4 + Dalpha_)); |
253 |
|
|
} else { // alpha_imp_max >= M_PI/2 so alpha_imp_inf inaccurate |
254 |
|
|
alpha_lim_minus = std::max(alpha_lim_minus, alpha_imp_minus); |
255 |
|
|
alpha_inf_bound = std::max(std::max(alpha_0_min, alpha_inf4 + Dalpha_), |
256 |
|
|
alpha_lim_minus); |
257 |
|
|
} |
258 |
|
|
alpha_lim_plus = std::min(alpha_lim_plus, alpha_imp_plus); |
259 |
|
|
alpha_sup_bound = std::min(std::min(alpha_0_max, M_PI / 2), |
260 |
|
|
std::min(alpha_lim_plus, alpha_imp_sup)); |
261 |
|
|
} |
262 |
|
|
|
263 |
|
|
hppDout(info, "alpha_inf_bound: " << alpha_inf_bound); |
264 |
|
|
hppDout(info, "alpha_sup_bound: " << alpha_sup_bound); |
265 |
|
|
|
266 |
|
|
if (alpha_inf_bound > alpha_sup_bound) { |
267 |
|
|
hppDout(info, "Constraints intersection is empty"); |
268 |
|
|
// problem.parabolaResults_ [2] ++; |
269 |
|
|
return core::PathPtr_t(); |
270 |
|
|
} |
271 |
|
|
|
272 |
|
|
/* Select alpha_0 as middle of ]alpha_inf_bound,alpha_sup_bound[ */ |
273 |
|
|
value_type alpha = 0.5 * (alpha_inf_bound + alpha_sup_bound); |
274 |
|
|
// for demo only : |
275 |
|
|
alpha = alpha_inf_bound + 0.1 * (alpha_sup_bound - alpha_inf_bound); |
276 |
|
|
/*if(alpha < 0) |
277 |
|
|
alpha = 0;*/ |
278 |
|
|
|
279 |
|
|
hppDout(info, "alpha: " << alpha); |
280 |
|
|
|
281 |
|
|
/* Verify that maximal heigh of smaller parab is not out of the bounds */ |
282 |
|
|
const vector_t coefsInf = |
283 |
|
|
computeCoefficients(alpha_inf_bound, theta, X_theta, Z, x_theta_0, z_0); |
284 |
|
|
bool maxHeightRespected = |
285 |
|
|
parabMaxHeightRespected(coefsInf, x_theta_0, x_theta_imp); |
286 |
|
|
if (!maxHeightRespected) { |
287 |
|
|
hppDout(info, "Path always out of the bounds"); |
288 |
|
|
// problem.parabolaResults_ [0] ++; |
289 |
|
|
return core::PathPtr_t(); |
290 |
|
|
} |
291 |
|
|
|
292 |
|
|
/* Compute Parabola coefficients */ |
293 |
|
|
vector_t coefs = |
294 |
|
|
computeCoefficients(alpha, theta, X_theta, Z, x_theta_0, z_0); |
295 |
|
|
hppDout(info, "coefs: " << coefs.transpose()); |
296 |
|
|
|
297 |
|
|
maxHeightRespected = parabMaxHeightRespected(coefs, x_theta_0, x_theta_imp); |
298 |
|
|
|
299 |
|
|
// fill ROM report, loop on ROM |
300 |
|
|
initialROMnames_.clear(); |
301 |
|
|
endROMnames_.clear(); |
302 |
|
|
fillROMnames(q1, &initialROMnames_); |
303 |
|
|
fillROMnames(q2, &endROMnames_); |
304 |
|
|
hppDout(info, "initialROMnames_ size= " << initialROMnames_.size()); |
305 |
|
|
hppDout(info, "endROMnames_ size= " << endROMnames_.size()); |
306 |
|
|
|
307 |
|
|
// parabola path with alpha_0 as the middle of alpha_0 bounds |
308 |
|
|
ParabolaPathPtr_t pp = |
309 |
|
|
ParabolaPath::create(device_.lock(), q1, q2, computeLength(q1, q2, coefs), |
310 |
|
|
coefs, V0_, Vimp_, initialROMnames_, endROMnames_); |
311 |
|
|
// checks |
312 |
|
|
hppDout(info, "pp->V0_= " << pp->V0_); |
313 |
|
|
hppDout(info, "pp->Vimp_= " << pp->Vimp_); |
314 |
|
|
hppDout(info, "pp->initialROMnames_ size= " << pp->initialROMnames_.size()); |
315 |
|
|
hppDout(info, "pp->endROMnames_ size= " << pp->endROMnames_.size()); |
316 |
|
|
|
317 |
|
|
bool hasCollisions = |
318 |
|
|
!rbPathValidation->validate(pp, false, validPart, pathReport, filter); |
319 |
|
|
std::size_t n = 0; |
320 |
|
|
if (hasCollisions || !maxHeightRespected) { |
321 |
|
|
// problem.parabolaResults_ [0] ++; // not increased during dichotomy |
322 |
|
|
hppDout(info, "parabola has collisions, start dichotomy"); |
323 |
|
|
while ((hasCollisions || !maxHeightRespected) && n < nLimit_) { |
324 |
|
|
alpha = dichotomy(alpha_inf_bound, alpha_sup_bound, n); |
325 |
|
|
|
326 |
|
|
hppDout(info, "alpha= " << alpha); |
327 |
|
|
coefs = computeCoefficients(alpha, theta, X_theta, Z, x_theta_0, z_0); |
328 |
|
|
maxHeightRespected = |
329 |
|
|
parabMaxHeightRespected(coefs, x_theta_0, x_theta_imp); |
330 |
|
|
pp = ParabolaPath::create(device_.lock(), q1, q2, |
331 |
|
|
computeLength(q1, q2, coefs), coefs, V0_, Vimp_, |
332 |
|
|
initialROMnames_, endROMnames_); |
333 |
|
|
hasCollisions = |
334 |
|
|
!rbPathValidation->validate(pp, false, validPart, pathReport, filter); |
335 |
|
|
hppDout(info, "Dichotomy iteration: " << n); |
336 |
|
|
n++; |
337 |
|
|
} // while |
338 |
|
|
} |
339 |
|
|
if (hasCollisions || !maxHeightRespected) return core::PathPtr_t(); |
340 |
|
|
core::Configuration_t init = pp->initial(); |
341 |
|
|
core::Configuration_t end = pp->end(); |
342 |
|
|
init.segment<3>(index) = pp->V0_; |
343 |
|
|
init[index + 5] = -g_; |
344 |
|
|
end.segment<3>(index) = pp->Vimp_; |
345 |
|
|
return TimedParabolaPath::create(device_.lock(), init, end, pp); |
346 |
|
|
} |
347 |
|
|
|
348 |
|
|
// From Pierre |
349 |
|
|
core::PathPtr_t SteeringMethodParabola::compute_random_3D_path( |
350 |
|
|
core::ConfigurationIn_t q1, core::ConfigurationIn_t q2, value_type *alpha0, |
351 |
|
|
value_type *v0) const { |
352 |
|
|
const core::PathValidationPtr_t pathValidation(problem()->pathValidation()); |
353 |
|
|
RbPrmPathValidationPtr_t rbPathValidation = |
354 |
|
|
dynamic_pointer_cast<RbPrmPathValidation>(pathValidation); |
355 |
|
|
std::vector<std::string> filter; |
356 |
|
|
core::PathValidationReportPtr_t report; |
357 |
|
|
core::PathPtr_t validPart; |
358 |
|
|
/* Define some constants */ |
359 |
|
|
// const core::size_type index = device_.lock ()->configSize() - device_.lock |
360 |
|
|
// ()->extraConfigSpace ().dimension (); |
361 |
|
|
// // ecs index |
362 |
|
|
const value_type x_0 = q1(0); |
363 |
|
|
const value_type y_0 = q1(1); |
364 |
|
|
const value_type z_0 = q1(2); |
365 |
|
|
const value_type x_imp = q2(0); |
366 |
|
|
const value_type y_imp = q2(1); |
367 |
|
|
const value_type z_imp = q2(2); |
368 |
|
|
value_type X = x_imp - x_0; |
369 |
|
|
value_type Y = y_imp - y_0; |
370 |
|
|
value_type Z = z_imp - z_0; |
371 |
|
|
const value_type theta = atan2(Y, X); |
372 |
|
|
const value_type x_theta_0 = cos(theta) * x_0 + sin(theta) * y_0; |
373 |
|
|
const value_type x_theta_imp = cos(theta) * x_imp + sin(theta) * y_imp; |
374 |
|
|
|
375 |
|
|
if (alpha_1_minus_ < 0) |
376 |
|
|
alpha_1_minus_ = 0; // otherwise we go in the wrong direction |
377 |
|
|
value_type interval = |
378 |
|
|
(alpha_1_plus_ - alpha_1_minus_) / |
379 |
|
|
2.; // according to friction cone computed in compute_3d_path |
380 |
|
|
value_type alpha = |
381 |
|
|
(((value_type)rand() / RAND_MAX) * interval) + alpha_1_minus_; |
382 |
|
|
value_type v = (((value_type)rand() / RAND_MAX) * V0max_); |
383 |
|
|
*alpha0 = alpha; |
384 |
|
|
*v0 = v; |
385 |
|
|
hppDout(notice, "Compute random path :"); |
386 |
|
|
hppDout(notice, "alpha_rand = " << alpha); |
387 |
|
|
hppDout(notice, "v_rand = " << v); |
388 |
|
|
|
389 |
|
|
value_type t = 3; // TODO : find better way to do it |
390 |
|
|
value_type x_theta_f = v * cos(alpha) * t + x_theta_0; |
391 |
|
|
value_type x_f = x_theta_f * cos(theta); |
392 |
|
|
value_type y_f = x_theta_f * sin(theta); |
393 |
|
|
value_type z_f = v * sin(alpha) * t - 0.5 * g_ * t * t + z_0; |
394 |
|
|
|
395 |
|
|
X = x_f - x_0; |
396 |
|
|
Y = y_f - y_0; |
397 |
|
|
Z = z_f - z_0; |
398 |
|
|
hppDout(notice, "x_f = " << x_f); |
399 |
|
|
hppDout(notice, "y_f = " << y_f); |
400 |
|
|
hppDout(notice, "z_f = " << z_f); |
401 |
|
|
core::ConfigurationPtr_t qnew(new core::Configuration_t(q2)); |
402 |
|
|
(*qnew)[0] = x_f; |
403 |
|
|
(*qnew)[1] = y_f; |
404 |
|
|
(*qnew)[2] = z_f; |
405 |
|
|
|
406 |
|
|
const value_type X_theta = X * cos(theta) + Y * sin(theta); |
407 |
|
|
#ifdef HPP_DEBUG |
408 |
|
|
const value_type x_theta_0_dot = |
409 |
|
|
sqrt((g_ * X_theta * X_theta) / (2 * (X_theta * tan(alpha) - Z))); |
410 |
|
|
const value_type inv_x_th_dot_0_sq = 1 / (x_theta_0_dot * x_theta_0_dot); |
411 |
|
|
// const value_type v = sqrt((1 + tan(alpha)*tan(alpha))) * x_theta_0_dot; |
412 |
|
|
// hppDout (notice, "v: " << v); |
413 |
|
|
const value_type Vimp = |
414 |
|
|
sqrt(1 + (-g_ * X * inv_x_th_dot_0_sq + tan(alpha)) * |
415 |
|
|
(-g_ * X * inv_x_th_dot_0_sq + tan(alpha))) * |
416 |
|
|
x_theta_0_dot; // x_theta_0_dot > 0 |
417 |
|
|
#endif |
418 |
|
|
hppDout(notice, "Vimp (after 3 seconde) : " << Vimp); |
419 |
|
|
|
420 |
|
|
/* Compute Parabola coefficients */ |
421 |
|
|
vector_t coefs = |
422 |
|
|
computeCoefficients(alpha, theta, X_theta, Z, x_theta_0, z_0); |
423 |
|
|
hppDout(info, "coefs: " << coefs.transpose()); |
424 |
|
|
|
425 |
|
|
// parabola path with alpha_0 as the middle of alpha_0 bounds |
426 |
|
|
ParabolaPathPtr_t pp = ParabolaPath::create( |
427 |
|
|
device_.lock(), q1, *qnew, computeLength(q1, *qnew, coefs), coefs); |
428 |
|
|
bool hasCollisions = |
429 |
|
|
!rbPathValidation->validate(pp, false, validPart, report, filter); |
430 |
|
|
bool maxHeightRespected = |
431 |
|
|
parabMaxHeightRespected(coefs, x_theta_0, x_theta_imp); |
432 |
|
|
|
433 |
|
|
if (hasCollisions || !maxHeightRespected) return core::PathPtr_t(); |
434 |
|
|
hppDout(notice, |
435 |
|
|
"Create path between : init : " << hpp::pinocchio::displayConfig(q1)); |
436 |
|
|
hppDout(notice, "Create path between : goal : " |
437 |
|
|
<< hpp::pinocchio::displayConfig(*qnew)); |
438 |
|
|
return pp; |
439 |
|
|
} |
440 |
|
|
|
441 |
|
|
bool SteeringMethodParabola::second_constraint( |
442 |
|
|
const value_type &X, const value_type &Y, value_type *alpha_lim_plus, |
443 |
|
|
value_type *alpha_lim_minus) const { |
444 |
|
|
bool fail = 0; |
445 |
|
|
const value_type A = g_ * X * X; |
446 |
|
|
const value_type B = -2 * X * V0max_ * V0max_; |
447 |
|
|
const value_type C = g_ * X * X + 2 * Y * V0max_ * V0max_; |
448 |
|
|
const value_type delta = B * B - 4 * A * C; |
449 |
|
|
|
450 |
|
|
if (delta < 0) |
451 |
|
|
fail = 1; |
452 |
|
|
else { |
453 |
|
|
if (X > 0) { |
454 |
|
|
*alpha_lim_plus = atan(0.5 * (-B + sqrt(delta)) / A); |
455 |
|
|
*alpha_lim_minus = atan(0.5 * (-B - sqrt(delta)) / A); |
456 |
|
|
} else { |
457 |
|
|
*alpha_lim_plus = atan(0.5 * (-B + sqrt(delta)) / A) + M_PI; |
458 |
|
|
*alpha_lim_minus = atan(0.5 * (-B - sqrt(delta)) / A) + M_PI; |
459 |
|
|
} |
460 |
|
|
} |
461 |
|
|
return fail; |
462 |
|
|
} |
463 |
|
|
|
464 |
|
|
bool SteeringMethodParabola::third_constraint(bool fail, const value_type &X, |
465 |
|
|
const value_type &Y, |
466 |
|
|
const value_type alpha_imp_min, |
467 |
|
|
const value_type alpha_imp_max, |
468 |
|
|
value_type *alpha_imp_sup, |
469 |
|
|
value_type *alpha_imp_inf, |
470 |
|
|
const value_type n2_angle) const { |
471 |
|
|
if (fail) |
472 |
|
|
return fail; |
473 |
|
|
else { |
474 |
|
|
// Pierre : disable this test, always return true (for testing) |
475 |
|
|
fail = false; |
476 |
|
|
*alpha_imp_sup = alpha_imp_max; |
477 |
|
|
*alpha_imp_inf = alpha_imp_min; |
478 |
|
|
return false; |
479 |
|
|
// ########## ^ a enlever ^ ######## // |
480 |
|
|
if (X > 0) { |
481 |
|
|
if (n2_angle >= 0) { |
482 |
|
|
if (alpha_imp_max > -M_PI / 2) { |
483 |
|
|
*alpha_imp_sup = atan(-tan(alpha_imp_min) + 2 * Y / X); |
484 |
|
|
*alpha_imp_inf = atan(-tan(alpha_imp_max) + 2 * Y / X); |
485 |
|
|
} else |
486 |
|
|
fail = 1; |
487 |
|
|
} else { // n2_angle < 0 |
488 |
|
|
if (alpha_imp_min < M_PI / 2) { |
489 |
|
|
*alpha_imp_sup = atan(-tan(alpha_imp_min) + 2 * Y / X); |
490 |
|
|
*alpha_imp_inf = atan(-tan(alpha_imp_max) + 2 * Y / X); |
491 |
|
|
} else |
492 |
|
|
fail = 1; |
493 |
|
|
} |
494 |
|
|
} else { // X < 0 // TODO: cases n2_angle > 0 or < 0 (2D only) |
495 |
|
|
if (alpha_imp_min < -M_PI / 2) { |
496 |
|
|
*alpha_imp_sup = atan(-tan(alpha_imp_min) + 2 * Y / X) + M_PI; |
497 |
|
|
*alpha_imp_inf = atan(-tan(alpha_imp_max) + 2 * Y / X) + M_PI; |
498 |
|
|
} else |
499 |
|
|
fail = 1; |
500 |
|
|
} |
501 |
|
|
} // ifNotfail |
502 |
|
|
return fail; |
503 |
|
|
} |
504 |
|
|
|
505 |
|
|
// at least one z value must be >= z_0 |
506 |
|
|
bool SteeringMethodParabola::fiveth_constraint(const core::ConfigurationIn_t q, |
507 |
|
|
const value_type theta, |
508 |
|
|
const int /*number*/, |
509 |
|
|
value_type *delta) const { |
510 |
|
|
const size_type index = device_.lock()->configSize() - |
511 |
|
|
device_.lock()->extraConfigSpace().dimension(); |
512 |
|
|
const value_type U = q(index); // n_x |
513 |
|
|
const value_type V = q(index + 1); // n_y |
514 |
|
|
const value_type W = q(index + 2); // n_z |
515 |
|
|
hppDout(info, "U= " << U << ", V= " << V << ", W= " << W); |
516 |
|
|
const value_type phi = atan(mu_); |
517 |
|
|
const value_type denomK = U * U + V * V - W * W * mu_ * mu_; |
518 |
|
|
const bool tanThetaDef = theta != M_PI / 2 && theta != -M_PI / 2; |
519 |
|
|
const value_type psi = M_PI / 2 - atan2(W, U * cos(theta) + V * sin(theta)); |
520 |
|
|
hppDout(info, "psi: " << psi); |
521 |
|
|
const bool nonVerticalCone = (psi < -phi && psi >= -M_PI / 2) || |
522 |
|
|
(psi > phi && psi < M_PI - phi) || |
523 |
|
|
(psi > M_PI + phi && psi <= 3 * M_PI / 2); |
524 |
|
|
value_type epsilon = 1; |
525 |
|
|
if (!nonVerticalCone && denomK < 0) epsilon = -1; |
526 |
|
|
|
527 |
|
|
if (denomK > -1e-6 && denomK < 1e-6) { // denomK (or 'A') = 0 |
528 |
|
|
hppDout(info, "denomK = 0 case"); |
529 |
|
|
if (tanThetaDef) { |
530 |
|
|
const value_type tanTheta = tan(theta); |
531 |
|
|
if (U + V * tanTheta != 0) { |
532 |
|
|
const value_type numH = |
533 |
|
|
mu_ * mu_ * (U * U + V * V * tanTheta * tanTheta) - |
534 |
|
|
U * U * tanTheta * tanTheta - V * V + |
535 |
|
|
2 * U * V * tanTheta * (1 + mu_ * mu_) - |
536 |
|
|
W * W * (1 + tanTheta * tanTheta); |
537 |
|
|
const value_type H = |
538 |
|
|
-numH / (2 * (1 + mu_ * mu_) * fabs(W) * fabs(U + V * tanTheta)); |
539 |
|
|
const value_type cos2delta = H / sqrt(1 + tanTheta * tanTheta + H * H); |
540 |
|
|
hppDout(info, "cos(2*delta): " << cos2delta); |
541 |
|
|
*delta = 0.5 * acos(cos2delta); |
542 |
|
|
hppDout(info, "delta: " << *delta); |
543 |
|
|
assert(*delta <= phi + 1e-5); |
544 |
|
|
return true; |
545 |
|
|
} else { // U + V*tanTheta = 0 |
546 |
|
|
*delta = M_PI / 4; |
547 |
|
|
hppDout(info, "delta: " << *delta); |
548 |
|
|
return true; |
549 |
|
|
} |
550 |
|
|
} else { // theta = +-pi/2 |
551 |
|
|
if (V != 0) { |
552 |
|
|
const value_type L = -(V * V * (1 + mu_ * mu_) - 1) / |
553 |
|
|
(2 * (1 + mu_ * mu_) * fabs(V) * fabs(W)); |
554 |
|
|
const value_type cos2delta = L / sqrt(1 + L * L); |
555 |
|
|
hppDout(info, "cos(2*delta): " << cos2delta); |
556 |
|
|
*delta = 0.5 * acos(cos2delta); |
557 |
|
|
hppDout(info, "delta: " << *delta); |
558 |
|
|
assert(*delta <= phi + 1e-5); |
559 |
|
|
return true; |
560 |
|
|
} else { // V = 0 |
561 |
|
|
hppDout(info, "cone-plane intersection is a line"); |
562 |
|
|
return false; |
563 |
|
|
} |
564 |
|
|
} |
565 |
|
|
} // if denomK = 0 |
566 |
|
|
|
567 |
|
|
if (tanThetaDef) { |
568 |
|
|
value_type x_plus, x_minus, z_x_plus, z_x_minus; |
569 |
|
|
const value_type tantheta = tan(theta); |
570 |
|
|
value_type discr = (U * U + W * W) * mu_ * mu_ - V * V - |
571 |
|
|
U * U * tantheta * tantheta + |
572 |
|
|
(V * V + W * W) * mu_ * mu_ * tantheta * tantheta + |
573 |
|
|
2 * (1 + mu_ * mu_) * U * V * tantheta; |
574 |
|
|
hppDout(info, "discr: " << discr); |
575 |
|
|
if (discr < 0) { |
576 |
|
|
hppDout(info, "cone-plane intersection empty"); |
577 |
|
|
return false; |
578 |
|
|
} |
579 |
|
|
if (discr < 5e-2) { |
580 |
|
|
hppDout(info, "cone-plane intersection too small"); |
581 |
|
|
return false; |
582 |
|
|
} |
583 |
|
|
const value_type K1 = (sqrt(discr) + U * W + U * W * mu_ * mu_ + |
584 |
|
|
V * W * tantheta + V * W * mu_ * mu_ * tantheta) / |
585 |
|
|
denomK; |
586 |
|
|
const value_type K2 = (-sqrt(discr) + U * W + U * W * mu_ * mu_ + |
587 |
|
|
V * W * tantheta + V * W * mu_ * mu_ * tantheta) / |
588 |
|
|
denomK; |
589 |
|
|
hppDout(info, "denomK= " << denomK); |
590 |
|
|
|
591 |
|
|
if (nonVerticalCone) { |
592 |
|
|
// non-vertical up |
593 |
|
|
hppDout(info, "non-vertical up"); |
594 |
|
|
if (U * cos(theta) + V * sin(theta) < 0) |
595 |
|
|
x_minus = -0.5; |
596 |
|
|
else |
597 |
|
|
x_minus = 0.5; |
598 |
|
|
x_plus = x_minus; |
599 |
|
|
z_x_minus = x_minus * K2; |
600 |
|
|
z_x_plus = x_plus * K1; |
601 |
|
|
|
602 |
|
|
if (psi > M_PI / 2) { // down: invert z_plus and z_minus |
603 |
|
|
hppDout(info, "non-vertical down"); |
604 |
|
|
z_x_plus = x_minus * K2; |
605 |
|
|
z_x_minus = x_plus * K1; |
606 |
|
|
} |
607 |
|
|
} else { // "vertical" cone |
608 |
|
|
if (-phi <= psi && psi <= phi) { // up |
609 |
|
|
hppDout(info, "vertical up"); |
610 |
|
|
x_minus = 0.5; |
611 |
|
|
if (denomK < 0) { |
612 |
|
|
x_minus = 0.5; |
613 |
|
|
x_plus = -x_minus; |
614 |
|
|
} else { |
615 |
|
|
x_minus = -0.5; |
616 |
|
|
x_plus = x_minus; |
617 |
|
|
} |
618 |
|
|
z_x_minus = x_minus * K2; |
619 |
|
|
z_x_plus = x_plus * K1; |
620 |
|
|
} else { // down |
621 |
|
|
hppDout(info, "vertical down"); |
622 |
|
|
if (denomK < 0) { |
623 |
|
|
x_minus = -0.5; |
624 |
|
|
x_plus = -x_minus; |
625 |
|
|
} else { |
626 |
|
|
x_minus = 0.5; |
627 |
|
|
x_plus = x_minus; |
628 |
|
|
} |
629 |
|
|
z_x_minus = x_minus * K2; |
630 |
|
|
z_x_plus = x_plus * K1; |
631 |
|
|
} |
632 |
|
|
} |
633 |
|
|
#ifndef HPP_DEBUG |
634 |
|
|
(void)z_x_plus; |
635 |
|
|
(void)z_x_minus; |
636 |
|
|
#endif |
637 |
|
|
// plot outputs |
638 |
|
|
hppDout(info, "q: " << hpp::pinocchio::displayConfig(q)); |
639 |
|
|
hppDout(info, "x_plus: " << x_plus); |
640 |
|
|
hppDout(info, "x_minus: " << x_minus); |
641 |
|
|
hppDout(info, "z_x_plus: " << z_x_plus); |
642 |
|
|
hppDout(info, "z_x_minus: " << z_x_minus); |
643 |
|
|
|
644 |
|
|
value_type cos2delta = epsilon * (1 + tantheta * tantheta + K1 * K2) / |
645 |
|
|
sqrt((1 + tantheta * tantheta + K1 * K1) * |
646 |
|
|
(1 + tantheta * tantheta + K2 * K2)); |
647 |
|
|
hppDout(info, "cos(2*delta): " << cos2delta); |
648 |
|
|
*delta = 0.5 * acos(cos2delta); |
649 |
|
|
hppDout(info, "delta: " << *delta); |
650 |
|
|
assert(*delta <= phi + 1e-5); |
651 |
|
|
return true; |
652 |
|
|
} else { // theta = +-pi/2 |
653 |
|
|
value_type discr = -U * U + (V * V + W * W) * (mu_ * mu_); |
654 |
|
|
hppDout(info, "discr: " << discr); |
655 |
|
|
if (discr < 0) { |
656 |
|
|
hppDout(info, "cone-plane intersection empty"); |
657 |
|
|
return false; |
658 |
|
|
} |
659 |
|
|
if (discr < 5e-2) { |
660 |
|
|
hppDout(info, "cone-plane intersection too small"); |
661 |
|
|
return false; |
662 |
|
|
} |
663 |
|
|
value_type G1 = ((1 + mu_ * mu_) * V * W + sqrt(discr)) / (denomK); |
664 |
|
|
value_type G2 = ((1 + mu_ * mu_) * V * W - sqrt(discr)) / (denomK); |
665 |
|
|
|
666 |
|
|
#ifdef HPP_DEBUG |
667 |
|
|
value_type y = 1; |
668 |
|
|
if (theta == -M_PI / 2) y = -1; |
669 |
|
|
hppDout(info, "y: " << y); |
670 |
|
|
value_type z_y_plus = G1 * y; // TODO: sign selection of y |
671 |
|
|
value_type z_y_minus = G2 * y; |
672 |
|
|
#endif |
673 |
|
|
hppDout(info, "z_y_plus: " << z_y_plus); |
674 |
|
|
hppDout(info, "z_y_minus: " << z_y_minus); |
675 |
|
|
|
676 |
|
|
value_type cos2delta = |
677 |
|
|
epsilon * (1 + G1 * G2) / sqrt((1 + G1 * G1) * (1 + G2 * G2)); |
678 |
|
|
hppDout(info, "cos(2*delta): " << cos2delta); |
679 |
|
|
*delta = 0.5 * acos(cos2delta); |
680 |
|
|
hppDout(info, "delta: " << *delta); |
681 |
|
|
assert(*delta <= phi + 1e-5); |
682 |
|
|
return true; |
683 |
|
|
} |
684 |
|
|
} |
685 |
|
|
|
686 |
|
|
bool SteeringMethodParabola::sixth_constraint( |
687 |
|
|
const value_type &X, const value_type &Y, value_type *alpha_imp_plus, |
688 |
|
|
value_type *alpha_imp_minus) const { |
689 |
|
|
bool fail = 0; |
690 |
|
|
const value_type A = g_ * X * X; |
691 |
|
|
const value_type B = -2 * X * Vimpmax_ * Vimpmax_ - 4 * X * Y * g_; |
692 |
|
|
const value_type C = |
693 |
|
|
g_ * X * X + 2 * Y * Vimpmax_ * Vimpmax_ + 4 * g_ * Y * Y; |
694 |
|
|
const value_type delta = B * B - 4 * A * C; |
695 |
|
|
|
696 |
|
|
if (delta < 0) |
697 |
|
|
fail = 1; |
698 |
|
|
else { |
699 |
|
|
if (X > 0) { |
700 |
|
|
*alpha_imp_plus = atan(0.5 * (-B + sqrt(delta)) / A); |
701 |
|
|
*alpha_imp_minus = atan(0.5 * (-B - sqrt(delta)) / A); |
702 |
|
|
} else { |
703 |
|
|
*alpha_imp_plus = atan(0.5 * (-B + sqrt(delta)) / A) + M_PI; |
704 |
|
|
*alpha_imp_minus = atan(0.5 * (-B - sqrt(delta)) / A) + M_PI; |
705 |
|
|
} |
706 |
|
|
} |
707 |
|
|
return fail; |
708 |
|
|
} |
709 |
|
|
|
710 |
|
|
// Function equivalent to sqrt( 1 + f'(x)^2 ) |
711 |
|
|
value_type SteeringMethodParabola::lengthFunction(const value_type x, |
712 |
|
|
const vector_t coefs) const { |
713 |
|
|
const value_type y = |
714 |
|
|
sqrt(1 + (2 * coefs(0) * x + coefs(1)) * (2 * coefs(0) * x + coefs(1))); |
715 |
|
|
return y; |
716 |
|
|
} |
717 |
|
|
|
718 |
|
|
/* value_type SteeringMethodParabola::computeLength |
719 |
|
|
(const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2, |
720 |
|
|
const vector_t coefs) const { |
721 |
|
|
const int N = 6; // number -1 of interval sub-divisions |
722 |
|
|
// for N = 4, computation error ~= 1e-5. |
723 |
|
|
// for N = 20, computation error ~= 1e-11. |
724 |
|
|
value_type length = 0; |
725 |
|
|
value_type x1 = q1 (0); |
726 |
|
|
value_type x2 = q2 (0); |
727 |
|
|
const value_type theta = coefs (3); |
728 |
|
|
x1 = cos(theta) * q1 (0) + sin(theta) * q1 (1); // x_theta_0 |
729 |
|
|
x2 = cos(theta) * q2 (0) + sin(theta) * q2 (1); // x_theta_imp |
730 |
|
|
|
731 |
|
|
// Define integration bounds |
732 |
|
|
if (x1 > x2) { // re-order integration bounds |
733 |
|
|
const value_type xtmp = x1; |
734 |
|
|
x1 = x2; |
735 |
|
|
x2 = xtmp; |
736 |
|
|
} |
737 |
|
|
|
738 |
|
|
const value_type dx = (x2 - x1) / N; // integration step size |
739 |
|
|
for (int i=0; i<N; i++) { |
740 |
|
|
length += dx*( 0.166666667*lengthFunction (x1 + i*dx, coefs) |
741 |
|
|
+ 0.666666667*lengthFunction (x1 + (i+0.5)*dx, coefs) |
742 |
|
|
+ 0.166666667*lengthFunction (x1 + (i+1)*dx, coefs)); |
743 |
|
|
// apparently, 1/6 and 2/3 are not recognized as floats ... |
744 |
|
|
} |
745 |
|
|
hppDout (info, "length = " << length); |
746 |
|
|
return length; |
747 |
|
|
}*/ |
748 |
|
|
|
749 |
|
|
// test (pierre) : |
750 |
|
|
value_type SteeringMethodParabola::computeLength( |
751 |
|
|
const core::ConfigurationIn_t q1, const core::ConfigurationIn_t q2, |
752 |
|
|
const vector_t coefs) const { |
753 |
|
|
const value_type theta = coefs(3); |
754 |
|
|
const value_type X = q2[0] - q1[0]; |
755 |
|
|
const value_type Y = q2[1] - q1[1]; |
756 |
|
|
; |
757 |
|
|
// theta = coef[3] |
758 |
|
|
const value_type X_theta = X * cos(theta) + Y * sin(theta); |
759 |
|
|
return X_theta; |
760 |
|
|
} |
761 |
|
|
|
762 |
|
|
vector_t SteeringMethodParabola::computeCoefficients( |
763 |
|
|
const value_type alpha, const value_type theta, const value_type X_theta, |
764 |
|
|
const value_type Z, const value_type x_theta_0, |
765 |
|
|
const value_type z_0) const { |
766 |
|
|
vector_t coefs(7); |
767 |
|
|
const value_type x_theta_0_dot = |
768 |
|
|
sqrt((g_ * X_theta * X_theta) / (2 * (X_theta * tan(alpha) - Z))); |
769 |
|
|
const value_type inv_x_th_dot_0_sq = 1 / (x_theta_0_dot * x_theta_0_dot); |
770 |
|
|
coefs(0) = -0.5 * g_ * inv_x_th_dot_0_sq; |
771 |
|
|
coefs(1) = tan(alpha) + g_ * x_theta_0 * inv_x_th_dot_0_sq; |
772 |
|
|
coefs(2) = z_0 - tan(alpha) * x_theta_0 - |
773 |
|
|
0.5 * g_ * x_theta_0 * x_theta_0 * inv_x_th_dot_0_sq; |
774 |
|
|
coefs(3) = theta; |
775 |
|
|
coefs(4) = alpha; |
776 |
|
|
coefs(5) = x_theta_0_dot; |
777 |
|
|
coefs(6) = x_theta_0; |
778 |
|
|
// Also compute initial and final velocities |
779 |
|
|
const value_type V0 = sqrt((1 + tan(alpha) * tan(alpha))) * x_theta_0_dot; |
780 |
|
|
#ifdef HPP_DEBUG |
781 |
|
|
const value_type Vimp = |
782 |
|
|
sqrt(1 + (-g_ * X_theta * inv_x_th_dot_0_sq + tan(alpha)) * |
783 |
|
|
(-g_ * X_theta * inv_x_th_dot_0_sq + tan(alpha))) * |
784 |
|
|
x_theta_0_dot; |
785 |
|
|
#endif |
786 |
|
|
hppDout(info, "V0: " << V0); |
787 |
|
|
hppDout(info, "Vimp: " << Vimp); |
788 |
|
|
V0_[0] = x_theta_0_dot * cos(theta); |
789 |
|
|
V0_[1] = x_theta_0_dot * sin(theta); |
790 |
|
|
V0_[2] = V0 * sin(alpha); |
791 |
|
|
Vimp_[0] = x_theta_0_dot * cos(theta); // x_theta_imp_dot = x_theta_0_dot |
792 |
|
|
Vimp_[1] = x_theta_0_dot * sin(theta); |
793 |
|
|
Vimp_[2] = -g_ * X_theta / x_theta_0_dot + x_theta_0_dot * tan(alpha); |
794 |
|
|
return coefs; |
795 |
|
|
} |
796 |
|
|
|
797 |
|
|
bool SteeringMethodParabola::parabMaxHeightRespected( |
798 |
|
|
const vector_t coefs, const value_type x_theta_0, |
799 |
|
|
const value_type x_theta_imp) const { |
800 |
|
|
const value_type x_theta_max = -0.5 * coefs(1) / coefs(0); |
801 |
|
|
const value_type z_x_theta_max = |
802 |
|
|
coefs(0) * x_theta_max * x_theta_max + coefs(1) * x_theta_max + coefs(2); |
803 |
|
|
if (x_theta_0 <= x_theta_max && x_theta_max <= x_theta_imp) { |
804 |
|
|
if (z_x_theta_max > device_.lock()->rootJoint()->upperBound(2)) { |
805 |
|
|
// hppDout (info, "z_x_theta_max: " << z_x_theta_max); |
806 |
|
|
return false; |
807 |
|
|
} |
808 |
|
|
} |
809 |
|
|
return true; |
810 |
|
|
} |
811 |
|
|
|
812 |
|
|
value_type SteeringMethodParabola::dichotomy(value_type a_inf, |
813 |
|
|
value_type a_plus, |
814 |
|
|
std::size_t n) const { |
815 |
|
|
value_type alpha, e; // e in ]0,1[ |
816 |
|
|
switch (n) { // NLimit_ <= 6, otherwise fill missing values for n>5 |
817 |
|
|
case 0: |
818 |
|
|
e = 0.25; |
819 |
|
|
break; |
820 |
|
|
case 1: |
821 |
|
|
e = 0.75; |
822 |
|
|
break; |
823 |
|
|
case 2: |
824 |
|
|
e = 0.125; |
825 |
|
|
break; |
826 |
|
|
case 3: |
827 |
|
|
e = 0.375; |
828 |
|
|
break; |
829 |
|
|
case 4: |
830 |
|
|
e = 0.625; |
831 |
|
|
break; |
832 |
|
|
case 5: |
833 |
|
|
e = 0.875; |
834 |
|
|
break; |
835 |
|
|
default: |
836 |
|
|
e = 0.5; |
837 |
|
|
break; // not supposed to happen |
838 |
|
|
} |
839 |
|
|
alpha = e * a_plus + (1 - e) * a_inf; |
840 |
|
|
return alpha; |
841 |
|
|
} |
842 |
|
|
|
843 |
|
|
void SteeringMethodParabola::fillROMnames( |
844 |
|
|
core::ConfigurationIn_t q, std::vector<std::string> *ROMnames) const { |
845 |
|
|
core::ValidationReportPtr_t report; |
846 |
|
|
const core::Configuration_t config = q; |
847 |
|
|
problem()->configValidations()->validate(config, report); |
848 |
|
|
core::RbprmValidationReportPtr_t rbReport = |
849 |
|
|
dynamic_pointer_cast<core::RbprmValidationReport>(report); |
850 |
|
|
if (rbReport) { |
851 |
|
|
hppDout(info, "nbROM= " << rbReport->ROMReports.size()); |
852 |
|
|
for (std::map<std::string, |
853 |
|
|
core::CollisionValidationReportPtr_t>::const_iterator it = |
854 |
|
|
rbReport->ROMReports.begin(); |
855 |
|
|
it != rbReport->ROMReports.end(); it++) { |
856 |
|
|
std::string ROMname = it->first; |
857 |
|
|
hppDout(info, "ROMname= " << ROMname); |
858 |
|
|
(*ROMnames).push_back(ROMname); |
859 |
|
|
} |
860 |
|
|
|
861 |
|
|
} else { |
862 |
|
|
hppDout(error, "Validation Report cannot be cast"); |
863 |
|
|
} |
864 |
|
|
} |
865 |
|
|
|
866 |
|
|
} // namespace rbprm |
867 |
|
|
} // namespace hpp |