| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// Copyright (c) 2016, LAAS-CNRS |
||
2 |
// Authors: Pierre Fernbach (pierre.fernbach@laas.fr) |
||
3 |
// |
||
4 |
// This file is part of hpp-core |
||
5 |
// hpp-core is free software: you can redistribute it |
||
6 |
// and/or modify it under the terms of the GNU Lesser General Public |
||
7 |
// License as published by the Free Software Foundation, either version |
||
8 |
// 3 of the License, or (at your option) any later version. |
||
9 |
// |
||
10 |
// hpp-core is distributed in the hope that it will be |
||
11 |
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
||
12 |
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
||
13 |
// General Lesser Public License for more details. You should have |
||
14 |
// received a copy of the GNU Lesser General Public License along with |
||
15 |
// hpp-core If not, see |
||
16 |
// <http://www.gnu.org/licenses/>. |
||
17 |
|||
18 |
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh> |
||
19 |
#include <hpp/core/kinodynamic-path.hh> |
||
20 |
#include <hpp/core/problem.hh> |
||
21 |
#include <hpp/core/weighed-distance.hh> |
||
22 |
#include <hpp/pinocchio/device.hh> |
||
23 |
#include <hpp/rbprm/planner/rbprm-node.hh> |
||
24 |
#include <hpp/rbprm/planner/rbprm-steering-kinodynamic.hh> |
||
25 |
#include <hpp/util/debug.hh> |
||
26 |
#include <hpp/util/timer.hh> |
||
27 |
|||
28 |
#define ignore_acc_bound 0 // testing and debug only |
||
29 |
|||
30 |
namespace hpp { |
||
31 |
namespace rbprm { |
||
32 |
|||
33 |
using centroidal_dynamics::MatrixXX; |
||
34 |
using centroidal_dynamics::Vector3; |
||
35 |
|||
36 |
37 |
SteeringMethodKinodynamic::SteeringMethodKinodynamic( |
|
37 |
37 |
core::ProblemConstPtr_t problem) |
|
38 |
: core::steeringMethod::Kinodynamic(problem), |
||
39 |
totalTimeComputed_(0), |
||
40 |
totalTimeValidated_(0), |
||
41 |
dirValid_(0), |
||
42 |
dirTotal_(0), |
||
43 |
rejectedPath_(0), |
||
44 |
maxLength_(50), |
||
45 |
37 |
device_(problem->robot()), |
|
46 |
lastDirection_(), |
||
47 |
sEq_(new centroidal_dynamics::Equilibrium( |
||
48 |
✓✗ | 37 |
problem->robot()->name(), problem->robot()->mass(), 4, |
49 |
✓✗✓✗ |
37 |
centroidal_dynamics::SOLVER_LP_QPOASES, true, 10, false)), |
50 |
boundsUpToDate_(false), |
||
51 |
✓✗ | 111 |
weak_() { |
52 |
✓✗ | 37 |
lastDirection_.setZero(); |
53 |
37 |
} |
|
54 |
|||
55 |
/// Copy constructor |
||
56 |
SteeringMethodKinodynamic::SteeringMethodKinodynamic( |
||
57 |
const SteeringMethodKinodynamic& other) |
||
58 |
: core::steeringMethod::Kinodynamic(other), |
||
59 |
totalTimeComputed_(0), |
||
60 |
totalTimeValidated_(0), |
||
61 |
dirValid_(0), |
||
62 |
dirTotal_(0), |
||
63 |
rejectedPath_(0), |
||
64 |
maxLength_(50), |
||
65 |
device_(other.device_), |
||
66 |
lastDirection_(other.lastDirection_), |
||
67 |
sEq_(new centroidal_dynamics::Equilibrium( |
||
68 |
problem()->robot()->name(), problem()->robot()->mass(), 4, |
||
69 |
centroidal_dynamics::SOLVER_LP_QPOASES, true, 10, false)), |
||
70 |
boundsUpToDate_(false), |
||
71 |
weak_() {} |
||
72 |
|||
73 |
core::PathPtr_t SteeringMethodKinodynamic::impl_compute( |
||
74 |
core::ConfigurationIn_t q1, core::ConfigurationIn_t q2) const { |
||
75 |
hppDout(notice, "Old prototype called !!!"); |
||
76 |
std::cout << "derecated prototype of steering method called" << std::endl; |
||
77 |
return core::steeringMethod::Kinodynamic::impl_compute(q1, q2); |
||
78 |
} |
||
79 |
|||
80 |
13579 |
core::PathPtr_t SteeringMethodKinodynamic::impl_compute( |
|
81 |
core::NodePtr_t x, core::ConfigurationIn_t q2) { |
||
82 |
13579 |
core::RbprmNodePtr_t node = static_cast<core::RbprmNodePtr_t>(x); |
|
83 |
✗✓ | 13579 |
assert(node && "Unable to cast near node to rbprmNode"); |
84 |
✗✓ | 13579 |
if (!node) return core::PathPtr_t(); |
85 |
// get kinodynamic path from core::steeringMethod::Kinodynamic |
||
86 |
hppStartBenchmark(FIND_A_MAX); |
||
87 |
✓✗✓✗ |
27158 |
core::PathPtr_t unboundedPath = setSteeringMethodBounds(node, q2, false); |
88 |
hppDout(notice, "end setBounds"); |
||
89 |
hppStopBenchmark(FIND_A_MAX); |
||
90 |
hppDisplayBenchmark(FIND_A_MAX); |
||
91 |
✓✗✓✗ ✗✓ |
13579 |
if ((std::fabs(aMax_[0]) + std::fabs(aMax_[1])) <= 0) |
92 |
return core::PathPtr_t(); |
||
93 |
✓✓ | 13579 |
if (boundsUpToDate_) return unboundedPath; |
94 |
// return |
||
95 |
// core::steeringMethod::Kinodynamic::impl_compute(*x->configuration(),q2); |
||
96 |
hppStartBenchmark(steering_kino); |
||
97 |
core::PathPtr_t path = |
||
98 |
✓✗✓✗ ✓✗✓✗ |
18 |
core::steeringMethod::Kinodynamic::impl_compute(*x->configuration(), q2); |
99 |
hppStopBenchmark(steering_kino); |
||
100 |
hppDisplayBenchmark(steering_kino); |
||
101 |
✗✓ | 6 |
if (!path) return core::PathPtr_t(); |
102 |
core::KinodynamicPathPtr_t kinoPath = |
||
103 |
12 |
dynamic_pointer_cast<core::KinodynamicPath>(path); |
|
104 |
✓✗✓✓ |
6 |
if (kinoPath->length() > maxLength_) { |
105 |
1 |
rejectedPath_++; |
|
106 |
1 |
return core::PathPtr_t(); |
|
107 |
} |
||
108 |
✓✗✓✗ |
5 |
Vector3 direction = kinoPath->getA1(); |
109 |
✓✗ | 5 |
direction.normalize(); |
110 |
5 |
dirTotal_++; |
|
111 |
✓✗✓✗ |
5 |
if (std::fabs(direction.dot(lastDirection_)) >= 0.8) dirValid_++; |
112 |
|||
113 |
✓✗ | 5 |
totalTimeComputed_ += kinoPath->length(); |
114 |
hppDout(notice, "TotaltimeComputed = " << totalTimeComputed_); |
||
115 |
|||
116 |
✓✗ | 5 |
assert(path && "Error while casting path shared ptr"); // really usefull ? |
117 |
// should never happen |
||
118 |
core::size_type configSize = |
||
119 |
✓✗ | 5 |
problem()->robot()->configSize() - |
120 |
5 |
problem()->robot()->extraConfigSpace().dimension(); |
|
121 |
|||
122 |
#if !ignore_acc_bound |
||
123 |
// check if acceleration is valid after each sign change : |
||
124 |
hppStartBenchmark(INTERMEDIATE_ACCELERATION_CHECKS); |
||
125 |
✓✗ | 10 |
core::vector_t t0 = kinoPath->getT0(); |
126 |
✓✗ | 10 |
core::vector_t t1 = kinoPath->getT1(); |
127 |
✓✗ | 10 |
core::vector_t tv = kinoPath->getTv(); |
128 |
5 |
double t = 0; |
|
129 |
core::ConfigurationPtr_t q( |
||
130 |
✓✗✓✗ ✓✗✓✗ |
10 |
new core::Configuration_t(problem()->robot()->configSize())); |
131 |
✓✗ | 5 |
core::vector3_t a; |
132 |
bool aValid; |
||
133 |
✓✗ | 5 |
double maxT = kinoPath->length(); |
134 |
hppDout(info, "## start checking intermediate accelerations"); |
||
135 |
5 |
double epsilon = 0.0001; |
|
136 |
5 |
t = epsilon; |
|
137 |
✓✗✓✗ |
5 |
(*kinoPath)(*q, t); |
138 |
hppDout(info, "q(t=" << t << ") = " << pinocchio::displayConfig(*q)); |
||
139 |
✓✗✓✗ |
5 |
a = (*q).segment<3>(configSize + 3); |
140 |
hppDout(info, "a = " << a); |
||
141 |
✓✗✓✗ ✓✗ |
5 |
sEq_->setG(node->getG()); |
142 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
5 |
aValid = sEq_->checkAdmissibleAcceleration(node->getH(), node->geth(), a); |
143 |
hppDout(info, "a valid : " << aValid); |
||
144 |
✗✓ | 5 |
if (!aValid) { |
145 |
return core::PathPtr_t(); |
||
146 |
} |
||
147 |
✓✓ | 20 |
for (size_t ijoint = 0; ijoint < 3; ijoint++) { |
148 |
15 |
t = epsilon; |
|
149 |
✓✗✗✓ |
15 |
if (t0[ijoint] > 0) { |
150 |
hppDout(info, "for joint " << ijoint); |
||
151 |
t = t0[ijoint] + |
||
152 |
epsilon; // add an epsilon to get the value after the sign change |
||
153 |
(*kinoPath)(*q, t); |
||
154 |
hppDout(info, "q(t=" << t << ") = " << pinocchio::displayConfig(*q)); |
||
155 |
a = (*q).segment<3>(configSize + 3); |
||
156 |
hppDout(info, "a = " << a); |
||
157 |
aValid = sEq_->checkAdmissibleAcceleration(node->getH(), node->geth(), a); |
||
158 |
hppDout(info, "a valid : " << aValid); |
||
159 |
if (!aValid && t < maxT) maxT = t; |
||
160 |
} |
||
161 |
✓✗✓✓ |
15 |
if (t1[ijoint] > 0) { |
162 |
hppDout(info, "for joint " << ijoint); |
||
163 |
✓✗ | 5 |
t += t1[ijoint]; // add an epsilon to get the value after the sign change |
164 |
✓✗✓✗ |
5 |
(*kinoPath)(*q, t); |
165 |
hppDout(info, "q(t=" << t << ") = " << pinocchio::displayConfig(*q)); |
||
166 |
✓✗✓✗ |
5 |
a = (*q).segment<3>(configSize + 3); |
167 |
hppDout(info, "a = " << a); |
||
168 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
5 |
aValid = sEq_->checkAdmissibleAcceleration(node->getH(), node->geth(), a); |
169 |
hppDout(info, "a valid : " << aValid); |
||
170 |
✗✓✗✗ |
5 |
if (!aValid && t < maxT) maxT = t; |
171 |
} |
||
172 |
✓✗✓✓ |
15 |
if (tv[ijoint] > 0) { |
173 |
✓✗ | 12 |
t += tv[ijoint]; |
174 |
✓✗✓✗ |
12 |
(*kinoPath)(*q, t); |
175 |
hppDout(info, "q(t=" << t << ") = " << pinocchio::displayConfig(*q)); |
||
176 |
✓✗✓✗ |
12 |
a = (*q).segment<3>(configSize + 3); |
177 |
hppDout(info, "a = " << a); |
||
178 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
12 |
aValid = sEq_->checkAdmissibleAcceleration(node->getH(), node->geth(), a); |
179 |
hppDout(info, "a valid : " << aValid); |
||
180 |
✗✓✗✗ |
12 |
if (!aValid && t < maxT) maxT = t; |
181 |
} |
||
182 |
} |
||
183 |
|||
184 |
hppDout(info, "t = " << kinoPath->length() << " maxT = " << maxT); |
||
185 |
✓✗✗✓ |
5 |
if (maxT < kinoPath->length()) { |
186 |
maxT -= epsilon; |
||
187 |
totalTimeValidated_ += maxT; |
||
188 |
hppDout(notice, "totalTimeValidated = " << totalTimeValidated_); |
||
189 |
core::PathPtr_t extracted = kinoPath->extract(core::interval_t( |
||
190 |
kinoPath->timeRange().first, kinoPath->timeRange().first + maxT)); |
||
191 |
hppDout(notice, "extracted path : end = " |
||
192 |
<< pinocchio::displayConfig((extracted->end()))); |
||
193 |
return extracted; |
||
194 |
} |
||
195 |
✓✗ | 5 |
totalTimeValidated_ += kinoPath->length(); |
196 |
hppDout(notice, "totalTimeValidated = " << totalTimeValidated_); |
||
197 |
hppStopBenchmark(INTERMEDIATE_ACCELERATION_CHECKS); |
||
198 |
hppDisplayBenchmark(INTERMEDIATE_ACCELERATION_CHECKS); |
||
199 |
#endif |
||
200 |
5 |
return kinoPath; |
|
201 |
} |
||
202 |
|||
203 |
// reverse (from q1 to x, but end() should always be x) |
||
204 |
431 |
core::PathPtr_t SteeringMethodKinodynamic::impl_compute( |
|
205 |
core::ConfigurationIn_t q1, core::NodePtr_t x) { |
||
206 |
431 |
core::RbprmNodePtr_t node = static_cast<core::RbprmNodePtr_t>(x); |
|
207 |
✗✓ | 431 |
assert(node && "Unable to cast near node to rbprmNode"); |
208 |
✗✓ | 431 |
if (!node) return core::PathPtr_t(); |
209 |
hppStartBenchmark(FIND_A_MAX); |
||
210 |
✓✗✓✗ |
862 |
core::PathPtr_t unboundedPath = setSteeringMethodBounds(node, q1, true); |
211 |
hppStopBenchmark(FIND_A_MAX); |
||
212 |
hppDisplayBenchmark(FIND_A_MAX); |
||
213 |
✓✗✓✗ ✗✓ |
431 |
if ((std::fabs(aMax_[0]) + std::fabs(aMax_[1])) <= 0) |
214 |
return core::PathPtr_t(); |
||
215 |
✓✗ | 431 |
if (boundsUpToDate_) return unboundedPath; |
216 |
hppStartBenchmark(steering_kino); |
||
217 |
core::PathPtr_t path = |
||
218 |
core::steeringMethod::Kinodynamic::impl_compute(q1, *x->configuration()); |
||
219 |
hppStopBenchmark(steering_kino); |
||
220 |
hppDisplayBenchmark(steering_kino); |
||
221 |
|||
222 |
if (!path) return core::PathPtr_t(); |
||
223 |
core::KinodynamicPathPtr_t kinoPath = |
||
224 |
dynamic_pointer_cast<core::KinodynamicPath>(path); |
||
225 |
if (kinoPath->length() > maxLength_) { |
||
226 |
rejectedPath_++; |
||
227 |
return core::PathPtr_t(); |
||
228 |
} |
||
229 |
totalTimeComputed_ += kinoPath->length(); |
||
230 |
Vector3 direction = kinoPath->getA1(); |
||
231 |
direction.normalize(); |
||
232 |
dirTotal_++; |
||
233 |
if (std::fabs(direction.dot(lastDirection_)) >= 0.8) dirValid_++; |
||
234 |
|||
235 |
hppStartBenchmark(INTERMEDIATE_ACCELERATION_CHECKS); |
||
236 |
hppDout(notice, "TotaltimeComputed = " << totalTimeComputed_); |
||
237 |
assert(path && "Error while casting path shared ptr"); // really usefull ? |
||
238 |
// should never happen |
||
239 |
core::size_type configSize = |
||
240 |
problem()->robot()->configSize() - |
||
241 |
problem()->robot()->extraConfigSpace().dimension(); |
||
242 |
// check if acceleration is valid after each sign change : |
||
243 |
core::vector_t t0 = kinoPath->getT0(); |
||
244 |
core::vector_t t1 = kinoPath->getT1(); |
||
245 |
core::vector_t tv = kinoPath->getTv(); |
||
246 |
double t = 0; |
||
247 |
core::ConfigurationPtr_t q( |
||
248 |
new core::Configuration_t(problem()->robot()->configSize())); |
||
249 |
core::vector3_t a; |
||
250 |
bool aValid; |
||
251 |
double minT = 0; |
||
252 |
|||
253 |
hppDout(info, "## start checking intermediate accelerations"); |
||
254 |
double epsilon = 0.0001; |
||
255 |
t = kinoPath->length() - epsilon; |
||
256 |
(*kinoPath)(*q, t); |
||
257 |
hppDout(info, "q(t=" << t << ") = " << pinocchio::displayConfig(*q)); |
||
258 |
a = (*q).segment<3>(configSize + 3); |
||
259 |
hppDout(info, "a = " << a); |
||
260 |
sEq_->setG(node->getG()); |
||
261 |
aValid = sEq_->checkAdmissibleAcceleration(node->getH(), node->geth(), a); |
||
262 |
hppDout(info, "a valid : " << aValid); |
||
263 |
if (!aValid) { |
||
264 |
return core::PathPtr_t(); |
||
265 |
} |
||
266 |
for (size_t ijoint = 0; ijoint < 3; ijoint++) { |
||
267 |
hppDout(info, "for joint " << ijoint); |
||
268 |
t = -epsilon; |
||
269 |
if (t0[ijoint] > 0) { |
||
270 |
hppDout(info, "for joint " << ijoint); |
||
271 |
t = t0[ijoint] - |
||
272 |
epsilon; // add an epsilon to get the value after the sign change |
||
273 |
(*kinoPath)(*q, t); |
||
274 |
hppDout(info, "q(t=" << t << ") = " << pinocchio::displayConfig(*q)); |
||
275 |
a = (*q).segment<3>(configSize + 3); |
||
276 |
hppDout(info, "a = " << a); |
||
277 |
aValid = sEq_->checkAdmissibleAcceleration(node->getH(), node->geth(), a); |
||
278 |
hppDout(info, "a valid : " << aValid); |
||
279 |
if (!aValid && t > minT) minT = t; |
||
280 |
} |
||
281 |
if (t1[ijoint] > 0) { |
||
282 |
t += t1[ijoint]; |
||
283 |
(*kinoPath)(*q, t); |
||
284 |
hppDout(info, "q(t=" << t << ") = " << pinocchio::displayConfig(*q)); |
||
285 |
a = (*q).segment<3>(configSize + 3); |
||
286 |
hppDout(info, "a = " << a); |
||
287 |
aValid = sEq_->checkAdmissibleAcceleration(node->getH(), node->geth(), a); |
||
288 |
hppDout(info, "a valid : " << aValid); |
||
289 |
if (!aValid && t > minT) minT = t; |
||
290 |
} |
||
291 |
if (tv[ijoint] > 0) { |
||
292 |
t += tv[ijoint]; |
||
293 |
(*kinoPath)(*q, t); |
||
294 |
hppDout(info, "q(t=" << t << ") = " << pinocchio::displayConfig(*q)); |
||
295 |
a = (*q).segment<3>(configSize + 3); |
||
296 |
hppDout(info, "a = " << a); |
||
297 |
aValid = sEq_->checkAdmissibleAcceleration(node->getH(), node->geth(), a); |
||
298 |
hppDout(info, "a valid : " << aValid); |
||
299 |
if (!aValid && t > minT) minT = t; |
||
300 |
} |
||
301 |
} |
||
302 |
hppDout(info, "t = " << kinoPath->length() << " minT = " << minT); |
||
303 |
if (minT > 0) { |
||
304 |
minT += epsilon; |
||
305 |
totalTimeValidated_ += (kinoPath->length() - minT); |
||
306 |
hppDout(notice, "totalTimeValidated = " << totalTimeValidated_); |
||
307 |
core::PathPtr_t extracted = |
||
308 |
kinoPath->extract(core::interval_t(minT, kinoPath->timeRange().second)); |
||
309 |
hppDout(notice, "extracted path : end = " |
||
310 |
<< pinocchio::displayConfig((extracted->end()))); |
||
311 |
return extracted; |
||
312 |
} |
||
313 |
totalTimeValidated_ += kinoPath->length(); |
||
314 |
hppDout(notice, "totalTimeValidated = " << totalTimeValidated_); |
||
315 |
hppStopBenchmark(INTERMEDIATE_ACCELERATION_CHECKS); |
||
316 |
hppDisplayBenchmark(INTERMEDIATE_ACCELERATION_CHECKS); |
||
317 |
|||
318 |
return kinoPath; |
||
319 |
} |
||
320 |
|||
321 |
14010 |
core::PathPtr_t SteeringMethodKinodynamic::computeDirection( |
|
322 |
const core::ConfigurationIn_t from, const core::ConfigurationIn_t to, |
||
323 |
bool reverse) { |
||
324 |
hppDout(notice, "Compute direction "); |
||
325 |
14010 |
core::PathPtr_t path; |
|
326 |
✓✓ | 14010 |
if (reverse) |
327 |
✓✗✓✗ ✓✗ |
431 |
path = core::steeringMethod::Kinodynamic::impl_compute(to, from); |
328 |
else |
||
329 |
✓✗✓✗ ✓✗ |
13579 |
path = core::steeringMethod::Kinodynamic::impl_compute(from, to); |
330 |
|||
331 |
✓✗ | 14010 |
Vector3 direction; |
332 |
✓✗ | 14010 |
direction = Vector3(0, 0, 0); |
333 |
✓✗ | 14010 |
if (path) { |
334 |
core::KinodynamicPathPtr_t kinoPath = |
||
335 |
28020 |
dynamic_pointer_cast<core::KinodynamicPath>(path); |
|
336 |
✓✓ | 14010 |
if (kinoPath) { |
337 |
✓✗✓✗ |
13949 |
direction = kinoPath->getA1(); |
338 |
✓✗ | 13949 |
direction.normalize(); |
339 |
} |
||
340 |
} |
||
341 |
✓✗ | 14010 |
lastDirection_ = direction; |
342 |
28020 |
return path; |
|
343 |
} |
||
344 |
|||
345 |
14010 |
core::PathPtr_t SteeringMethodKinodynamic::setSteeringMethodBounds( |
|
346 |
const core::RbprmNodePtr_t& node, const core::ConfigurationIn_t target, |
||
347 |
bool reverse) { |
||
348 |
✓✗✓✗ ✓✗ |
14010 |
Vector3 aMax = Vector3::Ones(3) * aMaxFixed_; |
349 |
✓✗ | 14010 |
aMax[2] = aMaxFixed_Z_; |
350 |
✓✗✓✗ |
14010 |
setAmax(aMax); |
351 |
|||
352 |
hppDout(notice, "Set bounds between : "); |
||
353 |
if (reverse) { |
||
354 |
hppDout(notice, "target : " << pinocchio::displayConfig(target)); |
||
355 |
hppDout(notice, |
||
356 |
"node : " << pinocchio::displayConfig(*(node->configuration()))); |
||
357 |
} else { |
||
358 |
hppDout(notice, |
||
359 |
"node : " << pinocchio::displayConfig(*(node->configuration()))); |
||
360 |
hppDout(notice, "target : " << pinocchio::displayConfig(target)); |
||
361 |
} |
||
362 |
|||
363 |
14010 |
double alpha0 = 1.; // main variable of our LP problem |
|
364 |
/* Vector3 toP,fromP,dPosition; |
||
365 |
Vector3 toV,fromV,dVelocity; |
||
366 |
const pinocchio::size_type indexECS =problem()->robot()->configSize() - |
||
367 |
problem()->robot()->extraConfigSpace().dimension (); // ecs index |
||
368 |
hppDout(notice,"near = |
||
369 |
"<<pinocchio::displayConfig((*(node->configuration())))); |
||
370 |
hppDout(notice,"target = "<<pinocchio::displayConfig(target)); |
||
371 |
if(reverse){ |
||
372 |
toP = node->configuration()->head(3); |
||
373 |
fromP = target.head(3); |
||
374 |
toV = node->configuration()->segment<3>(indexECS); |
||
375 |
fromV = target.segment<3>(indexECS); |
||
376 |
}else{ |
||
377 |
fromP = node->configuration()->head(3); |
||
378 |
toP = target.head(3); |
||
379 |
fromV = node->configuration()->segment<3>(indexECS); |
||
380 |
toV = target.segment<3>(indexECS); |
||
381 |
} |
||
382 |
dPosition = (toP - fromP); |
||
383 |
// dPosition.normalize(); |
||
384 |
dVelocity = (toV - fromV); |
||
385 |
// dVelocity.normalize(); |
||
386 |
hppDout(info, "delta position = "<<dPosition.transpose()); |
||
387 |
hppDout(info, "delta velocity = "<<dVelocity.transpose()); |
||
388 |
//direction = dPosition + dVelocity; |
||
389 |
direction = dPosition; |
||
390 |
direction.normalize(); |
||
391 |
*/ |
||
392 |
core::PathPtr_t path = |
||
393 |
✓✗✓✗ ✓✗✓✗ |
42030 |
computeDirection(*(node->configuration()), target, reverse); |
394 |
|||
395 |
✓✗✓✓ |
14010 |
if (lastDirection_.norm() <= std::numeric_limits<double>::epsilon()) { |
396 |
hppDout(notice, |
||
397 |
"Steering method kinodynamic failed to connect both states, return " |
||
398 |
"empty path"); |
||
399 |
61 |
boundsUpToDate_ = true; |
|
400 |
61 |
return core::PathPtr_t(); |
|
401 |
} |
||
402 |
|||
403 |
hppDout(info, "direction = " << lastDirection_.transpose()); |
||
404 |
hppDout(info, "vector = [" << (*(node->configuration()))[0] << "," |
||
405 |
<< (*(node->configuration()))[1] << "," |
||
406 |
<< (*(node->configuration()))[2] << "," |
||
407 |
<< lastDirection_[0] << "," << lastDirection_[1] |
||
408 |
<< "," << lastDirection_[2] << ",0]"); |
||
409 |
hppDout(notice, "number of contacts : " << node->getNumberOfContacts()); |
||
410 |
|||
411 |
// call to centroidal_dynamics_lib : |
||
412 |
✓✗✓✗ ✓✗ |
13949 |
sEq_->setG(node->getG()); |
413 |
✓✗✓✗ ✓✗ |
27898 |
centroidal_dynamics::LP_status lpStatus = sEq_->findMaximumAcceleration( |
414 |
✓✗✓✗ ✓✗ |
41847 |
node->getH(), node->geth(), lastDirection_, alpha0); |
415 |
if (lpStatus == centroidal_dynamics::LP_STATUS_UNBOUNDED) { |
||
416 |
hppDout(notice, "Primal LP problem is unbounded : " << (lpStatus)); |
||
417 |
} else if (lpStatus == centroidal_dynamics::LP_STATUS_OPTIMAL) { |
||
418 |
hppDout(notice, "Primal LP correctly solved: " << (lpStatus)); |
||
419 |
} else if (lpStatus == centroidal_dynamics::LP_STATUS_INFEASIBLE) { |
||
420 |
hppDout(notice, "Primal LP problem could not be solved: " << (lpStatus)); |
||
421 |
} else { |
||
422 |
hppDout(notice, "Unknown error in LP : " << lpStatus); |
||
423 |
} |
||
424 |
|||
425 |
hppDout(info, "Amax found : " << alpha0); |
||
426 |
✓✓ | 13949 |
if (alpha0 <= aMaxFixed_) { |
427 |
6 |
alpha0 -= 0.01; // FIXME : hardcoded "robustness" value to avoid hitting |
|
428 |
// the bounds |
||
429 |
hppDout(info, "Amax after min : " << alpha0); |
||
430 |
✓✗✓✗ |
6 |
aMax = alpha0 * lastDirection_; |
431 |
✓✓ | 24 |
for (size_t i = 0; i < 3; ++i) |
432 |
✓✗✓✗ |
18 |
aMax[i] = fabs(aMax[i]); // aMax store the amplitude |
433 |
6 |
boundsUpToDate_ = false; |
|
434 |
} else { |
||
435 |
13943 |
boundsUpToDate_ = true; |
|
436 |
hppDout(notice, "Bounds already up to date, return the path"); |
||
437 |
} |
||
438 |
|||
439 |
✓✗✓✓ ✓✗ |
13949 |
if ((aMax[2] < aMaxFixed_Z_)) aMax[2] = aMaxFixed_Z_; |
440 |
hppDout(info, "Amax vector : " << aMax.transpose()); |
||
441 |
✓✗✓✗ |
13949 |
setAmax(aMax); |
442 |
hppDout(info, "Amax vector in SM : " << aMax_.transpose()); |
||
443 |
// setVmax(2*Vector3::Ones(3)); //FIXME: read it from somewhere ? |
||
444 |
|||
445 |
13949 |
return path; |
|
446 |
} |
||
447 |
|||
448 |
} // namespace rbprm |
||
449 |
} // namespace hpp |
| Generated by: GCOVR (Version 4.2) |