1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017 CNRS |
3 |
|
|
// Authors: Steve Tonneau |
4 |
|
|
// |
5 |
|
|
// This file is part of hpp-rbprm |
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/pinocchio/configuration.hh> |
20 |
|
|
#include <hpp/rbprm/contact_generation/contact_generation.hh> |
21 |
|
|
#include <hpp/rbprm/contact_generation/reachability.hh> |
22 |
|
|
#include <hpp/rbprm/sampling/heuristic-tools.hh> |
23 |
|
|
#include <hpp/rbprm/stability/stability.hh> |
24 |
|
|
#include <hpp/rbprm/tools.hh> |
25 |
|
|
#include <pinocchio/spatial/se3.hpp> |
26 |
|
|
#ifdef PROFILE |
27 |
|
|
#include "hpp/rbprm/rbprm-profiler.hh" |
28 |
|
|
#endif |
29 |
|
|
#include <hpp/util/timer.hh> |
30 |
|
|
|
31 |
|
|
namespace hpp { |
32 |
|
|
namespace rbprm { |
33 |
|
|
namespace contact { |
34 |
|
|
|
35 |
|
|
ContactGenHelper::ContactGenHelper( |
36 |
|
|
RbPrmFullBodyPtr_t fb, const State& ps, |
37 |
|
|
pinocchio::ConfigurationIn_t configuration, |
38 |
|
|
const hpp::rbprm::affMap_t& affordances, |
39 |
|
|
const std::map<std::string, std::vector<std::string> >& affFilters, |
40 |
|
|
const double robustnessTreshold, const std::size_t maxContactBreaks, |
41 |
|
|
const std::size_t maxContactCreations, const bool checkStabilityMaintain, |
42 |
|
|
const bool checkStabilityGenerate, const fcl::Vec3f& direction, |
43 |
|
|
const fcl::Vec3f& acceleration, const bool contactIfFails, |
44 |
|
|
const bool stableForOneContact, const core::PathConstPtr_t& comPath, |
45 |
|
|
const double currentPathId) |
46 |
|
|
: fullBody_(fb), |
47 |
|
|
previousState_(ps), |
48 |
|
|
checkStabilityMaintain_(checkStabilityMaintain), |
49 |
|
|
contactIfFails_(contactIfFails), |
50 |
|
|
stableForOneContact_(stableForOneContact), |
51 |
|
|
acceleration_(acceleration), |
52 |
|
|
direction_(direction), |
53 |
|
|
robustnessTreshold_(robustnessTreshold), |
54 |
|
|
maxContactBreaks_(maxContactBreaks), |
55 |
|
|
maxContactCreations_(maxContactCreations), |
56 |
|
|
affordances_(affordances), |
57 |
|
|
affFilters_(affFilters), |
58 |
|
|
workingState_(previousState_), |
59 |
|
|
checkStabilityGenerate_(checkStabilityGenerate), |
60 |
|
|
comPath_(comPath), |
61 |
|
|
currentPathId_(currentPathId), |
62 |
|
|
quasiStatic_(false), |
63 |
|
|
testReachability_(true), |
64 |
|
|
maximiseContacts_(true), |
65 |
|
|
accept_unreachable_(false), |
66 |
|
|
tryQuasiStatic_(fb->staticStability()), |
67 |
|
|
reachabilityPointPerPhases_(0) { |
68 |
|
|
workingState_.configuration_ = configuration; |
69 |
|
|
workingState_.stable = false; |
70 |
|
|
} |
71 |
|
|
|
72 |
|
|
typedef std::vector<T_State> T_DepthState; |
73 |
|
|
|
74 |
|
|
bool push_if_new(T_State& states, const State currentState) { |
75 |
|
|
for (CIT_State cit = states.begin(); cit != states.end(); ++cit) { |
76 |
|
|
if (currentState.contactOrder_ == cit->contactOrder_) return false; |
77 |
|
|
} |
78 |
|
|
states.push_back(currentState); |
79 |
|
|
return true; |
80 |
|
|
} |
81 |
|
|
|
82 |
|
|
void reverse(std::queue<std::string>& queue) { |
83 |
|
|
assert(!queue.empty()); |
84 |
|
|
std::string temp(queue.front()); |
85 |
|
|
queue.pop(); |
86 |
|
|
if (!queue.empty()) reverse(queue); |
87 |
|
|
queue.push(temp); |
88 |
|
|
} |
89 |
|
|
|
90 |
|
|
void maintain_contacts_combinatorial_rec(const hpp::rbprm::State& currentState, |
91 |
|
|
const std::size_t depth, |
92 |
|
|
const std::size_t maxBrokenContacts, |
93 |
|
|
T_DepthState& res) { |
94 |
|
|
hppDout(notice, "maintain contact combinatorial recursif : "); |
95 |
|
|
if (!push_if_new(res[depth], currentState) || depth >= maxBrokenContacts) |
96 |
|
|
return; |
97 |
|
|
std::queue<std::string> contactOrder = currentState.contactOrder_; |
98 |
|
|
// TEST CODE : reverse order of the queue : |
99 |
|
|
hppDout(notice, "Size of contact queue : " << contactOrder.size()); |
100 |
|
|
// hppDout(notice,"Reverse :"); |
101 |
|
|
// reverse(contactOrder); |
102 |
|
|
// hppDout(notice,"Reverse done."); |
103 |
|
|
size_type size = contactOrder.size(); |
104 |
|
|
int i = 0; |
105 |
|
|
while (!contactOrder.empty() && size != i) { |
106 |
|
|
hpp::rbprm::State copyState = currentState; |
107 |
|
|
std::vector<std::string> fixed = currentState.fixedContacts(currentState); |
108 |
|
|
const std::string contactRemoved = contactOrder.front(); |
109 |
|
|
// if(! |
110 |
|
|
//((std::find(fixed.begin(), fixed.end(),std::string("hrp2_rleg_rom")) == |
111 |
|
|
// fixed.end() && contactRemoved == |
112 |
|
|
// std::string("hrp2_lleg_rom")) || (std::find(fixed.begin(), |
113 |
|
|
// fixed.end(),std::string("hrp2_lleg_rom")) == fixed.end() && |
114 |
|
|
// contactRemoved == std::string("hrp2_rleg_rom")))) |
115 |
|
|
{ |
116 |
|
|
hppDout(notice, "Try to remove contact " << contactRemoved); |
117 |
|
|
copyState.RemoveContact(contactRemoved); |
118 |
|
|
hppDout(notice, "Done."); |
119 |
|
|
maintain_contacts_combinatorial_rec(copyState, depth + 1, |
120 |
|
|
maxBrokenContacts, res); |
121 |
|
|
} |
122 |
|
|
/*else |
123 |
|
|
{ |
124 |
|
|
std::cout << "avoided both leg removed" << std::endl; |
125 |
|
|
contactOrder.push(contactRemoved); |
126 |
|
|
}*/ |
127 |
|
|
++i; |
128 |
|
|
contactOrder.pop(); |
129 |
|
|
} |
130 |
|
|
} |
131 |
|
|
|
132 |
|
|
Q_State flatten(const T_DepthState& depthstates) { |
133 |
|
|
Q_State res; |
134 |
|
|
for (T_DepthState::const_iterator cit = depthstates.begin(); |
135 |
|
|
cit != depthstates.end(); ++cit) { |
136 |
|
|
for (CIT_State ccit = cit->begin(); ccit != cit->end(); ++ccit) |
137 |
|
|
res.push(*ccit); |
138 |
|
|
} |
139 |
|
|
return res; |
140 |
|
|
} |
141 |
|
|
|
142 |
|
|
Q_State maintain_contacts_combinatorial(const hpp::rbprm::State& currentState, |
143 |
|
|
const std::size_t maxBrokenContacts) { |
144 |
|
|
T_DepthState res(maxBrokenContacts + 1); |
145 |
|
|
hppDout(notice, "maintain contact combinatorial : "); |
146 |
|
|
if (currentState.nbContacts == 1) { |
147 |
|
|
res[0].push_back(currentState); |
148 |
|
|
hppDout(warning, |
149 |
|
|
"Maintain_contacts_combinatorial called with a state with only 1 " |
150 |
|
|
"contact. FIXME, why does it happen ? "); |
151 |
|
|
} else { |
152 |
|
|
maintain_contacts_combinatorial_rec(currentState, 0, maxBrokenContacts, |
153 |
|
|
res); |
154 |
|
|
} |
155 |
|
|
return flatten(res); |
156 |
|
|
} |
157 |
|
|
|
158 |
|
|
using namespace projection; |
159 |
|
|
|
160 |
|
|
bool maintain_contacts_stability_rec( |
161 |
|
|
hpp::rbprm::RbPrmFullBodyPtr_t fullBody, |
162 |
|
|
pinocchio::ConfigurationIn_t targetRootConfiguration, Q_State& candidates, |
163 |
|
|
const std::size_t contactLength, const fcl::Vec3f& acceleration, |
164 |
|
|
const double robustness, ProjectionReport& currentRep) { |
165 |
|
|
if (stability::IsStable(fullBody, currentRep.result_, acceleration) > |
166 |
|
|
robustness) { |
167 |
|
|
currentRep.result_.stable = true; |
168 |
|
|
return true; |
169 |
|
|
} |
170 |
|
|
currentRep.result_.stable = false; |
171 |
|
|
if (!candidates.empty()) { |
172 |
|
|
State cState = candidates.front(); |
173 |
|
|
candidates.pop(); |
174 |
|
|
// removed more contacts, cannot be stable if previous state was not |
175 |
|
|
if (cState.contactOrder_.size() < contactLength) return false; |
176 |
|
|
ProjectionReport rep = |
177 |
|
|
projectToRootConfiguration(fullBody, targetRootConfiguration, cState); |
178 |
|
|
Q_State copy_candidates = candidates; |
179 |
|
|
if (maintain_contacts_stability_rec(fullBody, targetRootConfiguration, |
180 |
|
|
copy_candidates, contactLength, |
181 |
|
|
acceleration, robustness, rep)) { |
182 |
|
|
currentRep = rep; |
183 |
|
|
candidates = copy_candidates; |
184 |
|
|
return true; |
185 |
|
|
} |
186 |
|
|
} |
187 |
|
|
return false; |
188 |
|
|
} |
189 |
|
|
|
190 |
|
|
pinocchio::CollisionObjectPtr_t second( |
191 |
|
|
const std::pair<std::string, pinocchio::CollisionObjectPtr_t>& p) { |
192 |
|
|
return p.second; |
193 |
|
|
} |
194 |
|
|
|
195 |
|
|
std::vector<pinocchio::CollisionObjectPtr_t> getAffObjectsForLimb( |
196 |
|
|
const std::string& limb, const affMap_t& affordances, |
197 |
|
|
const std::map<std::string, std::vector<std::string> >& affFilters) { |
198 |
|
|
std::vector<pinocchio::CollisionObjectPtr_t> affs; |
199 |
|
|
std::vector<std::string> affTypes; |
200 |
|
|
bool settingFound = false; |
201 |
|
|
for (std::map<std::string, std::vector<std::string> >::const_iterator fIt = |
202 |
|
|
affFilters.begin(); |
203 |
|
|
fIt != affFilters.end(); ++fIt) { |
204 |
|
|
std::size_t found = fIt->first.find(limb); |
205 |
|
|
if (found != std::string::npos) { |
206 |
|
|
affTypes = fIt->second; |
207 |
|
|
settingFound = true; |
208 |
|
|
break; |
209 |
|
|
} |
210 |
|
|
} |
211 |
|
|
if (!settingFound) { |
212 |
|
|
// TODO: Keep warning or delete it? |
213 |
|
|
std::cout << "No affordance filter setting found for limb " << limb |
214 |
|
|
<< ". Has such filter been set?" << std::endl; |
215 |
|
|
// Use all AFF OBJECTS as default if no filter setting exists |
216 |
|
|
for (std::map<std::string, |
217 |
|
|
std::vector<std::pair<std::string, |
218 |
|
|
pinocchio::CollisionObjectPtr_t> > >:: |
219 |
|
|
const_iterator affordanceIt = affordances.map.begin(); |
220 |
|
|
affordanceIt != affordances.map.end(); ++affordanceIt) { |
221 |
|
|
std::transform(affordanceIt->second.begin(), affordanceIt->second.end(), |
222 |
|
|
std::back_inserter(affs), second); |
223 |
|
|
} |
224 |
|
|
} else { |
225 |
|
|
for (std::vector<std::string>::const_iterator affTypeIt = affTypes.begin(); |
226 |
|
|
affTypeIt != affTypes.end(); ++affTypeIt) { |
227 |
|
|
affMap_t::const_iterator affIt = affordances.map.find(*affTypeIt); |
228 |
|
|
std::transform(affIt->second.begin(), affIt->second.end(), |
229 |
|
|
std::back_inserter(affs), second); |
230 |
|
|
} |
231 |
|
|
} |
232 |
|
|
if (affs.empty()) |
233 |
|
|
throw std::runtime_error("No aff objects found for limb " + limb); |
234 |
|
|
return affs; |
235 |
|
|
} |
236 |
|
|
|
237 |
|
|
ProjectionReport maintain_contacts_stability(ContactGenHelper& contactGenHelper, |
238 |
|
|
ProjectionReport& currentRep) { |
239 |
|
|
const std::size_t contactLength(currentRep.result_.contactOrder_.size()); |
240 |
|
|
// contactGenHelper.candidates_.pop(); // TODO REMOVE (TEST) |
241 |
|
|
maintain_contacts_stability_rec( |
242 |
|
|
contactGenHelper.fullBody_, contactGenHelper.workingState_.configuration_, |
243 |
|
|
contactGenHelper.candidates_, contactLength, |
244 |
|
|
contactGenHelper.acceleration_, contactGenHelper.robustnessTreshold_, |
245 |
|
|
currentRep); |
246 |
|
|
hppDout(notice, |
247 |
|
|
"check stability maintain contact : " << currentRep.result_.stable); |
248 |
|
|
return currentRep; |
249 |
|
|
} |
250 |
|
|
|
251 |
|
|
std::vector<std::string> extractEffectorsName(const rbprm::T_Limb& limbs) { |
252 |
|
|
std::vector<std::string> res; |
253 |
|
|
for (rbprm::T_Limb::const_iterator cit = limbs.begin(); cit != limbs.end(); |
254 |
|
|
++cit) |
255 |
|
|
res.push_back(cit->first); |
256 |
|
|
return res; |
257 |
|
|
} |
258 |
|
|
|
259 |
|
|
std::vector<std::string> sortLimbs(const State& currentState, |
260 |
|
|
const std::vector<std::string>& freeLimbs) { |
261 |
|
|
std::vector<std::string> res1; |
262 |
|
|
std::vector<std::string> res2; |
263 |
|
|
// first add unused limbs |
264 |
|
|
// then undraw from contact order |
265 |
|
|
std::queue<std::string> order = currentState.contactOrder_; |
266 |
|
|
while (!order.empty()) { |
267 |
|
|
const std::string l = order.front(); |
268 |
|
|
order.pop(); |
269 |
|
|
if (std::find(freeLimbs.begin(), freeLimbs.end(), l) != freeLimbs.end()) |
270 |
|
|
tools::insertIfNew(res1, l); |
271 |
|
|
} |
272 |
|
|
for (std::vector<std::string>::const_iterator cit = freeLimbs.begin(); |
273 |
|
|
cit != freeLimbs.end(); ++cit) { |
274 |
|
|
if (std::find(res1.begin(), res1.end(), *cit) == res1.end()) { |
275 |
|
|
res2.push_back(*cit); |
276 |
|
|
} |
277 |
|
|
} |
278 |
|
|
res2.insert(res2.end(), res1.begin(), res1.end()); |
279 |
|
|
// res2.insert(res2.begin(), res1.begin(), res1.end()); |
280 |
|
|
return res2; |
281 |
|
|
} |
282 |
|
|
|
283 |
|
|
ProjectionReport genColFree(ContactGenHelper& contactGenHelper, |
284 |
|
|
ProjectionReport& currentRep) { |
285 |
|
|
ProjectionReport res = currentRep; |
286 |
|
|
// identify broken limbs and find collision free configurations for each one |
287 |
|
|
// of them. |
288 |
|
|
std::vector<std::string> effNames( |
289 |
|
|
extractEffectorsName(contactGenHelper.fullBody_->GetLimbs())); |
290 |
|
|
std::vector<std::string> freeLimbs = rbprm::freeEffectors( |
291 |
|
|
currentRep.result_, effNames.begin(), effNames.end()); |
292 |
|
|
freeLimbs = sortLimbs(contactGenHelper.workingState_, freeLimbs); |
293 |
|
|
for (std::vector<std::string>::const_iterator cit = freeLimbs.begin(); |
294 |
|
|
cit != freeLimbs.end() && res.success_; ++cit) { |
295 |
|
|
res = projection::setCollisionFree( |
296 |
|
|
contactGenHelper.fullBody_, |
297 |
|
|
contactGenHelper.fullBody_->GetLimbCollisionValidation().at(*cit), *cit, |
298 |
|
|
res.result_); |
299 |
|
|
hppDout(notice, "free limb in maintain contact : " << *cit); |
300 |
|
|
} |
301 |
|
|
// gen collision free configuration for the limbs not used for contact in last |
302 |
|
|
// : |
303 |
|
|
hppDout(notice, "size of val map = " << contactGenHelper.fullBody_ |
304 |
|
|
->GetLimbCollisionValidation() |
305 |
|
|
.size()); |
306 |
|
|
std::vector<std::string> effNotContactingNames(extractEffectorsName( |
307 |
|
|
contactGenHelper.fullBody_->GetNonContactingLimbs())); |
308 |
|
|
for (std::vector<std::string>::const_iterator cit = |
309 |
|
|
effNotContactingNames.begin(); |
310 |
|
|
cit != effNotContactingNames.end() && res.success_; ++cit) { |
311 |
|
|
hppDout(notice, "for limb name = " << *cit); |
312 |
|
|
res = projection::setCollisionFree( |
313 |
|
|
contactGenHelper.fullBody_, |
314 |
|
|
contactGenHelper.fullBody_->GetLimbCollisionValidation().at(*cit), *cit, |
315 |
|
|
res.result_); |
316 |
|
|
hppDout(notice, |
317 |
|
|
"free limb not used for contact in maintain contact : " << *cit); |
318 |
|
|
} |
319 |
|
|
return res; |
320 |
|
|
} |
321 |
|
|
|
322 |
|
|
void stringCombinatorialRec(std::vector<std::vector<std::string> >& res, |
323 |
|
|
const std::vector<std::string>& candidates, |
324 |
|
|
const std::size_t depth) { |
325 |
|
|
if (depth == 0) return; |
326 |
|
|
std::vector<std::vector<std::string> > newStates; |
327 |
|
|
for (std::vector<std::vector<std::string> >::iterator it = res.begin(); |
328 |
|
|
it != res.end(); ++it) { |
329 |
|
|
for (std::vector<std::string>::const_iterator canditates_it = |
330 |
|
|
candidates.begin(); |
331 |
|
|
canditates_it != candidates.end(); ++canditates_it) { |
332 |
|
|
std::vector<std::string> contacts = *it; |
333 |
|
|
if (tools::insertIfNew(contacts, *canditates_it)) { |
334 |
|
|
newStates.push_back(contacts); |
335 |
|
|
} |
336 |
|
|
} |
337 |
|
|
} |
338 |
|
|
stringCombinatorialRec(newStates, candidates, depth - 1); |
339 |
|
|
res.insert(res.end(), newStates.begin(), newStates.end()); |
340 |
|
|
} |
341 |
|
|
|
342 |
|
|
std::vector<std::vector<std::string> > stringCombinatorial( |
343 |
|
|
const std::vector<std::string>& candidates, const std::size_t maxDepth, |
344 |
|
|
const bool maximiseContacts = false) { |
345 |
|
|
std::vector<std::vector<std::string> > res; |
346 |
|
|
std::vector<std::string> tmp; |
347 |
|
|
res.push_back(tmp); |
348 |
|
|
stringCombinatorialRec(res, candidates, maxDepth); |
349 |
|
|
if (maximiseContacts) { |
350 |
|
|
// put first element (no contact creation) at the end |
351 |
|
|
std::rotate(res.begin(), res.begin() + 1, res.end()); |
352 |
|
|
} |
353 |
|
|
return res; |
354 |
|
|
} |
355 |
|
|
|
356 |
|
|
void gen_contacts_combinatorial_rec( |
357 |
|
|
const std::vector<std::string>& freeEffectors, const State& previous, |
358 |
|
|
T_ContactState& res, const std::size_t maxCreatedContacts, |
359 |
|
|
const bool maximiseContact = false) { |
360 |
|
|
std::vector<std::vector<std::string> > allNewStates = |
361 |
|
|
stringCombinatorial(freeEffectors, maxCreatedContacts, maximiseContact); |
362 |
|
|
for (std::vector<std::vector<std::string> >::const_iterator cit = |
363 |
|
|
allNewStates.begin(); |
364 |
|
|
cit != allNewStates.end(); ++cit) { |
365 |
|
|
ContactState contactState; |
366 |
|
|
contactState.first = previous; |
367 |
|
|
contactState.second = *cit; |
368 |
|
|
res.push(contactState); |
369 |
|
|
} |
370 |
|
|
} |
371 |
|
|
|
372 |
|
|
T_ContactState gen_contacts_combinatorial( |
373 |
|
|
const std::vector<std::string>& freeEffectors, const State& previous, |
374 |
|
|
const std::size_t maxCreatedContacts, const bool maximiseContacts) { |
375 |
|
|
T_ContactState res; |
376 |
|
|
; |
377 |
|
|
gen_contacts_combinatorial_rec(freeEffectors, previous, res, |
378 |
|
|
maxCreatedContacts, maximiseContacts); |
379 |
|
|
return res; |
380 |
|
|
} |
381 |
|
|
|
382 |
|
|
T_ContactState gen_contacts_combinatorial(ContactGenHelper& contactGenHelper) { |
383 |
|
|
State& cState = contactGenHelper.workingState_; |
384 |
|
|
std::vector<std::string> effNames( |
385 |
|
|
extractEffectorsName(contactGenHelper.fullBody_->GetLimbs())); |
386 |
|
|
const std::vector<std::string> freeLimbs = |
387 |
|
|
rbprm::freeEffectors(cState, effNames.begin(), effNames.end()); |
388 |
|
|
hppDout(notice, |
389 |
|
|
"in gen contact, number of free limbs : " << freeLimbs.size()); |
390 |
|
|
return gen_contacts_combinatorial(freeLimbs, cState, |
391 |
|
|
contactGenHelper.maxContactCreations_, |
392 |
|
|
contactGenHelper.maximiseContacts_); |
393 |
|
|
} |
394 |
|
|
|
395 |
|
|
ProjectionReport maintain_contacts(ContactGenHelper& contactGenHelper) { |
396 |
|
|
hppDout(notice, "Begin maintain contact"); |
397 |
|
|
ProjectionReport rep; |
398 |
|
|
hppDout(notice, "Get candidates"); |
399 |
|
|
if (contactGenHelper.candidates_.empty() && |
400 |
|
|
!contactGenHelper.workingState_.contacts_.empty()) { |
401 |
|
|
hppDout(notice, "candidate list empty, gen combinatorial : "); |
402 |
|
|
contactGenHelper.candidates_ = maintain_contacts_combinatorial( |
403 |
|
|
contactGenHelper.workingState_, contactGenHelper.maxContactBreaks_); |
404 |
|
|
} else if (!contactGenHelper.candidates_.empty()) { |
405 |
|
|
hppDout(notice, "candidate list already generated, take the next one."); |
406 |
|
|
// contactGenHelper.candidates_.pop(); // first candidate already treated. |
407 |
|
|
} |
408 |
|
|
hppDout(notice, "candidates OK"); |
409 |
|
|
Q_State& candidates = contactGenHelper.candidates_; |
410 |
|
|
while (!candidates.empty() && !rep.success_) { |
411 |
|
|
// retrieve latest state |
412 |
|
|
State cState = candidates.front(); |
413 |
|
|
candidates.pop(); |
414 |
|
|
hppDout(notice, "Project toRootConfiguration : "); |
415 |
|
|
rep = projectToRootConfiguration( |
416 |
|
|
contactGenHelper.fullBody_, |
417 |
|
|
contactGenHelper.workingState_.configuration_, cState); |
418 |
|
|
hppDout(notice, "maintain contacts, projection success : " |
419 |
|
|
<< rep.success_ << " for contacts : "); |
420 |
|
|
for (std::map<std::string, bool>::const_iterator cit = |
421 |
|
|
cState.contacts_.begin(); |
422 |
|
|
cit != cState.contacts_.end(); ++cit) { |
423 |
|
|
hppDout(notice, "limb : " << cit->first << ", contact = " << cit->second); |
424 |
|
|
} |
425 |
|
|
if (rep.success_) rep = genColFree(contactGenHelper, rep); |
426 |
|
|
if (rep.success_) { |
427 |
|
|
// collision validation |
428 |
|
|
hpp::core::ValidationReportPtr_t valRep( |
429 |
|
|
new hpp::core::CollisionValidationReport); |
430 |
|
|
rep.success_ = |
431 |
|
|
contactGenHelper.fullBody_->GetCollisionValidation()->validate( |
432 |
|
|
rep.result_.configuration_, valRep); |
433 |
|
|
hppDout(notice, |
434 |
|
|
"maintain contact collision for config : r([" |
435 |
|
|
<< pinocchio::displayConfig(rep.result_.configuration_) |
436 |
|
|
<< "])"); |
437 |
|
|
hppDout(notice, "valide : " << rep.success_); |
438 |
|
|
if (!rep.success_) { |
439 |
|
|
valRep->print(std::cout); |
440 |
|
|
std::cout << std::endl; |
441 |
|
|
hppDout(notice, "report = " << *valRep); |
442 |
|
|
} |
443 |
|
|
} |
444 |
|
|
if (rep.success_) { |
445 |
|
|
if (contactGenHelper.quasiStatic_ && contactGenHelper.testReachability_) { |
446 |
|
|
reachability::Result resReachability = reachability::isReachable( |
447 |
|
|
contactGenHelper.fullBody_, contactGenHelper.workingState_, |
448 |
|
|
rep.result_); |
449 |
|
|
rep.success_ = resReachability.success(); |
450 |
|
|
hppDout(notice, "Reachability test for maintain contact : succes = " |
451 |
|
|
<< rep.success_); |
452 |
|
|
rep.status_ = rep.success_ ? REACHABLE_CONTACT : STABLE_CONTACT; |
453 |
|
|
if (!rep.success_) hppDout(notice, "NOT REACHABLE in maintain contact"); |
454 |
|
|
} |
455 |
|
|
} |
456 |
|
|
} |
457 |
|
|
hppDout(notice, "maintain contact, check stability maintain : " |
458 |
|
|
<< contactGenHelper.checkStabilityMaintain_); |
459 |
|
|
if (rep.success_ && contactGenHelper.checkStabilityMaintain_) |
460 |
|
|
return maintain_contacts_stability(contactGenHelper, rep); |
461 |
|
|
return rep; |
462 |
|
|
} |
463 |
|
|
|
464 |
|
|
sampling::T_OctreeReport CollideOctree(const ContactGenHelper& contactGenHelper, |
465 |
|
|
const std::string& limbName, |
466 |
|
|
RbPrmLimbPtr_t limb, |
467 |
|
|
const sampling::heuristic evaluate, |
468 |
|
|
const sampling::HeuristicParam& params) { |
469 |
|
|
pinocchio::Transform3f transformpinocchio = |
470 |
|
|
limb->octreeRoot(); // get root transform from configuration |
471 |
|
|
fcl::Transform3f transform(transformpinocchio.rotation(), |
472 |
|
|
transformpinocchio.translation()); |
473 |
|
|
std::vector<pinocchio::CollisionObjectPtr_t> affordances = |
474 |
|
|
getAffObjectsForLimb(limbName, contactGenHelper.affordances_, |
475 |
|
|
contactGenHelper.affFilters_); |
476 |
|
|
|
477 |
|
|
// #pragma omp parallel for |
478 |
|
|
// request samples which collide with each of the collision objects |
479 |
|
|
sampling::heuristic eval = evaluate == 0 ? limb->evaluate_ : evaluate; |
480 |
|
|
std::size_t i(0); |
481 |
|
|
if (affordances.empty()) throw std::runtime_error("No aff objects found!!!"); |
482 |
|
|
|
483 |
|
|
std::vector<sampling::T_OctreeReport> reports(affordances.size()); |
484 |
|
|
for (std::vector<pinocchio::CollisionObjectPtr_t>::const_iterator oit = |
485 |
|
|
affordances.begin(); |
486 |
|
|
oit != affordances.end(); ++oit, ++i) { |
487 |
|
|
if (eval) |
488 |
|
|
sampling::GetCandidates(limb->sampleContainer_, transform, *oit, |
489 |
|
|
contactGenHelper.direction_, reports[i], params, |
490 |
|
|
eval); |
491 |
|
|
else |
492 |
|
|
sampling::GetCandidates(limb->sampleContainer_, transform, *oit, |
493 |
|
|
contactGenHelper.direction_, reports[i], params); |
494 |
|
|
} |
495 |
|
|
sampling::T_OctreeReport finalSet; |
496 |
|
|
// order samples according to EFORT |
497 |
|
|
for (std::vector<sampling::T_OctreeReport>::const_iterator cit = |
498 |
|
|
reports.begin(); |
499 |
|
|
cit != reports.end(); ++cit) { |
500 |
|
|
finalSet.insert(cit->begin(), cit->end()); |
501 |
|
|
} |
502 |
|
|
return finalSet; |
503 |
|
|
} |
504 |
|
|
|
505 |
|
|
hpp::rbprm::State findValidCandidate(const ContactGenHelper& contactGenHelper, |
506 |
|
|
const std::string& limbId, |
507 |
|
|
RbPrmLimbPtr_t limb, |
508 |
|
|
core::CollisionValidationPtr_t validation, |
509 |
|
|
bool& found_sample, bool& found_stable, |
510 |
|
|
bool& unstableContact, |
511 |
|
|
const sampling::HeuristicParam& params, |
512 |
|
|
const sampling::heuristic evaluate = 0) { |
513 |
|
|
State current(contactGenHelper.workingState_); |
514 |
|
|
current.stable = false; |
515 |
|
|
State intermediateState(current); // state before new contact creation |
516 |
|
|
State previous( |
517 |
|
|
contactGenHelper.previousState_); // previous state, before contact break |
518 |
|
|
sampling::T_OctreeReport finalSet = |
519 |
|
|
CollideOctree(contactGenHelper, limbId, limb, evaluate, params); |
520 |
|
|
core::Configuration_t moreRobust, bestUnreachable, configuration; |
521 |
|
|
configuration = current.configuration_; |
522 |
|
|
double maxRob = -std::numeric_limits<double>::max(); |
523 |
|
|
sampling::T_OctreeReport::const_iterator it = finalSet.begin(); |
524 |
|
|
fcl::Vec3f position, normal; |
525 |
|
|
fcl::Matrix3f rotation; |
526 |
|
|
ProjectionReport rep; |
527 |
|
|
double robustness; |
528 |
|
|
bool isReachable; |
529 |
|
|
int evaluatedCandidates = 0; |
530 |
|
|
hppDout(notice, "in findValidCandidate for limb : " << limbId); |
531 |
|
|
hppDout(notice, "number of candidate : " << finalSet.size()); |
532 |
|
|
for (; !found_sample && it != finalSet.end(); ++it) { |
533 |
|
|
hppStartBenchmark(EVALUATE_CONTACT_CANDIDATE); |
534 |
|
|
evaluatedCandidates++; |
535 |
|
|
const sampling::OctreeReport& bestReport = *it; |
536 |
|
|
hppDout(notice, "heuristic value = " << it->value_); |
537 |
|
|
core::Configuration_t conf_before(configuration); |
538 |
|
|
sampling::Load(*bestReport.sample_, conf_before); |
539 |
|
|
hppDout(notice, "config before projection : r([" |
540 |
|
|
<< pinocchio::displayConfig(conf_before) << "])"); |
541 |
|
|
hppStartBenchmark(PROJECTION_CONTACT); |
542 |
|
|
/*ProjectionReport */ rep = |
543 |
|
|
projectSampleToObstacle(contactGenHelper.fullBody_, limbId, limb, |
544 |
|
|
bestReport, validation, configuration, current); |
545 |
|
|
hppStopBenchmark(PROJECTION_CONTACT); |
546 |
|
|
hppDisplayBenchmark(PROJECTION_CONTACT); |
547 |
|
|
hppDout(notice, |
548 |
|
|
"config : r([" << pinocchio::displayConfig(configuration) << "])"); |
549 |
|
|
hppDout(notice, "projection to obstacle success = " << rep.success_); |
550 |
|
|
if (rep.success_) { |
551 |
|
|
hppDout(notice, "CheckStabilityGenerate : " |
552 |
|
|
<< contactGenHelper.checkStabilityGenerate_); |
553 |
|
|
hppDout(notice, |
554 |
|
|
"StableOneContact" << contactGenHelper.stableForOneContact_); |
555 |
|
|
hppDout(notice, "Num contact : " << rep.result_.nbContacts); |
556 |
|
|
if (!contactGenHelper.checkStabilityGenerate_ || |
557 |
|
|
(rep.result_.nbContacts == 1 && |
558 |
|
|
!contactGenHelper |
559 |
|
|
.stableForOneContact_)) { // only check projection, success ! |
560 |
|
|
hppDout(notice, |
561 |
|
|
"Don't check stability : one contact or not the last contact"); |
562 |
|
|
position = rep.result_.contactPositions_.at(limbId); |
563 |
|
|
rotation = rep.result_.contactRotation_.at(limbId); |
564 |
|
|
normal = rep.result_.contactNormals_.at(limbId); |
565 |
|
|
found_sample = true; |
566 |
|
|
} else { // check stability and reachability |
567 |
|
|
hppStartBenchmark(STABILITY_CONTACT); |
568 |
|
|
robustness = |
569 |
|
|
stability::IsStable(contactGenHelper.fullBody_, rep.result_, |
570 |
|
|
contactGenHelper.acceleration_); |
571 |
|
|
hppStopBenchmark(STABILITY_CONTACT); |
572 |
|
|
hppDisplayBenchmark(STABILITY_CONTACT); |
573 |
|
|
hppDout(notice, "stability rob = " << robustness); |
574 |
|
|
if (robustness >= contactGenHelper.robustnessTreshold_) { |
575 |
|
|
hppDout(notice, "stability OK, test reachability : "); |
576 |
|
|
if (contactGenHelper.testReachability_) { |
577 |
|
|
reachability::Result resReachability; |
578 |
|
|
if (contactGenHelper.quasiStatic_) { |
579 |
|
|
resReachability = reachability::isReachable( |
580 |
|
|
contactGenHelper.fullBody_, intermediateState, rep.result_); |
581 |
|
|
// resReachability = |
582 |
|
|
// reachability::isReachable(contactGenHelper.fullBody_,previous,rep.result_); |
583 |
|
|
// // TODO : use a parameter to choose between both cases |
584 |
|
|
} else { |
585 |
|
|
hppStartBenchmark(REACHABILITY_CONTACT); |
586 |
|
|
resReachability = reachability::isReachableDynamic( |
587 |
|
|
contactGenHelper.fullBody_, previous, rep.result_, |
588 |
|
|
contactGenHelper.tryQuasiStatic_, std::vector<double>(), |
589 |
|
|
contactGenHelper.reachabilityPointPerPhases_); |
590 |
|
|
hppStopBenchmark(REACHABILITY_CONTACT); |
591 |
|
|
hppDisplayBenchmark(REACHABILITY_CONTACT); |
592 |
|
|
} |
593 |
|
|
isReachable = resReachability.success(); |
594 |
|
|
} else { |
595 |
|
|
isReachable = true; |
596 |
|
|
} |
597 |
|
|
if (isReachable) { // reachable |
598 |
|
|
position = rep.result_.contactPositions_.at(limbId); |
599 |
|
|
rotation = rep.result_.contactRotation_.at(limbId); |
600 |
|
|
normal = rep.result_.contactNormals_.at(limbId); |
601 |
|
|
found_sample = true; |
602 |
|
|
// TODO : save path (if not quasistatic)?? |
603 |
|
|
} else { |
604 |
|
|
hppDout(notice, "NOT REACHABLE"); |
605 |
|
|
if (!found_stable) { |
606 |
|
|
maxRob = std::max(robustness, maxRob); |
607 |
|
|
// if no reachable state are found, we keep the first stable |
608 |
|
|
// configuration found (ie. the one with the best heuristic score) |
609 |
|
|
bestUnreachable = configuration; |
610 |
|
|
found_stable = true; |
611 |
|
|
position = rep.result_.contactPositions_.at(limbId); |
612 |
|
|
rotation = rep.result_.contactRotation_.at(limbId); |
613 |
|
|
normal = rep.result_.contactNormals_.at(limbId); |
614 |
|
|
} |
615 |
|
|
/* // DEBUGING PURPOSE : call again evaluate on the sample found |
616 |
|
|
hppDout(notice,"found sample, evaluate : "); // remove |
617 |
|
|
Eigen::Vector3d eDir(contactGenHelper.direction_[0], |
618 |
|
|
contactGenHelper.direction_[1], contactGenHelper.direction_[2]); |
619 |
|
|
// remove eDir.normalize(); // remove hppDout(notice,"sample |
620 |
|
|
= |
621 |
|
|
"<<(*(bestReport.sample_)).startRank_); // remove |
622 |
|
|
sampling::heuristic eval = evaluate == 0 ? limb->evaluate_ : |
623 |
|
|
evaluate;// remove |
624 |
|
|
(*eval)(*(bestReport.sample_), eDir, normal, params); // TODO : |
625 |
|
|
comment when not debugging core::Configuration_t confBefore= |
626 |
|
|
current.configuration_; // remove |
627 |
|
|
sampling::Load(*(bestReport.sample_),confBefore); //remove |
628 |
|
|
hppDout(notice,"config before projection : |
629 |
|
|
r(["<<pinocchio::displayConfig(confBefore)<<"])"); //remove |
630 |
|
|
*/ |
631 |
|
|
} |
632 |
|
|
} |
633 |
|
|
// if no stable candidate is found, select best contact |
634 |
|
|
// anyway |
635 |
|
|
else if ((robustness > maxRob) && contactGenHelper.contactIfFails_) { |
636 |
|
|
hppDout(notice, "unstable contact, but the most robust yet."); |
637 |
|
|
moreRobust = configuration; |
638 |
|
|
maxRob = robustness; |
639 |
|
|
position = rep.result_.contactPositions_.at(limbId); |
640 |
|
|
rotation = rep.result_.contactRotation_.at(limbId); |
641 |
|
|
normal = rep.result_.contactNormals_.at(limbId); |
642 |
|
|
unstableContact = true; |
643 |
|
|
} |
644 |
|
|
} |
645 |
|
|
} |
646 |
|
|
hppStopBenchmark(EVALUATE_CONTACT_CANDIDATE); |
647 |
|
|
hppDisplayBenchmark(EVALUATE_CONTACT_CANDIDATE); |
648 |
|
|
} |
649 |
|
|
if (found_sample || found_stable || unstableContact) { |
650 |
|
|
current.contacts_[limbId] = true; |
651 |
|
|
current.contactNormals_[limbId] = normal; |
652 |
|
|
current.contactPositions_[limbId] = position; |
653 |
|
|
current.contactRotation_[limbId] = rotation; |
654 |
|
|
current.contactOrder_.push(limbId); |
655 |
|
|
hppDout(notice, "In find valid candidate, for limb : " << limbId); |
656 |
|
|
hppDout(notice, "position : " << position); |
657 |
|
|
hppDout(notice, "normal : " << normal); |
658 |
|
|
} |
659 |
|
|
if (found_sample) { |
660 |
|
|
hppDout(notice, "Valid sample found, stable and reachable"); |
661 |
|
|
current.configuration_ = configuration; |
662 |
|
|
current.stable = true; |
663 |
|
|
} else if (found_stable) { |
664 |
|
|
hppDout(notice, "Valid sample found, stable BUT NOT REACHABLE"); |
665 |
|
|
current.configuration_ = bestUnreachable; |
666 |
|
|
current.stable = true; |
667 |
|
|
} else if (unstableContact) { |
668 |
|
|
hppDout(notice, "No valid sample found, take an unstable one"); |
669 |
|
|
current.configuration_ = moreRobust; |
670 |
|
|
current.stable = false; |
671 |
|
|
} |
672 |
|
|
/* |
673 |
|
|
std::ofstream fout; |
674 |
|
|
fout.open("/local/fernbac/bench_iros18/bench_contactGeneration/candidates.log", |
675 |
|
|
std::fstream::out | std::fstream::app); fout<<"evaluatedCandidates |
676 |
|
|
"<<evaluatedCandidates<<std::endl; fout.close(); |
677 |
|
|
*/ |
678 |
|
|
if (contactGenHelper.fullBody_->usePosturalTaskContactCreation() && |
679 |
|
|
!found_sample) { |
680 |
|
|
// as the contact generator is not complete when |
681 |
|
|
// usePosturalTaskContactCreation==True, if it fail we retry without this |
682 |
|
|
// option |
683 |
|
|
contactGenHelper.fullBody_->usePosturalTaskContactCreation(false); |
684 |
|
|
current = findValidCandidate(contactGenHelper, limbId, limb, validation, |
685 |
|
|
found_sample, found_stable, unstableContact, |
686 |
|
|
params, evaluate); |
687 |
|
|
contactGenHelper.fullBody_->usePosturalTaskContactCreation(true); |
688 |
|
|
} |
689 |
|
|
|
690 |
|
|
return current; |
691 |
|
|
} |
692 |
|
|
|
693 |
|
|
ProjectionReport generate_contact(const ContactGenHelper& contactGenHelper, |
694 |
|
|
const std::string& limbName, |
695 |
|
|
sampling::HeuristicParam& params, |
696 |
|
|
const sampling::heuristic evaluate) { |
697 |
|
|
ProjectionReport rep; |
698 |
|
|
hppDout(notice, "in generate_contact, check stability = " |
699 |
|
|
<< contactGenHelper.checkStabilityGenerate_); |
700 |
|
|
RbPrmLimbPtr_t limb = contactGenHelper.fullBody_->GetLimbs().at(limbName); |
701 |
|
|
core::CollisionValidationPtr_t validation = |
702 |
|
|
contactGenHelper.fullBody_->GetLimbCollisionValidation().at(limbName); |
703 |
|
|
limb->limb_->robot()->currentConfiguration( |
704 |
|
|
contactGenHelper.workingState_.configuration_); |
705 |
|
|
limb->limb_->robot()->computeForwardKinematics(); |
706 |
|
|
// pick first sample which is collision free |
707 |
|
|
bool found_sample(false); |
708 |
|
|
bool found_stable(false); |
709 |
|
|
bool unstableContact( |
710 |
|
|
false); // set to true in case no stable contact is found |
711 |
|
|
params.comPath_ = contactGenHelper.comPath_; |
712 |
|
|
params.currentPathId_ = contactGenHelper.currentPathId_; |
713 |
|
|
params.limbReferenceOffset_ = limb->effectorReferencePosition_; |
714 |
|
|
hppDout(notice, |
715 |
|
|
"findValidCandidate for effector : " << limb->effector_.name()); |
716 |
|
|
rep.result_ = findValidCandidate(contactGenHelper, limbName, limb, validation, |
717 |
|
|
found_sample, found_stable, unstableContact, |
718 |
|
|
params, evaluate); |
719 |
|
|
|
720 |
|
|
if (found_sample) { |
721 |
|
|
hppDout(notice, "found reachable sample : " << pinocchio::displayConfig( |
722 |
|
|
rep.result_.configuration_)); |
723 |
|
|
rep.status_ = REACHABLE_CONTACT; |
724 |
|
|
rep.success_ = true; |
725 |
|
|
#ifdef PROFILE |
726 |
|
|
RbPrmProfiler& watch = getRbPrmProfiler(); |
727 |
|
|
watch.add_to_count("reachable contact", 1); |
728 |
|
|
#endif |
729 |
|
|
} else if (found_stable) { |
730 |
|
|
hppDout(notice, "NOT REACHABLE in generate_contact"); |
731 |
|
|
hppDout(notice, "found stable sample : " << pinocchio::displayConfig( |
732 |
|
|
rep.result_.configuration_)); |
733 |
|
|
rep.status_ = STABLE_CONTACT; |
734 |
|
|
if (contactGenHelper.accept_unreachable_) |
735 |
|
|
rep.success_ = true; |
736 |
|
|
else |
737 |
|
|
rep.success_ = false; |
738 |
|
|
#ifdef PROFILE |
739 |
|
|
RbPrmProfiler& watch = getRbPrmProfiler(); |
740 |
|
|
watch.add_to_count("unreachable stable contact", 1); |
741 |
|
|
#endif |
742 |
|
|
} else if (unstableContact) { |
743 |
|
|
hppDout(notice, "unstable contact"); |
744 |
|
|
rep.status_ = UNSTABLE_CONTACT; |
745 |
|
|
rep.success_ = !contactGenHelper.checkStabilityGenerate_; |
746 |
|
|
#ifdef PROFILE |
747 |
|
|
RbPrmProfiler& watch = getRbPrmProfiler(); |
748 |
|
|
watch.add_to_count("unstable contact", 1); |
749 |
|
|
#endif |
750 |
|
|
} else { |
751 |
|
|
hppDout(notice, "set Collision Free"); |
752 |
|
|
rep = setCollisionFree(contactGenHelper.fullBody_, validation, limbName, |
753 |
|
|
rep.result_); |
754 |
|
|
rep.status_ = NO_CONTACT; |
755 |
|
|
rep.success_ = false; |
756 |
|
|
#ifdef PROFILE |
757 |
|
|
RbPrmProfiler& watch = getRbPrmProfiler(); |
758 |
|
|
watch.add_to_count("no contact", 1); |
759 |
|
|
#endif |
760 |
|
|
} |
761 |
|
|
return rep; |
762 |
|
|
} |
763 |
|
|
|
764 |
|
|
ProjectionReport gen_contacts(ContactGenHelper& contactGenHelper) { |
765 |
|
|
ProjectionReport rep; |
766 |
|
|
T_ContactState candidates = gen_contacts_combinatorial(contactGenHelper); |
767 |
|
|
hppDout(notice, "gen_contact candidates size : " << candidates.size()); |
768 |
|
|
hppDout(notice, "working state config : r([" |
769 |
|
|
<< pinocchio::displayConfig( |
770 |
|
|
contactGenHelper.workingState_.configuration_) |
771 |
|
|
<< "])"); |
772 |
|
|
bool checkStability(contactGenHelper.checkStabilityGenerate_); |
773 |
|
|
while (!candidates.empty() && !rep.success_) { |
774 |
|
|
// retrieve latest state |
775 |
|
|
ContactState cState = candidates.front(); |
776 |
|
|
candidates.pop(); |
777 |
|
|
hppDout(notice, "generateContact, number of limbs to test : " |
778 |
|
|
<< cState.second.size()); |
779 |
|
|
if (cState.second.empty() && checkStability && |
780 |
|
|
(contactGenHelper.workingState_.nbContacts >= 2 || |
781 |
|
|
contactGenHelper.stableForOneContact_)) { |
782 |
|
|
hppDout(notice, |
783 |
|
|
"List of free limbs empty in gen_contact, check stability for " |
784 |
|
|
"workingState with contact maintained"); |
785 |
|
|
double robustness = stability::IsStable(contactGenHelper.fullBody_, |
786 |
|
|
contactGenHelper.workingState_, |
787 |
|
|
contactGenHelper.acceleration_); |
788 |
|
|
hppDout(notice, "stability rob = " << robustness); |
789 |
|
|
if (robustness >= contactGenHelper.robustnessTreshold_) { |
790 |
|
|
contactGenHelper.workingState_.stable = true; |
791 |
|
|
} |
792 |
|
|
} |
793 |
|
|
if (cState.second.empty() && |
794 |
|
|
(contactGenHelper.workingState_.stable || !checkStability)) { |
795 |
|
|
if (contactGenHelper.workingState_.nbContacts >= 2) { |
796 |
|
|
hppDout(notice, "working state stable, contact maintained OK."); |
797 |
|
|
rep.result_ = contactGenHelper.workingState_; |
798 |
|
|
rep.status_ = STABLE_CONTACT; |
799 |
|
|
rep.success_ = true; |
800 |
|
|
return rep; |
801 |
|
|
} |
802 |
|
|
} |
803 |
|
|
contactGenHelper.checkStabilityGenerate_ = |
804 |
|
|
false; // stability not mandatory before last contact is created |
805 |
|
|
for (std::vector<std::string>::const_iterator cit = cState.second.begin(); |
806 |
|
|
cit != cState.second.end(); ++cit) { |
807 |
|
|
hppDout(notice, "Try to generate contact for limb : " << *cit); |
808 |
|
|
sampling::HeuristicParam params; |
809 |
|
|
params.contactPositions_ = cState.first.contactPositions_; |
810 |
|
|
contactGenHelper.fullBody_->device_->currentConfiguration( |
811 |
|
|
cState.first.configuration_); |
812 |
|
|
contactGenHelper.fullBody_->device_->computeForwardKinematics(); |
813 |
|
|
params.comPosition_ = |
814 |
|
|
contactGenHelper.fullBody_->device_->positionCenterOfMass(); |
815 |
|
|
size_type cfgSize(cState.first.configuration_.rows()); |
816 |
|
|
params.comSpeed_ = fcl::Vec3f(cState.first.configuration_[cfgSize - 6], |
817 |
|
|
cState.first.configuration_[cfgSize - 5], |
818 |
|
|
cState.first.configuration_[cfgSize - 4]); |
819 |
|
|
params.comAcceleration_ = contactGenHelper.acceleration_; |
820 |
|
|
params.sampleLimbName_ = *cit; |
821 |
|
|
params.tfWorldRoot_ = fcl::Transform3f(); |
822 |
|
|
params.tfWorldRoot_.setTranslation(fcl::Vec3f( |
823 |
|
|
cState.first.configuration_[0], cState.first.configuration_[1], |
824 |
|
|
cState.first.configuration_[2])); |
825 |
|
|
params.tfWorldRoot_.setQuatRotation(fcl::Quaternion3f( |
826 |
|
|
cState.first.configuration_[6], cState.first.configuration_[3], |
827 |
|
|
cState.first.configuration_[4], cState.first.configuration_[5])); |
828 |
|
|
|
829 |
|
|
if (cit + 1 == cState.second.end()) |
830 |
|
|
contactGenHelper.checkStabilityGenerate_ = checkStability; |
831 |
|
|
rep = generate_contact(contactGenHelper, *cit, params); |
832 |
|
|
if (rep.success_) { |
833 |
|
|
contactGenHelper.workingState_ = rep.result_; |
834 |
|
|
} |
835 |
|
|
// else |
836 |
|
|
// break; |
837 |
|
|
} |
838 |
|
|
} |
839 |
|
|
contactGenHelper.checkStabilityGenerate_ = checkStability; |
840 |
|
|
return rep; |
841 |
|
|
} |
842 |
|
|
|
843 |
|
|
projection::ProjectionReport repositionContacts(ContactGenHelper& helper) { |
844 |
|
|
ProjectionReport resultReport; |
845 |
|
|
State result = helper.workingState_; |
846 |
|
|
result.stable = false; |
847 |
|
|
State previous = result; |
848 |
|
|
// replace existing contacts |
849 |
|
|
// start with older contact created |
850 |
|
|
std::stack<std::string> poppedContacts; |
851 |
|
|
std::queue<std::string> oldOrder = result.contactOrder_; |
852 |
|
|
std::queue<std::string> newOrder; |
853 |
|
|
std::string nContactName = ""; |
854 |
|
|
core::Configuration_t savedConfig = helper.previousState_.configuration_; |
855 |
|
|
core::Configuration_t config = savedConfig; |
856 |
|
|
while (!result.stable && !oldOrder.empty()) { |
857 |
|
|
std::string previousContactName = oldOrder.front(); |
858 |
|
|
std::string groupName = |
859 |
|
|
helper.fullBody_->GetLimbs().at(previousContactName)->limb_->name(); |
860 |
|
|
const std::vector<std::string>& group = |
861 |
|
|
helper.fullBody_->GetGroups().at(groupName); |
862 |
|
|
oldOrder.pop(); |
863 |
|
|
core::ConfigurationIn_t save = |
864 |
|
|
helper.fullBody_->device_->currentConfiguration(); |
865 |
|
|
bool notFound(true); |
866 |
|
|
for (std::vector<std::string>::const_iterator cit = group.begin(); |
867 |
|
|
notFound && cit != group.end(); ++cit) { |
868 |
|
|
result.RemoveContact(*cit); |
869 |
|
|
helper.workingState_ = result; |
870 |
|
|
|
871 |
|
|
sampling::HeuristicParam params; |
872 |
|
|
params.contactPositions_ = helper.workingState_.contactPositions_; |
873 |
|
|
helper.fullBody_->device_->currentConfiguration(result.configuration_); |
874 |
|
|
helper.fullBody_->device_->computeForwardKinematics(); |
875 |
|
|
params.comPosition_ = helper.fullBody_->device_->positionCenterOfMass(); |
876 |
|
|
size_type cfgSize(helper.workingState_.configuration_.rows()); |
877 |
|
|
params.comSpeed_ = |
878 |
|
|
fcl::Vec3f(helper.workingState_.configuration_[cfgSize - 6], |
879 |
|
|
helper.workingState_.configuration_[cfgSize - 5], |
880 |
|
|
helper.workingState_.configuration_[cfgSize - 4]); |
881 |
|
|
params.comAcceleration_ = helper.acceleration_; |
882 |
|
|
params.sampleLimbName_ = *cit; |
883 |
|
|
params.tfWorldRoot_ = fcl::Transform3f(); |
884 |
|
|
params.tfWorldRoot_.setTranslation( |
885 |
|
|
fcl::Vec3f(helper.workingState_.configuration_[0], |
886 |
|
|
helper.workingState_.configuration_[1], |
887 |
|
|
helper.workingState_.configuration_[2])); |
888 |
|
|
params.tfWorldRoot_.setQuatRotation( |
889 |
|
|
fcl::Quaternion3f(helper.workingState_.configuration_[6], |
890 |
|
|
helper.workingState_.configuration_[3], |
891 |
|
|
helper.workingState_.configuration_[4], |
892 |
|
|
helper.workingState_.configuration_[5])); |
893 |
|
|
|
894 |
|
|
projection::ProjectionReport rep = |
895 |
|
|
contact::generate_contact(helper, *cit, params); |
896 |
|
|
if (rep.status_ == STABLE_CONTACT || REACHABLE_CONTACT) { |
897 |
|
|
nContactName = *cit; |
898 |
|
|
notFound = false; |
899 |
|
|
result = rep.result_; |
900 |
|
|
} else { |
901 |
|
|
result = previous; |
902 |
|
|
config = savedConfig; |
903 |
|
|
} |
904 |
|
|
} |
905 |
|
|
if (notFound) { |
906 |
|
|
config = savedConfig; |
907 |
|
|
result.configuration_ = savedConfig; |
908 |
|
|
poppedContacts.push(previousContactName); |
909 |
|
|
helper.fullBody_->device_->currentConfiguration(save); |
910 |
|
|
} |
911 |
|
|
} |
912 |
|
|
while (!poppedContacts.empty()) { |
913 |
|
|
newOrder.push(poppedContacts.top()); |
914 |
|
|
poppedContacts.pop(); |
915 |
|
|
} |
916 |
|
|
while (!oldOrder.empty()) { |
917 |
|
|
newOrder.push(oldOrder.front()); |
918 |
|
|
oldOrder.pop(); |
919 |
|
|
} |
920 |
|
|
if (result.stable) { |
921 |
|
|
newOrder.push(nContactName); |
922 |
|
|
resultReport.status_ = STABLE_CONTACT; |
923 |
|
|
resultReport.success_ = true; |
924 |
|
|
} |
925 |
|
|
result.contactOrder_ = newOrder; |
926 |
|
|
resultReport.result_ = result; |
927 |
|
|
return resultReport; |
928 |
|
|
} |
929 |
|
|
|
930 |
|
|
} // namespace contact |
931 |
|
|
} // namespace rbprm |
932 |
|
|
} // namespace hpp |