1 |
|
|
// Copyright (c) 2014, LAAS-CNRS |
2 |
|
|
// Authors: Steve Tonneau (steve.tonneau@laas.fr) |
3 |
|
|
// |
4 |
|
|
// This file is part of hpp-rbprm. |
5 |
|
|
// hpp-rbprm 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-rbprm 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-rbprm. If not, see <http://www.gnu.org/licenses/>. |
16 |
|
|
|
17 |
|
|
#include <hpp/core/config-projector.hh> |
18 |
|
|
#include <hpp/pinocchio/configuration.hh> |
19 |
|
|
#include <hpp/rbprm/contact_generation/algorithm.hh> |
20 |
|
|
#include <hpp/rbprm/contact_generation/reachability.hh> |
21 |
|
|
#include <hpp/rbprm/interpolation/interpolation-constraints.hh> |
22 |
|
|
#include <hpp/rbprm/interpolation/rbprm-path-interpolation.hh> |
23 |
|
|
#include <hpp/rbprm/projection/projection.hh> |
24 |
|
|
#include <hpp/rbprm/sampling/heuristic-tools.hh> |
25 |
|
|
#ifdef PROFILE |
26 |
|
|
#include "hpp/rbprm/rbprm-profiler.hh" |
27 |
|
|
#endif |
28 |
|
|
|
29 |
|
|
namespace hpp { |
30 |
|
|
namespace rbprm { |
31 |
|
|
namespace interpolation { |
32 |
|
|
|
33 |
|
|
RbPrmInterpolationPtr_t RbPrmInterpolation::create( |
34 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t robot, const hpp::rbprm::State& start, |
35 |
|
|
const hpp::rbprm::State& end, const core::PathVectorConstPtr_t path, |
36 |
|
|
const bool testReachability, const bool quasiStatic) { |
37 |
|
|
RbPrmInterpolation* rbprmDevice = new RbPrmInterpolation( |
38 |
|
|
path, robot, start, end, testReachability, quasiStatic); |
39 |
|
|
RbPrmInterpolationPtr_t res(rbprmDevice); |
40 |
|
|
res->init(res); |
41 |
|
|
return res; |
42 |
|
|
} |
43 |
|
|
|
44 |
|
|
RbPrmInterpolation::~RbPrmInterpolation() { |
45 |
|
|
// NOTHING |
46 |
|
|
} |
47 |
|
|
|
48 |
|
|
// ======================================================================== |
49 |
|
|
|
50 |
|
|
core::Configuration_t RbPrmInterpolation::configPosition( |
51 |
|
|
core::ConfigurationIn_t previous, const core::PathVectorConstPtr_t path, |
52 |
|
|
double i) { |
53 |
|
|
core::Configuration_t configuration = previous; |
54 |
|
|
size_t pathConfigSize = |
55 |
|
|
path->outputSize() - robot_->device_->extraConfigSpace().dimension(); |
56 |
|
|
core::Configuration_t configPosition(path->outputSize()); |
57 |
|
|
(*path)(configPosition, std::min(i, path->timeRange().second)); |
58 |
|
|
configuration.head(pathConfigSize) = configPosition.head(pathConfigSize); |
59 |
|
|
configuration.tail(robot_->device_->extraConfigSpace().dimension()) = |
60 |
|
|
configPosition.tail(robot_->device_->extraConfigSpace().dimension()); |
61 |
|
|
// configuration[2] = configuration[2] + 0.05; //walk static |
62 |
|
|
// configuration[2] = configuration[2] + 0.02; //stairs |
63 |
|
|
return configuration; |
64 |
|
|
} |
65 |
|
|
|
66 |
|
|
rbprm::T_StateFrame RbPrmInterpolation::Interpolate( |
67 |
|
|
const affMap_t& affordances, |
68 |
|
|
const std::map<std::string, std::vector<std::string> >& affFilters, |
69 |
|
|
const double timeStep, const double robustnessTreshold, |
70 |
|
|
const bool filterStates) { |
71 |
|
|
if (!path_) |
72 |
|
|
throw std::runtime_error( |
73 |
|
|
"Cannot interpolate; no path given to interpolator "); |
74 |
|
|
T_Configuration configs; |
75 |
|
|
const core::interval_t& range = path_->timeRange(); |
76 |
|
|
configs.push_back(start_.configuration_); |
77 |
|
|
hppDout(notice, |
78 |
|
|
"config start = " << pinocchio::displayConfig(start_.configuration_)); |
79 |
|
|
int j = 0; |
80 |
|
|
// for(double i = range.first + timeStep; i< range.second; i+= timeStep) |
81 |
|
|
for (double i = range.first + timeStep; i < range.second; |
82 |
|
|
i += timeStep, ++j) { |
83 |
|
|
configs.push_back(configPosition(configs.back(), path_, i)); |
84 |
|
|
// hppDout(notice,"config added = |
85 |
|
|
// "<<pinocchio::displayConfig(configs.back())); |
86 |
|
|
} |
87 |
|
|
configs.push_back(configPosition(configs.back(), path_, range.second)); |
88 |
|
|
return Interpolate(affordances, affFilters, configs, robustnessTreshold, |
89 |
|
|
timeStep, range.first, filterStates); |
90 |
|
|
} |
91 |
|
|
|
92 |
|
|
/// |
93 |
|
|
/// \brief replaceLimbContactForState Try to create a state with all the |
94 |
|
|
/// contacts of stateCurrent, except for LimbId which have the contact of |
95 |
|
|
/// stateGoal \param robot \param stateCurrent \param stateGoal \param limbId |
96 |
|
|
/// \return a projection report |
97 |
|
|
/// |
98 |
|
|
projection::ProjectionReport replaceLimbContactForState( |
99 |
|
|
RbPrmFullBodyPtr_t robot, State stateCurrent, State stateGoal, |
100 |
|
|
std::string limbId) { |
101 |
|
|
stateCurrent.RemoveContact(limbId); |
102 |
|
|
hppDout(notice, "Try to replace contact for limb : " << limbId); |
103 |
|
|
pinocchio::Configuration_t configuration = stateCurrent.configuration_; |
104 |
|
|
core::ConfigProjectorPtr_t proj = |
105 |
|
|
core::ConfigProjector::create(robot->device_, "proj", 1e-4, 1000); |
106 |
|
|
interpolation::addContactConstraints( |
107 |
|
|
robot, robot->device_, proj, stateCurrent, |
108 |
|
|
stateCurrent.fixedContacts(stateCurrent)); |
109 |
|
|
std::vector<bool> rotationMaskGoal; |
110 |
|
|
for (std::size_t i = 0; i < 3; ++i) { |
111 |
|
|
rotationMaskGoal.push_back(true); |
112 |
|
|
} |
113 |
|
|
return projection::projectEffector( |
114 |
|
|
proj, robot, limbId, robot->GetLimb(limbId), |
115 |
|
|
robot->GetCollisionValidation(), configuration, |
116 |
|
|
stateGoal.contactRotation_.at(limbId), rotationMaskGoal, |
117 |
|
|
stateGoal.contactPositions_.at(limbId), |
118 |
|
|
stateGoal.contactNormals_.at(limbId), stateCurrent); |
119 |
|
|
} |
120 |
|
|
|
121 |
|
|
// greedy algorithm to try the combination of order to move the legs until a |
122 |
|
|
// successfull one is found |
123 |
|
|
rbprm::T_StateFrame RbPrmInterpolation::addGoalConfigRec( |
124 |
|
|
const rbprm::T_StateFrame& states, |
125 |
|
|
const std::vector<std::string> variations) { |
126 |
|
|
hppDout(notice, "addGoalConfigRec, variations size : " << variations.size()); |
127 |
|
|
if (variations.size() == 1) { |
128 |
|
|
return states; |
129 |
|
|
} |
130 |
|
|
State midStateGoal(states.back().second); |
131 |
|
|
projection::ProjectionReport projReport; |
132 |
|
|
for (size_t id = 0; id < variations.size(); ++id) { |
133 |
|
|
std::string limbId(variations[id]); |
134 |
|
|
hppDout(notice, "addGoalConfigRec, try to move limb : " << limbId); |
135 |
|
|
projReport = replaceLimbContactForState(robot_, midStateGoal, end_, limbId); |
136 |
|
|
// If projection was successful, we add the new intermediate state to the |
137 |
|
|
// results |
138 |
|
|
if (projReport.success_) { |
139 |
|
|
hppDout(notice, "projection of contact successful"); |
140 |
|
|
hppDout(notice, "New intermediate state : " << pinocchio::displayConfig( |
141 |
|
|
projReport.result_.configuration_)); |
142 |
|
|
rbprm::T_StateFrame results(states.begin(), |
143 |
|
|
states.end()); // copy input states |
144 |
|
|
results.push_back(std::make_pair( |
145 |
|
|
states.back().first, |
146 |
|
|
projReport |
147 |
|
|
.result_)); // there will be 2 states at the same time index ... |
148 |
|
|
std::vector<std::string> remainingVariations(variations); |
149 |
|
|
remainingVariations.erase(remainingVariations.begin() + id); |
150 |
|
|
return addGoalConfigRec(results, remainingVariations); |
151 |
|
|
} else { |
152 |
|
|
hppDout(notice, "addGoalConfigRec projection failed for limb " << limbId); |
153 |
|
|
} |
154 |
|
|
} // all projections failed, return original list |
155 |
|
|
return states; |
156 |
|
|
} |
157 |
|
|
|
158 |
|
|
// Try to add the desired final configuration at the end of the current contact |
159 |
|
|
// sequence, while guaranteing that there is always only one contact variation |
160 |
|
|
// between states |
161 |
|
|
rbprm::T_StateFrame RbPrmInterpolation::addGoalConfig( |
162 |
|
|
const rbprm::T_StateFrame& states) { |
163 |
|
|
hppDout(notice, "AddGoalConfig, size of state list : " << states.size()); |
164 |
|
|
rbprm::T_StateFrame results(states.begin(), |
165 |
|
|
states.end()); // copy input states |
166 |
|
|
State lastState = states.back().second; |
167 |
|
|
|
168 |
|
|
std::vector<std::string> variationsGoal(lastState.contactVariations( |
169 |
|
|
end_)); // all limb that must be moved to reach the goal configuration |
170 |
|
|
if (variationsGoal.size() == 0) { |
171 |
|
|
hppDout(notice, "no contact variation, return input list"); |
172 |
|
|
return results; |
173 |
|
|
} else if (variationsGoal.size() == 1) { |
174 |
|
|
hppDout(notice, |
175 |
|
|
"specified goal and last state are already adjacent, add the goal " |
176 |
|
|
"state to the list"); |
177 |
|
|
results.push_back(std::make_pair( |
178 |
|
|
states.back().first, |
179 |
|
|
end_)); // there will be 2 states at the same time index ... |
180 |
|
|
return results; |
181 |
|
|
} |
182 |
|
|
const bool usePosturalTask = robot_->usePosturalTaskContactCreation(); |
183 |
|
|
robot_->usePosturalTaskContactCreation( |
184 |
|
|
false); // temporary disable this setting for projecting exactly on the |
185 |
|
|
// goal config |
186 |
|
|
// Last state and end are not adjacent, we keep lastState in the list and |
187 |
|
|
// produce intermediate states for each contact transition |
188 |
|
|
hppDout( |
189 |
|
|
notice, |
190 |
|
|
" Last state and end are not adjacent, try to add intermediate state"); |
191 |
|
|
// for each different contact, try to replace to it's position in end_ and add |
192 |
|
|
// an intermediate state |
193 |
|
|
results = addGoalConfigRec(states, variationsGoal); |
194 |
|
|
if ((results.back().second.contactVariations(end_)).size() == 1) { |
195 |
|
|
hppDout(notice, |
196 |
|
|
"LastState is adjacent to goal, add it to the list and return"); |
197 |
|
|
if (path_) |
198 |
|
|
results.push_back(std::make_pair(path_->timeRange().second, end_)); |
199 |
|
|
else |
200 |
|
|
results.push_back(std::make_pair(results.back().first, end_)); |
201 |
|
|
} else { |
202 |
|
|
hppDout(notice, |
203 |
|
|
"LastState is still not adjacent to goal, contact sequence do not " |
204 |
|
|
"end with the goal state"); |
205 |
|
|
} |
206 |
|
|
robot_->usePosturalTaskContactCreation( |
207 |
|
|
usePosturalTask); // put back previous setting |
208 |
|
|
return results; |
209 |
|
|
} |
210 |
|
|
|
211 |
|
|
/** |
212 |
|
|
* @brief loadPreviousConfiguration take the root and extra dof config from |
213 |
|
|
* config, and the whole body from previous |
214 |
|
|
* @param previous take the whole body configuration of the configuration |
215 |
|
|
* @param config take the root and extra dof configuration of this configuration |
216 |
|
|
* @return |
217 |
|
|
*/ |
218 |
|
|
core::Configuration_t loadPreviousConfiguration( |
219 |
|
|
const DevicePtr_t& device, const core::Configuration_t& previous, |
220 |
|
|
const core::Configuration_t& config) { |
221 |
|
|
core::Configuration_t res(previous); |
222 |
|
|
res.head<7>() = config.head<7>(); |
223 |
|
|
res.tail(device->extraConfigSpace().dimension()) = |
224 |
|
|
config.tail(device->extraConfigSpace().dimension()); |
225 |
|
|
return res; |
226 |
|
|
} |
227 |
|
|
|
228 |
|
|
rbprm::T_StateFrame RbPrmInterpolation::Interpolate( |
229 |
|
|
const affMap_t& affordances, |
230 |
|
|
const std::map<std::string, std::vector<std::string> >& affFilters, |
231 |
|
|
const hpp::rbprm::T_Configuration& configs, const double robustnessTreshold, |
232 |
|
|
const pinocchio::value_type timeStep, const pinocchio::value_type initValue, |
233 |
|
|
const bool filterStates) { |
234 |
|
|
hppDout( |
235 |
|
|
notice, |
236 |
|
|
"Begin interpolate in path-interpolation, number of configs to test : " |
237 |
|
|
<< configs.size()); |
238 |
|
|
int nbFailures = 0; |
239 |
|
|
size_t accIndex = robot_->device_->configSize() - |
240 |
|
|
robot_->device_->extraConfigSpace().dimension() + |
241 |
|
|
3; // index of the start of the acceleration vector (of |
242 |
|
|
// size 3), in the configuration vector |
243 |
|
|
hppDout(notice, "acceleration index : " << accIndex); |
244 |
|
|
pinocchio::value_type currentVal(initValue); |
245 |
|
|
rbprm::T_StateFrame states; |
246 |
|
|
states.push_back(std::make_pair(currentVal, this->start_)); |
247 |
|
|
Configuration_t lastConfig(this->start_.configuration_); |
248 |
|
|
CIT_Configuration lastIterator = configs.begin(); |
249 |
|
|
std::size_t nbRecontacts = 0; |
250 |
|
|
std::size_t repos = 0; |
251 |
|
|
bool allowFailure = true; |
252 |
|
|
Eigen::Vector3d dir, acc; |
253 |
|
|
acc = Eigen::Vector3d::Zero(); |
254 |
|
|
const PathConstPtr_t comPath = dynamic_pointer_cast<const core::Path>(path_); |
255 |
|
|
#ifdef PROFILE |
256 |
|
|
RbPrmProfiler& watch = getRbPrmProfiler(); |
257 |
|
|
watch.reset_all(); |
258 |
|
|
watch.start("complete generation"); |
259 |
|
|
#endif |
260 |
|
|
for (CIT_Configuration cit = configs.begin() + 1; cit != configs.end(); |
261 |
|
|
++cit, currentVal += timeStep) { |
262 |
|
|
const State& previous = states.back().second; |
263 |
|
|
core::Configuration_t configuration = |
264 |
|
|
loadPreviousConfiguration(robot_->device_, lastConfig, *cit); |
265 |
|
|
if (accIndex < (std::size_t)configuration.size()) { |
266 |
|
|
acc = configuration.segment<3>(accIndex); |
267 |
|
|
dir = configuration.segment<3>(accIndex - 3); |
268 |
|
|
} else |
269 |
|
|
dir = configuration.head<3>() - previous.configuration_.head<3>(); |
270 |
|
|
fcl::Vec3f direction(dir[0], dir[1], dir[2]); |
271 |
|
|
bool nonZero(false); |
272 |
|
|
direction.normalize(); |
273 |
|
|
if (!nonZero) direction = fcl::Vec3f(0, 0, 1.); |
274 |
|
|
// TODO Direction 6d |
275 |
|
|
hppDout(notice, |
276 |
|
|
"#call ComputeContact, looking for state " << states.size() - 1); |
277 |
|
|
hpp::rbprm::contact::ContactReport rep = contact::ComputeContacts( |
278 |
|
|
previous, robot_, configuration, affordances, affFilters, direction, |
279 |
|
|
robustnessTreshold, acc, comPath, currentVal, testReachability_, |
280 |
|
|
quasiStatic_); |
281 |
|
|
State& newState = rep.result_; |
282 |
|
|
|
283 |
|
|
const bool sameAsPrevious = |
284 |
|
|
rep.success_ && rep.contactMaintained_ && !rep.contactCreated_; |
285 |
|
|
const bool& multipleBreaks = rep.multipleBreaks_; |
286 |
|
|
const bool& respositioned = rep.repositionedInPlace_; |
287 |
|
|
hppDout(notice, |
288 |
|
|
"success : " << rep.success_ << " ; contacts maintained : " |
289 |
|
|
<< rep.contactMaintained_ |
290 |
|
|
<< " ; contact created : " << rep.contactCreated_ |
291 |
|
|
<< " ; sameAsPrevious : " << sameAsPrevious |
292 |
|
|
<< " ; multipleBreak : " << multipleBreaks |
293 |
|
|
<< " ; repositionned : " << respositioned |
294 |
|
|
<< " ; allowFailure : " << allowFailure |
295 |
|
|
<< " ; status :" << rep.status_); |
296 |
|
|
|
297 |
|
|
if (allowFailure && (!rep.success_ || rep.multipleBreaks_)) { |
298 |
|
|
++nbFailures; |
299 |
|
|
if (cit != configs.end() && (cit + 1) != configs.end()) { |
300 |
|
|
++cit; |
301 |
|
|
} |
302 |
|
|
currentVal += timeStep; |
303 |
|
|
if (nbFailures > 1) { |
304 |
|
|
/* |
305 |
|
|
std::ofstream fout; |
306 |
|
|
fout.open("/local/fernbac/bench_iros18/success/log_success.log",std::fstream::app); |
307 |
|
|
fout<<"failed."<<std::endl; |
308 |
|
|
fout.close(); |
309 |
|
|
*/ |
310 |
|
|
std::cout << "failed " << std::endl; |
311 |
|
|
#ifdef PROFILE |
312 |
|
|
watch.stop("complete generation"); |
313 |
|
|
watch.add_to_count("planner failed", 1); |
314 |
|
|
std::ofstream fout; |
315 |
|
|
fout.open("log.txt", std::fstream::out | std::fstream::app); |
316 |
|
|
std::ostream* fp = &fout; |
317 |
|
|
watch.report_count(*fp); |
318 |
|
|
fout.close(); |
319 |
|
|
#endif |
320 |
|
|
hppDout(notice, "Abort interpolate, too much fails"); |
321 |
|
|
return FilterStates(states, filterStates); |
322 |
|
|
// return states; |
323 |
|
|
} |
324 |
|
|
} |
325 |
|
|
if (multipleBreaks && !allowFailure) { |
326 |
|
|
++nbRecontacts; |
327 |
|
|
cit--; |
328 |
|
|
currentVal -= timeStep; |
329 |
|
|
} else if (!multipleBreaks && respositioned) { |
330 |
|
|
++nbRecontacts; |
331 |
|
|
cit--; |
332 |
|
|
currentVal -= timeStep; |
333 |
|
|
} else { |
334 |
|
|
nbRecontacts = 0; |
335 |
|
|
} |
336 |
|
|
if (respositioned) { |
337 |
|
|
++repos; |
338 |
|
|
if (repos > 20) { |
339 |
|
|
/*std::ofstream fout; |
340 |
|
|
fout.open("/local/fernbac/bench_iros18/success/log_success.log",std::fstream::app); |
341 |
|
|
fout<<"failed, too much repositionning"<<std::endl; |
342 |
|
|
fout.close(); |
343 |
|
|
*/ |
344 |
|
|
std::cout << "failed, too much repositionning" << std::endl; |
345 |
|
|
#ifdef PROFILE |
346 |
|
|
watch.stop("complete generation"); |
347 |
|
|
watch.add_to_count("planner failed", 1); |
348 |
|
|
std::ofstream fout; |
349 |
|
|
fout.open("log.txt", std::fstream::out | std::fstream::app); |
350 |
|
|
std::ostream* fp = &fout; |
351 |
|
|
watch.report_count(*fp); |
352 |
|
|
fout.close(); |
353 |
|
|
#endif |
354 |
|
|
hppDout(notice, "Abort interpolate, too much repositionning"); |
355 |
|
|
return FilterStates(states, filterStates); |
356 |
|
|
} |
357 |
|
|
cit = lastIterator; |
358 |
|
|
currentVal = states.back().first; |
359 |
|
|
} |
360 |
|
|
|
361 |
|
|
newState.nbContacts = newState.contactNormals_.size(); |
362 |
|
|
/* |
363 |
|
|
// code to add the last valid config for each state : |
364 |
|
|
if(sameAsPrevious){ |
365 |
|
|
states.pop_back(); |
366 |
|
|
} |
367 |
|
|
else{ |
368 |
|
|
hppDout(notice,"new state added at index "<<states.size()-1<<" conf = |
369 |
|
|
r(["<<pinocchio::displayConfig(states.back().second.configuration_)<<"])"); |
370 |
|
|
hppDout(notice,"First configuration for state "<<states.size()<<" : |
371 |
|
|
r(["<<pinocchio::displayConfig(newState.configuration_)<<"])"); |
372 |
|
|
} |
373 |
|
|
if(states.empty()){ // initial state, we keep the initial configuration but |
374 |
|
|
update the timing states.push_back(std::make_pair(currentVal, |
375 |
|
|
this->start_)); }else{ states.push_back(std::make_pair(currentVal, |
376 |
|
|
newState)); |
377 |
|
|
} |
378 |
|
|
*/ |
379 |
|
|
|
380 |
|
|
// code to add the first valid config of each states : |
381 |
|
|
if (!sameAsPrevious) { |
382 |
|
|
states.push_back(std::make_pair(currentVal, newState)); |
383 |
|
|
hppDout(notice, "new state added at index " |
384 |
|
|
<< states.size() - 1 << " conf = r([" |
385 |
|
|
<< pinocchio::displayConfig( |
386 |
|
|
states.back().second.configuration_) |
387 |
|
|
<< "])"); |
388 |
|
|
lastIterator = cit; |
389 |
|
|
} else { |
390 |
|
|
hppDout(notice, "Same as previous, new config = r([" |
391 |
|
|
<< pinocchio::displayConfig(newState.configuration_) |
392 |
|
|
<< "])"); |
393 |
|
|
} |
394 |
|
|
// allowFailure = nbRecontacts < robot_->GetLimbs().size(); |
395 |
|
|
allowFailure = nbRecontacts < 2; |
396 |
|
|
lastConfig = newState.configuration_; |
397 |
|
|
} |
398 |
|
|
// states.push_back(std::make_pair(this->path_->timeRange().second, |
399 |
|
|
// this->end_)); |
400 |
|
|
#ifdef PROFILE |
401 |
|
|
watch.add_to_count("planner succeeded", 1); |
402 |
|
|
watch.stop("complete generation"); |
403 |
|
|
/*std::ofstream fout; |
404 |
|
|
fout.open("log.txt", std::fstream::out | std::fstream::app); |
405 |
|
|
std::ostream* fp = &fout; |
406 |
|
|
watch.report_all_and_count(2,*fp); |
407 |
|
|
fout.close();*/ |
408 |
|
|
#endif |
409 |
|
|
/* |
410 |
|
|
std::ofstream fout; |
411 |
|
|
fout.open("/local/fernbac/bench_iros18/success/log_success.log",std::fstream::app); |
412 |
|
|
fout<<"Planner succeeded"<<std::endl; |
413 |
|
|
fout.close(); |
414 |
|
|
*/ |
415 |
|
|
hppDout(notice, "Interpolate finished, filter and add goal : "); |
416 |
|
|
if (states.size() > 1) { |
417 |
|
|
states = addGoalConfig(states); |
418 |
|
|
states = FilterStates(states, filterStates); |
419 |
|
|
} |
420 |
|
|
return states; |
421 |
|
|
} |
422 |
|
|
|
423 |
|
|
void RbPrmInterpolation::init(const RbPrmInterpolationWkPtr_t& weakPtr) { |
424 |
|
|
weakPtr_ = weakPtr; |
425 |
|
|
} |
426 |
|
|
|
427 |
|
|
RbPrmInterpolation::RbPrmInterpolation( |
428 |
|
|
const core::PathVectorConstPtr_t path, |
429 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t robot, const hpp::rbprm::State& start, |
430 |
|
|
const hpp::rbprm::State& end, const bool testReachability, |
431 |
|
|
const bool quasiStatic) |
432 |
|
|
: path_(path), |
433 |
|
|
start_(start), |
434 |
|
|
end_(end), |
435 |
|
|
testReachability_(testReachability), |
436 |
|
|
quasiStatic_(quasiStatic), |
437 |
|
|
robot_(robot) { |
438 |
|
|
// TODO |
439 |
|
|
} |
440 |
|
|
|
441 |
|
|
bool EqStringVec(const std::vector<std::string>& v1, |
442 |
|
|
const std::vector<std::string>& v2) { |
443 |
|
|
return (v1.size() == v2.size()) && |
444 |
|
|
std::equal(v1.begin(), v1.end(), v2.begin()); |
445 |
|
|
} |
446 |
|
|
|
447 |
|
|
StateFrame RbPrmInterpolation::findBestRepositionState( |
448 |
|
|
T_StateFrame candidates, std::vector<std::string> limbsNames) { |
449 |
|
|
StateFrame bestState = candidates.back(); |
450 |
|
|
double bestScore = -std::numeric_limits<double>::infinity(); |
451 |
|
|
double currentScore; |
452 |
|
|
fcl::Vec3f direction = candidates.back().second.configuration_.head<3>() - |
453 |
|
|
candidates.front().second.configuration_.head<3>(); |
454 |
|
|
// TODO : iterate over candidates and call the heuristic for limbsName. Then |
455 |
|
|
// return the state with the best score (average of scores if more than one |
456 |
|
|
// limbs) |
457 |
|
|
for (CIT_StateFrame sit = candidates.begin(); sit != candidates.end(); |
458 |
|
|
++sit) { |
459 |
|
|
currentScore = 0; |
460 |
|
|
for (std::vector<std::string>::const_iterator lit = limbsNames.begin(); |
461 |
|
|
lit != limbsNames.end(); ++lit) { |
462 |
|
|
RbPrmLimbPtr_t limb = robot_->GetLimbs().at(*lit); |
463 |
|
|
sampling::Sample sample(limb->limb_, limb->effector_, |
464 |
|
|
sit->second.configuration_, limb->offset_, |
465 |
|
|
limb->limbOffset_, 0); |
466 |
|
|
currentScore += limb->evaluate_(sample, direction, |
467 |
|
|
sit->second.contactNormals_.at(*lit), |
468 |
|
|
sampling::HeuristicParam()); |
469 |
|
|
} |
470 |
|
|
if (currentScore > bestScore) { |
471 |
|
|
bestScore = currentScore; |
472 |
|
|
bestState = *sit; |
473 |
|
|
hppDout(notice, "here, config = " << pinocchio::displayConfig( |
474 |
|
|
bestState.second.configuration_)); |
475 |
|
|
} |
476 |
|
|
} |
477 |
|
|
hppDout(notice, |
478 |
|
|
"Filtering, looking for best candidate : best score = " << bestScore); |
479 |
|
|
|
480 |
|
|
return bestState; |
481 |
|
|
} |
482 |
|
|
|
483 |
|
|
bool RbPrmInterpolation::testReachability(const State& s0, const State& s1) { |
484 |
|
|
if (testReachability_) { |
485 |
|
|
State state0(s0); |
486 |
|
|
State state1(s1); |
487 |
|
|
reachability::Result resReachability; |
488 |
|
|
if (quasiStatic_) { |
489 |
|
|
resReachability = reachability::isReachable(robot_, state0, state1); |
490 |
|
|
} else { |
491 |
|
|
resReachability = |
492 |
|
|
reachability::isReachableDynamic(robot_, state0, state1); |
493 |
|
|
} |
494 |
|
|
return resReachability.success(); |
495 |
|
|
} else { |
496 |
|
|
return true; |
497 |
|
|
} |
498 |
|
|
} |
499 |
|
|
|
500 |
|
|
void RbPrmInterpolation::FilterRepositioning(const CIT_StateFrame& from, |
501 |
|
|
const CIT_StateFrame to, |
502 |
|
|
T_StateFrame& res) { |
503 |
|
|
if (from == to) return; |
504 |
|
|
State current = (from)->second; |
505 |
|
|
State current_m1 = (from - 1)->second; |
506 |
|
|
State current_p1 = (from + 1)->second; |
507 |
|
|
if (EqStringVec(current.contactBreaks(current_m1), |
508 |
|
|
current_p1.contactBreaks(current_m1)) && |
509 |
|
|
EqStringVec(current.contactCreations(current_m1), |
510 |
|
|
current_p1.contactCreations(current)) && |
511 |
|
|
testReachability(current_m1, current_p1)) { |
512 |
|
|
if (from + 1 == to) return; |
513 |
|
|
// Check if there is others state with the same contacts, and only add the |
514 |
|
|
// one with the best score for the heuristic : |
515 |
|
|
/*bool reposition(true); |
516 |
|
|
size_t id = 2; |
517 |
|
|
T_StateFrame repositionnedStates; |
518 |
|
|
repositionnedStates.push_back(std::make_pair((from)->first, |
519 |
|
|
(from)->second)); |
520 |
|
|
repositionnedStates.push_back(std::make_pair((from+1)->first, |
521 |
|
|
(from+1)->second)); while(reposition && (from+id != to)){ |
522 |
|
|
current_m1=(from+id-1)->second; |
523 |
|
|
current=(from+id)->second; |
524 |
|
|
current_p1 = (from+id+1)->second; |
525 |
|
|
if(EqStringVec(current.contactBreaks(current_m1), |
526 |
|
|
current_p1.contactBreaks(current_m1)) && |
527 |
|
|
EqStringVec(current.contactCreations(current_m1), |
528 |
|
|
current_p1.contactCreations(current))){ |
529 |
|
|
repositionnedStates.push_back(std::make_pair((from+id)->first, |
530 |
|
|
(from+id)->second)); |
531 |
|
|
repositionnedStates.push_back(std::make_pair((from+id+1)->first, |
532 |
|
|
(from+id+1)->second)); id+=2; |
533 |
|
|
} |
534 |
|
|
else |
535 |
|
|
reposition = false; |
536 |
|
|
|
537 |
|
|
} |
538 |
|
|
hppDout(notice,"repositionned contacts found : number of states = |
539 |
|
|
"<<repositionnedStates.size()); |
540 |
|
|
// iterate over respoitionnedStates and find the one with the best score |
541 |
|
|
with the heuristic (for the limb that move) |
542 |
|
|
|
543 |
|
|
T_StateFrame repositionnedStates; |
544 |
|
|
repositionnedStates.push_back(std::make_pair((from)->first, |
545 |
|
|
(from)->second)); |
546 |
|
|
repositionnedStates.push_back(std::make_pair((from+1)->first, |
547 |
|
|
(from+1)->second)); std::vector<std::string> limbsNames = |
548 |
|
|
current.contactCreations(current_m1); StateFrame bestState = |
549 |
|
|
findBestRepositionState(repositionnedStates,limbsNames); |
550 |
|
|
*/ |
551 |
|
|
res.push_back(std::make_pair((from + 1)->first, (from + 1)->second)); |
552 |
|
|
FilterRepositioning(from + 2, to, res); |
553 |
|
|
} else { |
554 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
555 |
|
|
FilterRepositioning(from + 1, to, res); |
556 |
|
|
} |
557 |
|
|
} |
558 |
|
|
|
559 |
|
|
void RbPrmInterpolation::FilterBreakCreate(const CIT_StateFrame& from, |
560 |
|
|
const CIT_StateFrame to, |
561 |
|
|
T_StateFrame& res) { |
562 |
|
|
if (from == to) return; |
563 |
|
|
const State& current = (from)->second; |
564 |
|
|
const State& current_m1 = (from - 1)->second; |
565 |
|
|
const State& current_p1 = (from + 1)->second; |
566 |
|
|
if (current.contactCreations(current_m1).empty() && |
567 |
|
|
current_p1.contactBreaks(current).empty() && |
568 |
|
|
EqStringVec(current_p1.contactCreations(current), |
569 |
|
|
current.contactBreaks(current_m1)) && |
570 |
|
|
testReachability(current_m1, current_p1)) { |
571 |
|
|
if (from + 1 == to) return; |
572 |
|
|
res.push_back(std::make_pair((from + 1)->first, (from + 1)->second)); |
573 |
|
|
FilterBreakCreate(from + 2, to, res); |
574 |
|
|
} else { |
575 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
576 |
|
|
FilterBreakCreate(from + 1, to, res); |
577 |
|
|
} |
578 |
|
|
} |
579 |
|
|
|
580 |
|
|
T_StateFrame RbPrmInterpolation::FilterRepositioning( |
581 |
|
|
const T_StateFrame& originStates) { |
582 |
|
|
if (originStates.size() < 3) return originStates; |
583 |
|
|
T_StateFrame res; |
584 |
|
|
res.push_back(originStates.front()); |
585 |
|
|
FilterRepositioning(originStates.begin() + 1, originStates.end() - 1, res); |
586 |
|
|
res.push_back(originStates.back()); |
587 |
|
|
return res; |
588 |
|
|
} |
589 |
|
|
|
590 |
|
|
T_StateFrame RbPrmInterpolation::FilterBreakCreate( |
591 |
|
|
const T_StateFrame& originStates) { |
592 |
|
|
if (originStates.size() < 3) return originStates; |
593 |
|
|
T_StateFrame res; |
594 |
|
|
res.push_back(originStates.front()); |
595 |
|
|
FilterBreakCreate(originStates.begin() + 1, originStates.end() - 1, res); |
596 |
|
|
res.push_back(originStates.back()); |
597 |
|
|
return res; |
598 |
|
|
} |
599 |
|
|
|
600 |
|
|
T_StateFrame FilterObsolete(const T_StateFrame& originStates) { |
601 |
|
|
if (originStates.size() < 3) return originStates; |
602 |
|
|
T_StateFrame res; |
603 |
|
|
res.push_back(originStates.front()); |
604 |
|
|
CIT_StateFrame cit = originStates.begin(); |
605 |
|
|
for (CIT_StateFrame cit2 = originStates.begin() + 1; |
606 |
|
|
cit2 != originStates.end() - 1; ++cit, ++cit2) { |
607 |
|
|
const State& current = (cit2)->second; |
608 |
|
|
const State& current_m1 = (cit)->second; |
609 |
|
|
if ((current.configuration_ - current_m1.configuration_).norm() > |
610 |
|
|
std::numeric_limits<double>::epsilon() && |
611 |
|
|
!(current.contactBreaks(current_m1).empty() && |
612 |
|
|
current.contactCreations(current_m1).empty())) { |
613 |
|
|
res.push_back(std::make_pair(cit2->first, cit2->second)); |
614 |
|
|
} |
615 |
|
|
} |
616 |
|
|
res.push_back(originStates.back()); |
617 |
|
|
|
618 |
|
|
cit = res.begin(); |
619 |
|
|
std::size_t idx = 0; |
620 |
|
|
for (T_StateFrame::const_iterator cit2 = res.begin() + 1; |
621 |
|
|
cit2 != res.end() - 1; ++cit, ++cit2, ++idx) { |
622 |
|
|
const State prev = cit->second; |
623 |
|
|
const State next = cit2->second; |
624 |
|
|
std::vector<std::string> breaks = next.contactBreaks(prev); |
625 |
|
|
std::vector<std::string> creations = next.contactCreations(prev); |
626 |
|
|
if (breaks.size() > 1 || creations.size() > 1) { |
627 |
|
|
std::cout << "AFTER FILTER " << std::endl; |
628 |
|
|
std::cout << "\t REMOVING CONTACT " << breaks.size() << std::endl; |
629 |
|
|
for (std::vector<std::string>::const_iterator tf = breaks.begin(); |
630 |
|
|
tf != breaks.end(); ++tf) |
631 |
|
|
std::cout << "\t \t " << *tf << std::endl; |
632 |
|
|
std::cout << "\t CREATING CONTACT " << creations.size() << std::endl; |
633 |
|
|
for (std::vector<std::string>::const_iterator tf = creations.begin(); |
634 |
|
|
tf != creations.end(); ++tf) |
635 |
|
|
std::cout << "\t \t " << *tf << std::endl; |
636 |
|
|
|
637 |
|
|
std::cout << "END AFTERAFTER FILTER " << breaks.size() << std::endl; |
638 |
|
|
} |
639 |
|
|
} |
640 |
|
|
return res; |
641 |
|
|
} |
642 |
|
|
|
643 |
|
|
void RbPrmInterpolation::tryReplaceStates(const CIT_StateFrame& from, |
644 |
|
|
const CIT_StateFrame to, |
645 |
|
|
T_StateFrame& res) { |
646 |
|
|
if (from == to) { |
647 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
648 |
|
|
res.push_back(std::make_pair((from + 1)->first, (from + 1)->second)); |
649 |
|
|
return; |
650 |
|
|
} |
651 |
|
|
const State& ci0 = (from)->second; |
652 |
|
|
const State& ci1 = (from + 1)->second; |
653 |
|
|
const State& ci2 = (from + 2)->second; |
654 |
|
|
const State& ci3 = (from + 3)->second; |
655 |
|
|
|
656 |
|
|
hppDout(notice, "Try Replace State : "); |
657 |
|
|
if (ci3.contactCreations(ci2).size() == 1 && |
658 |
|
|
EqStringVec(ci1.contactBreaks(ci0), ci3.contactBreaks(ci2)) && |
659 |
|
|
EqStringVec(ci1.contactBreaks(ci0), ci1.contactCreations(ci0)) && |
660 |
|
|
EqStringVec(ci1.contactBreaks(ci0), ci3.contactCreations(ci2)) && |
661 |
|
|
!EqStringVec(ci1.contactBreaks(ci0), ci2.contactBreaks(ci1)) && |
662 |
|
|
!EqStringVec(ci1.contactCreations(ci0), ci2.contactCreations(ci1))) { |
663 |
|
|
hppDout(notice, "condition on contact OK"); |
664 |
|
|
// try to create a state s1_bis : s1 with the new contact in the same |
665 |
|
|
// position as in s3 |
666 |
|
|
// robot_->device_->currentConfiguration(ci3.configuration_); |
667 |
|
|
// robot_->device_->computeForwardKinematics(); |
668 |
|
|
State s1_bis(ci1); |
669 |
|
|
s1_bis.configuration_ = ci3.configuration_; |
670 |
|
|
// get contact information from state 3 : |
671 |
|
|
std::string contactCreate = ci3.contactCreations(ci2)[0]; |
672 |
|
|
hppDout(notice, "contact to change : " << contactCreate); |
673 |
|
|
fcl::Vec3f n = ci3.contactNormals_.at(contactCreate); |
674 |
|
|
fcl::Vec3f p = ci3.contactPositions_.at(contactCreate) + |
675 |
|
|
robot_->GetLimb(contactCreate)->offset_; |
676 |
|
|
fcl::Vec3f p1 = ci1.contactPositions_.at(contactCreate) + |
677 |
|
|
robot_->GetLimb(contactCreate)->offset_; |
678 |
|
|
hppDout(notice, "position : " << p); |
679 |
|
|
hppDout(notice, "normal : " << n); |
680 |
|
|
hppDout(notice, "difference with previous position : " << (p1 - p).norm()); |
681 |
|
|
// fcl::Matrix3f r = ci3.contactRotation_.at(contactCreate); |
682 |
|
|
projection::ProjectionReport rep = projection::projectStateToObstacle( |
683 |
|
|
robot_, contactCreate, robot_->GetLimb(contactCreate), s1_bis, n, p); |
684 |
|
|
hppDout(notice, "projection success : " << rep.success_); |
685 |
|
|
if (rep.success_) { |
686 |
|
|
rep = projection::projectToRootConfiguration(robot_, ci1.configuration_, |
687 |
|
|
rep.result_); |
688 |
|
|
} |
689 |
|
|
ValidationReportPtr_t rport( |
690 |
|
|
ValidationReportPtr_t(new CollisionValidationReport)); |
691 |
|
|
if ((p1 - p).norm() < 0.2 && rep.success_ && |
692 |
|
|
robot_->GetCollisionValidation()->validate(rep.result_.configuration_, |
693 |
|
|
rport) && |
694 |
|
|
testReachability(rep.result_, ci3)) { |
695 |
|
|
hppDout(notice, "projection is collision free !"); |
696 |
|
|
// success ! add s1_bis instead of s1, and skip s2 : |
697 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
698 |
|
|
res.push_back(std::make_pair((from + 1)->first, rep.result_)); |
699 |
|
|
if (from + 1 == to) return; |
700 |
|
|
if (from + 2 == to) { |
701 |
|
|
res.push_back(std::make_pair((from + 3)->first, (from + 3)->second)); |
702 |
|
|
return; |
703 |
|
|
} |
704 |
|
|
tryReplaceStates(from + 3, to, res); |
705 |
|
|
} else { |
706 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
707 |
|
|
tryReplaceStates(from + 1, to, res); |
708 |
|
|
} |
709 |
|
|
} else { |
710 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
711 |
|
|
tryReplaceStates(from + 1, to, res); |
712 |
|
|
} |
713 |
|
|
} |
714 |
|
|
|
715 |
|
|
T_StateFrame RbPrmInterpolation::tryReplaceStates( |
716 |
|
|
const T_StateFrame& originStates) { |
717 |
|
|
hppDout(notice, |
718 |
|
|
"Begin tryReplaceStates, size of list : " << originStates.size()); |
719 |
|
|
if (originStates.size() < 4) return originStates; |
720 |
|
|
T_StateFrame res; |
721 |
|
|
tryReplaceStates(originStates.begin(), originStates.end() - 3, res); |
722 |
|
|
res.push_back(originStates.back()); |
723 |
|
|
return res; |
724 |
|
|
} |
725 |
|
|
|
726 |
|
|
void RbPrmInterpolation::trySkipStates(const CIT_StateFrame& from, |
727 |
|
|
const CIT_StateFrame to, |
728 |
|
|
T_StateFrame& res) { |
729 |
|
|
if (from == to) { |
730 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
731 |
|
|
res.push_back(std::make_pair((from + 1)->first, (from + 1)->second)); |
732 |
|
|
return; |
733 |
|
|
} |
734 |
|
|
const State& ci0 = (from)->second; |
735 |
|
|
const State& ci1 = (from + 1)->second; |
736 |
|
|
const State& ci2 = (from + 2)->second; |
737 |
|
|
const State& ci3 = (from + 3)->second; |
738 |
|
|
|
739 |
|
|
hppDout(notice, "Try Skip State : "); |
740 |
|
|
if (ci2.contactCreations(ci1).size() == 1 && |
741 |
|
|
EqStringVec(ci1.contactBreaks(ci0), ci3.contactBreaks(ci2)) && |
742 |
|
|
EqStringVec(ci1.contactBreaks(ci0), ci1.contactCreations(ci0)) && |
743 |
|
|
EqStringVec(ci1.contactBreaks(ci0), ci3.contactCreations(ci2)) && |
744 |
|
|
!EqStringVec(ci1.contactBreaks(ci0), ci2.contactBreaks(ci1)) && |
745 |
|
|
!EqStringVec(ci1.contactCreations(ci0), ci2.contactCreations(ci1))) { |
746 |
|
|
hppDout(notice, "condition on contact OK"); |
747 |
|
|
// try to create a state s2_bis : s2 with the previous contact in the same |
748 |
|
|
// position as in s0 |
749 |
|
|
State s2_bis(ci2); |
750 |
|
|
s2_bis.configuration_ = ci0.configuration_; |
751 |
|
|
// get contact information from state 0 : |
752 |
|
|
std::string contactCreate = ci1.contactCreations(ci0)[0]; |
753 |
|
|
hppDout(notice, "contact to change : " << contactCreate); |
754 |
|
|
fcl::Vec3f n = ci0.contactNormals_.at(contactCreate); |
755 |
|
|
fcl::Vec3f p = ci0.contactPositions_.at(contactCreate) + |
756 |
|
|
robot_->GetLimb(contactCreate)->offset_; |
757 |
|
|
p -= n * 10e-3; // FIXME see 'epsilon' in |
758 |
|
|
// projection::computeProjectionMatrix, why is it added ? |
759 |
|
|
fcl::Vec3f p1 = ci1.contactPositions_.at(contactCreate) + |
760 |
|
|
robot_->GetLimb(contactCreate)->offset_; |
761 |
|
|
p1 -= ci1.contactNormals_.at(contactCreate) * 10e-3; |
762 |
|
|
hppDout(notice, "position : " << p); |
763 |
|
|
hppDout(notice, "normal : " << n); |
764 |
|
|
hppDout(notice, "difference with previous position : " << (p1 - p).norm()); |
765 |
|
|
// fcl::Matrix3f r = ci3.contactRotation_.at(contactCreate); |
766 |
|
|
projection::ProjectionReport rep = projection::projectStateToObstacle( |
767 |
|
|
robot_, contactCreate, robot_->GetLimb(contactCreate), s2_bis, n, p); |
768 |
|
|
hppDout(notice, "projection success : " << rep.success_); |
769 |
|
|
if (rep.success_) { |
770 |
|
|
rep = projection::projectToRootConfiguration(robot_, ci2.configuration_, |
771 |
|
|
rep.result_); |
772 |
|
|
} |
773 |
|
|
ValidationReportPtr_t rport( |
774 |
|
|
ValidationReportPtr_t(new CollisionValidationReport)); |
775 |
|
|
if ((p1 - p).norm() < 0.1 && rep.success_ && |
776 |
|
|
robot_->GetCollisionValidation()->validate(rep.result_.configuration_, |
777 |
|
|
rport) && |
778 |
|
|
testReachability(ci0, rep.result_) && |
779 |
|
|
testReachability(rep.result_, ci3)) { |
780 |
|
|
hppDout(notice, "projection is collision free !"); |
781 |
|
|
// success ! add s2_bis instead of s2, and skip s1 : |
782 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
783 |
|
|
res.push_back(std::make_pair((from + 2)->first, rep.result_)); |
784 |
|
|
if (from + 1 == to) return; |
785 |
|
|
if (from + 2 == to) { |
786 |
|
|
res.push_back(std::make_pair((from + 3)->first, (from + 3)->second)); |
787 |
|
|
return; |
788 |
|
|
} |
789 |
|
|
trySkipStates(from + 3, to, res); |
790 |
|
|
} else { |
791 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
792 |
|
|
trySkipStates(from + 1, to, res); |
793 |
|
|
} |
794 |
|
|
} else { |
795 |
|
|
res.push_back(std::make_pair(from->first, from->second)); |
796 |
|
|
trySkipStates(from + 1, to, res); |
797 |
|
|
} |
798 |
|
|
} |
799 |
|
|
|
800 |
|
|
T_StateFrame RbPrmInterpolation::trySkipStates( |
801 |
|
|
const T_StateFrame& originStates) { |
802 |
|
|
hppDout(notice, |
803 |
|
|
"Begin trySkipStates, size of list : " << originStates.size()); |
804 |
|
|
if (originStates.size() < 4) return originStates; |
805 |
|
|
T_StateFrame res; |
806 |
|
|
trySkipStates(originStates.begin(), originStates.end() - 3, res); |
807 |
|
|
res.push_back(originStates.back()); |
808 |
|
|
return res; |
809 |
|
|
} |
810 |
|
|
|
811 |
|
|
T_StateFrame RbPrmInterpolation::FilterStatesRec( |
812 |
|
|
const T_StateFrame& originStates) { |
813 |
|
|
hppDout(notice, "FilterStatesRec"); |
814 |
|
|
return trySkipStates(tryReplaceStates( |
815 |
|
|
FilterObsolete(FilterBreakCreate(FilterRepositioning(originStates))))); |
816 |
|
|
} |
817 |
|
|
|
818 |
|
|
T_StateFrame RbPrmInterpolation::FilterStates(const T_StateFrame& originStates, |
819 |
|
|
const bool deep) { |
820 |
|
|
// make sure they re ok |
821 |
|
|
hppDout(notice, "Begin filter states !"); |
822 |
|
|
hppDout(notice, "Number of state before filtering : " << originStates.size()); |
823 |
|
|
if (originStates.size() <= 2) { |
824 |
|
|
hppDout(notice, "Return original state list"); |
825 |
|
|
return originStates; |
826 |
|
|
} |
827 |
|
|
T_StateFrame::const_iterator cit = originStates.begin(); |
828 |
|
|
std::size_t idx = 0; |
829 |
|
|
for (T_StateFrame::const_iterator cit2 = originStates.begin() + 1; |
830 |
|
|
cit2 != originStates.end() - 1; ++cit, ++cit2, ++idx) { |
831 |
|
|
const State prev = cit->second; |
832 |
|
|
const State next = cit2->second; |
833 |
|
|
std::vector<std::string> breaks = next.contactBreaks(prev); |
834 |
|
|
std::vector<std::string> creations = next.contactCreations(prev); |
835 |
|
|
if (breaks.size() > 1 || creations.size() > 1) { |
836 |
|
|
hppDout(notice, "too many contact changes between the two states."); |
837 |
|
|
std::cout << "BEFORE FILTER " << std::endl; |
838 |
|
|
std::cout << "\t REMOVING CONTACT " << breaks.size() << std::endl; |
839 |
|
|
for (std::vector<std::string>::const_iterator tf = breaks.begin(); |
840 |
|
|
tf != breaks.end(); ++tf) |
841 |
|
|
std::cout << "\t \t " << *tf << std::endl; |
842 |
|
|
std::cout << "\t CREATING CONTACT " << creations.size() << std::endl; |
843 |
|
|
for (std::vector<std::string>::const_iterator tf = creations.begin(); |
844 |
|
|
tf != creations.end(); ++tf) |
845 |
|
|
std::cout << "\t \t " << *tf << std::endl; |
846 |
|
|
|
847 |
|
|
std::cout << "END BEFORE FILTER " << breaks.size() << std::endl; |
848 |
|
|
} |
849 |
|
|
} |
850 |
|
|
hppDout(notice, |
851 |
|
|
"End of checks for too many contact changes, filter redunbdant " |
852 |
|
|
"states, size of list : " |
853 |
|
|
<< originStates.size()); |
854 |
|
|
T_StateFrame res = originStates; |
855 |
|
|
if (deep) { |
856 |
|
|
std::size_t previousSize; |
857 |
|
|
do { |
858 |
|
|
hppDout(notice, |
859 |
|
|
"Begin iteration of filtering, size of res : " << res.size()); |
860 |
|
|
previousSize = res.size(); |
861 |
|
|
res = FilterStatesRec(res); |
862 |
|
|
hppDout(notice, |
863 |
|
|
"End iteration of filtering, size of res : " << res.size()); |
864 |
|
|
} while (res.size() != previousSize); |
865 |
|
|
hppDout(notice, "End of filtering, final size : " << res.size()); |
866 |
|
|
return res; |
867 |
|
|
} else { |
868 |
|
|
return FilterObsolete(originStates); |
869 |
|
|
} |
870 |
|
|
} |
871 |
|
|
} // namespace interpolation |
872 |
|
|
} // namespace rbprm |
873 |
|
|
} // namespace hpp |