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/algorithm.hh> |
21 |
|
|
#include <hpp/rbprm/contact_generation/contact_generation.hh> |
22 |
|
|
#include <hpp/rbprm/rbprm-fullbody.hh> |
23 |
|
|
#include <hpp/rbprm/rbprm-state.hh> |
24 |
|
|
#include <hpp/rbprm/sampling/heuristic-tools.hh> |
25 |
|
|
#include <hpp/rbprm/stability/stability.hh> |
26 |
|
|
#include <hpp/rbprm/tools.hh> |
27 |
|
|
|
28 |
|
|
namespace hpp { |
29 |
|
|
namespace rbprm { |
30 |
|
|
namespace contact { |
31 |
|
|
|
32 |
|
|
ContactReport::ContactReport() |
33 |
|
|
: projection::ProjectionReport(), |
34 |
|
|
contactMaintained_(false), |
35 |
|
|
multipleBreaks_(false), |
36 |
|
|
contactCreated_(false), |
37 |
|
|
repositionedInPlace_(false) { |
38 |
|
|
// NOTHING |
39 |
|
|
} |
40 |
|
|
|
41 |
|
|
ContactReport::ContactReport(const projection::ProjectionReport& parent) |
42 |
|
|
: projection::ProjectionReport(parent), |
43 |
|
|
contactMaintained_(false), |
44 |
|
|
multipleBreaks_(false), |
45 |
|
|
contactCreated_(false), |
46 |
|
|
repositionedInPlace_(false) { |
47 |
|
|
// NOTHING |
48 |
|
|
} |
49 |
|
|
|
50 |
|
|
ContactReport generateContactReport(const projection::ProjectionReport& parent, |
51 |
|
|
const ContactGenHelper& helper, |
52 |
|
|
bool repositionedInPlace = false) { |
53 |
|
|
const State& previous = helper.previousState_; |
54 |
|
|
const State& result = parent.result_; |
55 |
|
|
ContactReport report(parent); |
56 |
|
|
report.contactCreated_ = (result.contactCreations(previous).size() > 0); |
57 |
|
|
report.multipleBreaks_ = |
58 |
|
|
(result.contactBreaks(previous).size() > helper.maxContactBreaks_); |
59 |
|
|
report.repositionedInPlace_ = repositionedInPlace; |
60 |
|
|
report.contactMaintained_ = |
61 |
|
|
!repositionedInPlace && (result.contactBreaks(previous).size() == 0); |
62 |
|
|
return report; |
63 |
|
|
} |
64 |
|
|
|
65 |
|
|
projection::ProjectionReport genContactFromOneMaintainCombinatorial( |
66 |
|
|
ContactGenHelper& helper) { |
67 |
|
|
// retrieve the first feasible result of maintain combinatorial... |
68 |
|
|
hppDout(notice, |
69 |
|
|
"Try to maintain contact for configuration : r([" |
70 |
|
|
<< pinocchio::displayConfig(helper.workingState_.configuration_) |
71 |
|
|
<< "])"); |
72 |
|
|
projection::ProjectionReport rep = contact::maintain_contacts(helper); |
73 |
|
|
hppDout(notice, "maintain contact, success = " << rep.success_); |
74 |
|
|
for (std::map<std::string, bool>::const_iterator cit = |
75 |
|
|
rep.result_.contacts_.begin(); |
76 |
|
|
cit != rep.result_.contacts_.end(); ++cit) { |
77 |
|
|
hppDout(notice, "contact : " << cit->first << " = " << cit->second); |
78 |
|
|
hppDout(notice, |
79 |
|
|
"position : " |
80 |
|
|
<< rep.result_.contactPositions_.at(cit->first).transpose()); |
81 |
|
|
hppDout( |
82 |
|
|
notice, "normal : " |
83 |
|
|
<< rep.result_.contactNormals_.at(cit->first).transpose()); |
84 |
|
|
} |
85 |
|
|
hppDout(notice, "genContact, after maintain : config : " |
86 |
|
|
<< pinocchio::displayConfig(rep.result_.configuration_)); |
87 |
|
|
if (rep.success_) { |
88 |
|
|
// ... if found, then try to generate feasible contact for this |
89 |
|
|
// combinatorial. |
90 |
|
|
hppDout(notice, "maintain contact success, gen contact : "); |
91 |
|
|
helper.workingState_ = rep.result_; |
92 |
|
|
return gen_contacts(helper); |
93 |
|
|
} |
94 |
|
|
return rep; |
95 |
|
|
} |
96 |
|
|
|
97 |
|
|
// if contact generation failed, tries to reposition the contacts without moving |
98 |
|
|
// the root |
99 |
|
|
ContactReport handleFailure(ContactGenHelper& helper) { |
100 |
|
|
helper.workingState_ = helper.previousState_; |
101 |
|
|
projection::ProjectionReport rep = repositionContacts(helper); |
102 |
|
|
|
103 |
|
|
std::vector<std::string> breaks = |
104 |
|
|
rep.result_.contactBreaks(helper.previousState_); |
105 |
|
|
std::vector<std::string> creations = |
106 |
|
|
rep.result_.contactCreations(helper.previousState_); |
107 |
|
|
return generateContactReport(rep, helper, true); |
108 |
|
|
} |
109 |
|
|
|
110 |
|
|
ContactReport oneStep(ContactGenHelper& helper) { |
111 |
|
|
projection::ProjectionReport rep; |
112 |
|
|
hppDout(notice, "OneStep"); |
113 |
|
|
do rep = genContactFromOneMaintainCombinatorial(helper); |
114 |
|
|
while (!rep.success_ && !helper.candidates_.empty()); |
115 |
|
|
if (!rep.success_) // TODO only possible in quasi static |
116 |
|
|
{ |
117 |
|
|
hppDout(notice, "Fail in oneStep, try repositionning"); |
118 |
|
|
return handleFailure(helper); |
119 |
|
|
} |
120 |
|
|
return generateContactReport(rep, helper); |
121 |
|
|
} |
122 |
|
|
|
123 |
|
|
bool ContactExistsWithinGroup( |
124 |
|
|
const hpp::rbprm::RbPrmLimbPtr_t& limb, |
125 |
|
|
const hpp::rbprm::RbPrmFullBody::T_LimbGroup& limbGroups, |
126 |
|
|
const State& current) { |
127 |
|
|
const std::vector<std::string>& group = limbGroups.at(limb->limb_->name()); |
128 |
|
|
for (std::vector<std::string>::const_iterator cit = group.begin(); |
129 |
|
|
cit != group.end(); ++cit) { |
130 |
|
|
if (current.contactPositions_.find(*cit) != current.contactPositions_.end()) |
131 |
|
|
return true; |
132 |
|
|
} |
133 |
|
|
return false; |
134 |
|
|
} |
135 |
|
|
|
136 |
|
|
ContactComputationStatus ComputeStableContact( |
137 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t& body, State& current, |
138 |
|
|
core::CollisionValidationPtr_t /*validation*/, const std::string& limbId, |
139 |
|
|
const hpp::rbprm::RbPrmLimbPtr_t& /*limb*/, |
140 |
|
|
pinocchio::ConfigurationIn_t /*rbconfiguration*/, |
141 |
|
|
pinocchio::ConfigurationOut_t configuration, const affMap_t& affordances, |
142 |
|
|
const std::map<std::string, std::vector<std::string> >& affFilters, |
143 |
|
|
const fcl::Vec3f& direction, fcl::Vec3f& position, fcl::Vec3f& normal, |
144 |
|
|
const double robustnessTreshold, |
145 |
|
|
const fcl::Vec3f& acceleration = fcl::Vec3f(0, 0, 0), |
146 |
|
|
bool contactIfFails = true, bool stableForOneContact = true, |
147 |
|
|
bool checkStabilityGenerate = true, |
148 |
|
|
const sampling::heuristic evaluate = 0) { |
149 |
|
|
contact::ContactGenHelper contactGenHelper( |
150 |
|
|
body, current, current.configuration_, affordances, affFilters, |
151 |
|
|
robustnessTreshold, 1, 1, false, checkStabilityGenerate, direction, |
152 |
|
|
acceleration, contactIfFails, stableForOneContact); |
153 |
|
|
contactGenHelper.testReachability_ = |
154 |
|
|
false; // because this method is only called without previous states |
155 |
|
|
sampling::HeuristicParam params; |
156 |
|
|
params.contactPositions_ = current.contactPositions_; |
157 |
|
|
contactGenHelper.fullBody_->device_->currentConfiguration( |
158 |
|
|
contactGenHelper.workingState_.configuration_); |
159 |
|
|
contactGenHelper.fullBody_->device_->computeForwardKinematics(); |
160 |
|
|
params.comPosition_ = |
161 |
|
|
contactGenHelper.fullBody_->device_->positionCenterOfMass(); |
162 |
|
|
size_type cfgSize(contactGenHelper.workingState_.configuration_.rows()); |
163 |
|
|
params.comSpeed_ = |
164 |
|
|
fcl::Vec3f(contactGenHelper.workingState_.configuration_[cfgSize - 6], |
165 |
|
|
contactGenHelper.workingState_.configuration_[cfgSize - 5], |
166 |
|
|
contactGenHelper.workingState_.configuration_[cfgSize - 4]); |
167 |
|
|
params.comAcceleration_ = contactGenHelper.acceleration_; |
168 |
|
|
params.sampleLimbName_ = limbId; |
169 |
|
|
params.tfWorldRoot_ = fcl::Transform3f(); |
170 |
|
|
params.tfWorldRoot_.setTranslation(fcl::Vec3f(current.configuration_[0], |
171 |
|
|
current.configuration_[1], |
172 |
|
|
current.configuration_[2])); |
173 |
|
|
params.tfWorldRoot_.setQuatRotation( |
174 |
|
|
fcl::Quaternion3f(current.configuration_[6], current.configuration_[3], |
175 |
|
|
current.configuration_[4], current.configuration_[5])); |
176 |
|
|
hpp::rbprm::projection::ProjectionReport rep = |
177 |
|
|
contact::generate_contact(contactGenHelper, limbId, params, evaluate); |
178 |
|
|
|
179 |
|
|
current = rep.result_; |
180 |
|
|
configuration = rep.result_.configuration_; |
181 |
|
|
if (rep.status_ != NO_CONTACT) { |
182 |
|
|
position = rep.result_.contactPositions_[limbId]; |
183 |
|
|
normal = rep.result_.contactNormals_[limbId]; |
184 |
|
|
hppDout(notice, "Create contact for limb " << limbId); |
185 |
|
|
hppDout(notice, "position : " << position); |
186 |
|
|
hppDout(notice, "normal : " << normal); |
187 |
|
|
} |
188 |
|
|
if (rep.status_ == STABLE_CONTACT || REACHABLE_CONTACT) { |
189 |
|
|
current.stable = true; |
190 |
|
|
} |
191 |
|
|
return rep.status_; |
192 |
|
|
} |
193 |
|
|
|
194 |
|
|
hpp::rbprm::State ComputeContacts( |
195 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t& body, |
196 |
|
|
pinocchio::ConfigurationIn_t configuration, const affMap_t& affordances, |
197 |
|
|
const std::map<std::string, std::vector<std::string> >& affFilters, |
198 |
|
|
const fcl::Vec3f& direction, const double robustnessTreshold, |
199 |
|
|
const fcl::Vec3f& acceleration) { |
200 |
|
|
const T_Limb& limbs = body->GetLimbs(); |
201 |
|
|
const rbprm::RbPrmFullBody::T_LimbGroup& limbGroups = body->GetGroups(); |
202 |
|
|
const std::map<std::string, core::CollisionValidationPtr_t>& |
203 |
|
|
limbcollisionValidations = body->GetLimbCollisionValidation(); |
204 |
|
|
State result; |
205 |
|
|
// save old configuration |
206 |
|
|
hppDout(notice, "Begin compute contact, without previous state."); |
207 |
|
|
core::ConfigurationIn_t save = body->device_->currentConfiguration(); |
208 |
|
|
result.configuration_ = configuration; |
209 |
|
|
body->device_->currentConfiguration(configuration); |
210 |
|
|
body->device_->computeForwardKinematics(); |
211 |
|
|
bool checkStabilityGenerate(false); |
212 |
|
|
size_t id = 0; |
213 |
|
|
for (T_Limb::const_iterator lit = limbs.begin(); lit != limbs.end(); |
214 |
|
|
++lit, ++id) { |
215 |
|
|
if (!ContactExistsWithinGroup(lit->second, limbGroups, result)) { |
216 |
|
|
hppDout(notice, "ComputeContacts, call computeStable contact for limb : " |
217 |
|
|
<< lit->first); |
218 |
|
|
fcl::Vec3f normal, position; |
219 |
|
|
if (id == (limbs.size() - 1)) |
220 |
|
|
checkStabilityGenerate = |
221 |
|
|
true; // we only check stability for the last contact |
222 |
|
|
ComputeStableContact( |
223 |
|
|
body, result, limbcollisionValidations.at(lit->first), lit->first, |
224 |
|
|
lit->second, configuration, result.configuration_, affordances, |
225 |
|
|
affFilters, direction, position, normal, robustnessTreshold, |
226 |
|
|
acceleration, false, false, checkStabilityGenerate); |
227 |
|
|
} |
228 |
|
|
result.nbContacts = result.contactNormals_.size(); |
229 |
|
|
} |
230 |
|
|
// reload previous configuration |
231 |
|
|
body->device_->currentConfiguration(save); |
232 |
|
|
return result; |
233 |
|
|
} |
234 |
|
|
|
235 |
|
|
hpp::rbprm::contact::ContactReport ComputeContacts( |
236 |
|
|
const hpp::rbprm::State& previous, |
237 |
|
|
const hpp::rbprm::RbPrmFullBodyPtr_t& body, |
238 |
|
|
pinocchio::ConfigurationIn_t configuration, const affMap_t& affordances, |
239 |
|
|
const std::map<std::string, std::vector<std::string> >& affFilters, |
240 |
|
|
const fcl::Vec3f& direction, const double robustnessTreshold, |
241 |
|
|
const fcl::Vec3f& acceleration, const core::PathConstPtr_t& comPath, |
242 |
|
|
const double currentPathId, const bool testReachability, |
243 |
|
|
const bool quasiStatic) { |
244 |
|
|
// save old configuration |
245 |
|
|
core::ConfigurationIn_t save = body->device_->currentConfiguration(); |
246 |
|
|
pinocchio::Computation_t flag = body->device_->computationFlag(); |
247 |
|
|
pinocchio::Computation_t newflag = static_cast<pinocchio::Computation_t>( |
248 |
|
|
pinocchio::JOINT_POSITION | pinocchio::JACOBIAN | pinocchio::COM); |
249 |
|
|
// pinocchio::Computation_t newflag = static_cast <pinocchio::Computation_t> |
250 |
|
|
// (pinocchio::ALL); load new root position |
251 |
|
|
body->device_->controlComputation(newflag); |
252 |
|
|
body->device_->currentConfiguration(configuration); |
253 |
|
|
body->device_->computeForwardKinematics(); |
254 |
|
|
// try to maintain previous contacts |
255 |
|
|
hppDout(notice, "Compute contact, previous state : r([" |
256 |
|
|
<< pinocchio::displayConfig(previous.configuration_) |
257 |
|
|
<< "])"); |
258 |
|
|
contact::ContactGenHelper cHelper(body, previous, configuration, affordances, |
259 |
|
|
affFilters, robustnessTreshold, 1, 1, false, |
260 |
|
|
true, direction, acceleration, false, false, |
261 |
|
|
comPath, currentPathId); |
262 |
|
|
cHelper.testReachability_ = testReachability; |
263 |
|
|
cHelper.quasiStatic_ = quasiStatic; |
264 |
|
|
contact::ContactReport rep = contact::oneStep(cHelper); |
265 |
|
|
|
266 |
|
|
// copy extra dofs |
267 |
|
|
if (rep.success_) { |
268 |
|
|
const pinocchio::size_type& extraDim = |
269 |
|
|
body->device_->extraConfigSpace().dimension(); |
270 |
|
|
rep.result_.configuration_.tail(extraDim) = configuration.tail(extraDim); |
271 |
|
|
} |
272 |
|
|
body->device_->currentConfiguration(save); |
273 |
|
|
body->device_->controlComputation(flag); |
274 |
|
|
return rep; |
275 |
|
|
} |
276 |
|
|
|
277 |
|
|
} // namespace contact |
278 |
|
|
} // namespace rbprm |
279 |
|
|
} // namespace hpp |