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 <time.h> |
18 |
|
|
|
19 |
|
|
#include <Eigen/Eigen> |
20 |
|
|
#include <hpp/pinocchio/configuration.hh> |
21 |
|
|
#include <hpp/rbprm/sampling/heuristic-tools.hh> |
22 |
|
|
#include <hpp/rbprm/sampling/heuristic.hh> |
23 |
|
|
|
24 |
|
|
using namespace hpp; |
25 |
|
|
using namespace hpp::pinocchio; |
26 |
|
|
using namespace hpp::rbprm; |
27 |
|
|
using namespace hpp::rbprm::sampling; |
28 |
|
|
|
29 |
|
|
namespace { |
30 |
|
|
|
31 |
|
|
double dynamicHeuristic(const sampling::Sample& sample, |
32 |
|
|
const Eigen::Vector3d& /*direction*/, |
33 |
|
|
const Eigen::Vector3d& /*normal*/, |
34 |
|
|
const HeuristicParam& params) { |
35 |
|
|
fcl::Vec3f effectorPosition = |
36 |
|
|
transform(sample.effectorPosition_, params.tfWorldRoot_.getTranslation(), |
37 |
|
|
params.tfWorldRoot_.getRotation()); |
38 |
|
|
std::map<std::string, fcl::Vec3f> contacts; |
39 |
|
|
contacts.insert(params.contactPositions_.begin(), |
40 |
|
|
params.contactPositions_.end()); |
41 |
|
|
contacts.insert(std::make_pair(params.sampleLimbName_, effectorPosition)); |
42 |
|
|
removeNonGroundContacts(contacts, 0.25); // keep only ground contacts |
43 |
|
|
|
44 |
|
|
double g(-9.80665); |
45 |
|
|
double w2(params.comPosition_[2] / g); // w2 < 0 |
46 |
|
|
double w1x(-100. * w2); // w1 > 0 |
47 |
|
|
double w1y(-100. * w2); // w1 > 0 |
48 |
|
|
|
49 |
|
|
// We want : |w1*comSpeed| > |w2*comAcceleration| |
50 |
|
|
if (params.comSpeed_[0] != 0 && w2 * params.comAcceleration_[0] != 0) { |
51 |
|
|
while (std::abs(w1x * params.comSpeed_[0]) <= |
52 |
|
|
std::abs(w2 * params.comAcceleration_[0])) { |
53 |
|
|
w1x *= 1.5; |
54 |
|
|
} |
55 |
|
|
} |
56 |
|
|
if (params.comSpeed_[1] != 0 && w2 * params.comAcceleration_[0] != 0) { |
57 |
|
|
while (std::abs(w1y * params.comSpeed_[1]) <= |
58 |
|
|
std::abs(w2 * params.comAcceleration_[1])) { |
59 |
|
|
w1y *= 1.5; |
60 |
|
|
} |
61 |
|
|
} |
62 |
|
|
|
63 |
|
|
double x_interest(params.comPosition_[0] + w1x * params.comSpeed_[0] + |
64 |
|
|
w2 * params.comAcceleration_[0]); |
65 |
|
|
double y_interest(params.comPosition_[1] + w1y * params.comSpeed_[1] + |
66 |
|
|
w2 * params.comAcceleration_[1]); |
67 |
|
|
|
68 |
|
|
Vec2D interest(x_interest, y_interest); |
69 |
|
|
|
70 |
|
|
double result; |
71 |
|
|
try { |
72 |
|
|
Vec2D wcentroid( |
73 |
|
|
weightedCentroidConvex2D(convexHull(computeSupportPolygon(contacts)))); |
74 |
|
|
result = Vec2D::euclideanDist(interest, wcentroid); |
75 |
|
|
} catch (const std::string& s) { |
76 |
|
|
std::cout << s << std::endl; |
77 |
|
|
result = std::numeric_limits<double>::max(); |
78 |
|
|
} |
79 |
|
|
|
80 |
|
|
return -result; // '-' because minimize a value is equivalent to maximimze |
81 |
|
|
// its opposite |
82 |
|
|
} |
83 |
|
|
|
84 |
|
|
double EFORTHeuristic(const sampling::Sample& sample, |
85 |
|
|
const Eigen::Vector3d& direction, |
86 |
|
|
const Eigen::Vector3d& normal, |
87 |
|
|
const HeuristicParam& /*params*/) { |
88 |
|
|
double EFORT = -direction.transpose() * |
89 |
|
|
sample.jacobianProduct_.block<3, 3>(0, 0) * (-direction); |
90 |
|
|
return EFORT * Eigen::Vector3d::UnitZ().dot(normal); |
91 |
|
|
} |
92 |
|
|
|
93 |
|
|
double EFORTNormalHeuristic(const sampling::Sample& sample, |
94 |
|
|
const Eigen::Vector3d& direction, |
95 |
|
|
const Eigen::Vector3d& normal, |
96 |
|
|
const HeuristicParam& /*params*/) { |
97 |
|
|
double EFORT = -direction.transpose() * |
98 |
|
|
sample.jacobianProduct_.block<3, 3>(0, 0) * (-direction); |
99 |
|
|
return EFORT * direction.dot(normal); |
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
double ManipulabilityHeuristic(const sampling::Sample& sample, |
103 |
|
|
const Eigen::Vector3d& /*direction*/, |
104 |
|
|
const Eigen::Vector3d& normal, |
105 |
|
|
const HeuristicParam& /*params*/) { |
106 |
|
|
if (Eigen::Vector3d::UnitZ().dot(normal) < 0.7) return -1; |
107 |
|
|
return sample.staticValue_ * 10000 * Eigen::Vector3d::UnitZ().dot(normal) * |
108 |
|
|
100000 + |
109 |
|
|
((double)rand()) / ((double)(RAND_MAX)); |
110 |
|
|
} |
111 |
|
|
|
112 |
|
|
double RandomHeuristic(const sampling::Sample& /*sample*/, |
113 |
|
|
const Eigen::Vector3d& /*direction*/, |
114 |
|
|
const Eigen::Vector3d& /*normal*/, |
115 |
|
|
const HeuristicParam& /*params*/) { |
116 |
|
|
return ((double)rand()) / ((double)(RAND_MAX)); |
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
double ForwardHeuristic(const sampling::Sample& sample, |
120 |
|
|
const Eigen::Vector3d& direction, |
121 |
|
|
const Eigen::Vector3d& normal, |
122 |
|
|
const HeuristicParam& /*params*/) { |
123 |
|
|
/* //hppDout(notice,"static value : "<<sample.staticValue_); |
124 |
|
|
hppDout(notice,"eff position = "<<sample.effectorPosition_); |
125 |
|
|
hppDout(notice,"limb frame = "<<sample.effectorPositionInLimbFrame_); |
126 |
|
|
hppDout(notice,"direction = "<<direction);*/ |
127 |
|
|
return sample.staticValue_ * 1000. * Eigen::Vector3d::UnitZ().dot(normal) + |
128 |
|
|
100. * sample.effectorPositionInLimbFrame_.dot( |
129 |
|
|
fcl::Vec3f(direction(0), direction(1), |
130 |
|
|
sample.effectorPositionInLimbFrame_[2])) + |
131 |
|
|
((double)rand()) / ((double)(RAND_MAX)); |
132 |
|
|
} |
133 |
|
|
|
134 |
|
|
double DynamicWalkHeuristic(const sampling::Sample& sample, |
135 |
|
|
const Eigen::Vector3d& direction, |
136 |
|
|
const Eigen::Vector3d& normal, |
137 |
|
|
const HeuristicParam& params) { |
138 |
|
|
fcl::Vec3f dir(direction); |
139 |
|
|
fcl::Vec3f pos(sample.effectorPositionInLimbFrame_); |
140 |
|
|
fcl::Vec3f n(normal); |
141 |
|
|
n.normalize(); |
142 |
|
|
double weightDir, weightStatic; |
143 |
|
|
weightStatic = 100.; |
144 |
|
|
|
145 |
|
|
if (direction.norm() == 0 || |
146 |
|
|
std::isnan(direction.norm())) { // test for null vector, functions called |
147 |
|
|
// before this one can try to normalize |
148 |
|
|
// direction resulting in NaN |
149 |
|
|
weightDir = 0; |
150 |
|
|
dir = fcl::Vec3f(0, 0, 1); |
151 |
|
|
} else { |
152 |
|
|
weightDir = 100.; |
153 |
|
|
weightStatic = weightStatic * Eigen::Vector3d::UnitZ().dot(normal); |
154 |
|
|
dir[2] = 0; // FIXME : replace this by a projection on the surface plan ( |
155 |
|
|
// we know the normal) |
156 |
|
|
dir.normalize(); |
157 |
|
|
// hppDout(notice,"limb frame vlb = |
158 |
|
|
// ["<<sample.configuration_[0]<<","<<sample.configuration_[1]<<","<<sample.configuration_[2]<<","<<pos[0]<<","<<pos[1]<<","<<0<<" |
159 |
|
|
// ]"); |
160 |
|
|
pos = (params.tfWorldRoot_.getRotation() * (pos)); |
161 |
|
|
pos[2] = 0; // FIXME : replace this by a projection on the surface plan ( |
162 |
|
|
// we know the normal) |
163 |
|
|
// pos = pos.normalize(); |
164 |
|
|
// hppDout(notice,"root transform : "<<params.tfWorldRoot_); |
165 |
|
|
fcl::Vec3f limbRoot = |
166 |
|
|
sample.effectorPosition_ - sample.effectorPositionInLimbFrame_; |
167 |
|
|
// hppDout(notice,"limb origin : "<<limbRoot); |
168 |
|
|
|
169 |
|
|
// compute signed angle between dir and pos |
170 |
|
|
double angle = atan2((dir.cross(pos)).dot(n), dir.dot(pos)); |
171 |
|
|
if (limbRoot[1] > 0.) { // the current limb is on the left of the root |
172 |
|
|
// hppDout(notice,"left limb"); |
173 |
|
|
// test if the pos vector is on the right of the dir vector |
174 |
|
|
if (angle < 0) { |
175 |
|
|
weightDir /= 10.; |
176 |
|
|
dir = -dir; |
177 |
|
|
// hppDout(notice,"pos vector is on the wrong side of dir"); |
178 |
|
|
} |
179 |
|
|
} else { // the current limb is on the right of the root |
180 |
|
|
// hppDout(notice,"right limb"); |
181 |
|
|
// test if the pos vector is on the left of the dir vector |
182 |
|
|
if (angle > 0) { |
183 |
|
|
weightDir /= 10.; |
184 |
|
|
dir = -dir; |
185 |
|
|
// hppDout(notice,"pos vector is on the wrong side of dir"); |
186 |
|
|
} |
187 |
|
|
} |
188 |
|
|
} // if dir not null |
189 |
|
|
|
190 |
|
|
/* hppDout(notice,"eff position = "<<sample.effectorPosition_); |
191 |
|
|
hppDout(notice,"limb frame vl = |
192 |
|
|
["<<sample.configuration_[0]<<","<<sample.configuration_[1]<<","<<sample.configuration_[2]<<","<<pos[0]<<","<<pos[1]<<","<<pos[2]<<" |
193 |
|
|
]"); hppDout(notice,"direction vd = |
194 |
|
|
["<<sample.configuration_[0]<<","<<sample.configuration_[1]<<","<<sample.configuration_[2]<<","<<dir[0]<<","<<dir[1]<<","<<dir[2]<<" |
195 |
|
|
]"); hppDout(notice,"value of dot product = "<<pos.dot(dir)); |
196 |
|
|
hppDout(notice,"static value = |
197 |
|
|
"<<sample.staticValue_);*/ |
198 |
|
|
|
199 |
|
|
return weightStatic * sample.staticValue_ + weightDir * pos.dot(dir) + |
200 |
|
|
1. * pos.dot(fcl::Vec3f(params.comAcceleration_[0], |
201 |
|
|
params.comAcceleration_[1], dir[2])) + |
202 |
|
|
((double)rand()) / ((double)(RAND_MAX)); |
203 |
|
|
} |
204 |
|
|
|
205 |
|
|
double StaticHeuristic(const sampling::Sample& sample, |
206 |
|
|
const Eigen::Vector3d& /*direction*/, |
207 |
|
|
const Eigen::Vector3d& /*normal*/, |
208 |
|
|
const HeuristicParam& /*params*/) { |
209 |
|
|
/*hppDout(info,"sample : "); |
210 |
|
|
hppDout(info,"sample : "<<&sample); |
211 |
|
|
hppDout(info,"id = "<<sample.id_); |
212 |
|
|
hppDout(info,"length = "<<sample.length_); |
213 |
|
|
hppDout(info,"startRank = "<<sample.startRank_); |
214 |
|
|
hppDout(info,"effectorPosition = "<<sample.effectorPosition_); |
215 |
|
|
hppDout(info,"configuration = "<<sample.configuration_); |
216 |
|
|
hppDout(info,"staticValue = "<<sample.staticValue_); |
217 |
|
|
*/ |
218 |
|
|
return sample.staticValue_; |
219 |
|
|
} |
220 |
|
|
|
221 |
|
|
double fixedStepHeuristic(const sampling::Sample& sample, |
222 |
|
|
const Eigen::Vector3d& direction, |
223 |
|
|
const Eigen::Vector3d& normal, |
224 |
|
|
const HeuristicParam& params, const double t_step) { |
225 |
|
|
if (!params.comPath_) { |
226 |
|
|
hppDout(notice, |
227 |
|
|
"In heuristic : comPath was not provided, use only current " |
228 |
|
|
"analysis score"); |
229 |
|
|
return StaticHeuristic(sample, direction, normal, params); |
230 |
|
|
} |
231 |
|
|
// hppDout(notice,"FixedStep heuristic : comPath exist"); |
232 |
|
|
bool success; |
233 |
|
|
// Compute the 'ideal' contact position in t_step |
234 |
|
|
Configuration_t q_target = |
235 |
|
|
(*params.comPath_)(params.currentPathId_ + t_step, success).head<7>(); |
236 |
|
|
fcl::Transform3f tRootTarget; |
237 |
|
|
tRootTarget.setTranslation(fcl::Vec3f(q_target.head<3>())); |
238 |
|
|
fcl::Quaternion3f quatRoot(q_target[6], q_target[3], q_target[4], |
239 |
|
|
q_target[5]); |
240 |
|
|
tRootTarget.setQuatRotation(quatRoot); |
241 |
|
|
// hppDout(notice,"heuristic : tRootTarget = "<<tRootTarget.getRotation()); |
242 |
|
|
// hppDout(notice,"heuristic : tRootTarget = "<<tRootTarget.getTranslation()); |
243 |
|
|
// hppDout(notice,"heuristic : limbRef = "<<params.limbReferenceOffset_); |
244 |
|
|
fcl::Vec3f pTarget = |
245 |
|
|
(tRootTarget * params.limbReferenceOffset_).getTranslation(); |
246 |
|
|
// hppDout(notice,"heuristic : pTarget = |
247 |
|
|
// ["<<pTarget[0]<<","<<pTarget[1]<<","<<pTarget[2]<<"]"); |
248 |
|
|
// FIXME : we could factorize all of the above and only do it once for each |
249 |
|
|
// position of the CoM. But this require to know t_step in |
250 |
|
|
// contact_generation::generate_contact ... |
251 |
|
|
fcl::Vec3f pSample = |
252 |
|
|
(params.tfWorldRoot_ * sample.effectorPosition_).getTranslation(); |
253 |
|
|
/* hppDout(notice,"Heuristic : norm = |
254 |
|
|
"<<(pSample-pTarget).squaredNorm()); hppDout(notice,"Heuristic : 5 - norm = |
255 |
|
|
"<<5-(pSample-pTarget).squaredNorm()); hppDout(notice,"Heuristic : static = |
256 |
|
|
"<<sample.staticValue_); hppDout(notice,"Heuritic : = |
257 |
|
|
"<<(5.-(pSample-pTarget).squaredNorm()) + sample.staticValue_); |
258 |
|
|
*/ |
259 |
|
|
if ((pSample - pTarget).squaredNorm() > 1) |
260 |
|
|
hppDout(warning, |
261 |
|
|
"WARNING : In fixed step heuristic, norm is too high. You should " |
262 |
|
|
"change the hardcoded max value "); |
263 |
|
|
return (1. - (pSample - pTarget).squaredNorm()) + |
264 |
|
|
0.1 * |
265 |
|
|
sample |
266 |
|
|
.staticValue_; // 1 - because it's an heuristic and not a cost |
267 |
|
|
} |
268 |
|
|
|
269 |
|
|
double fixedStep1Heuristic(const sampling::Sample& sample, |
270 |
|
|
const Eigen::Vector3d& direction, |
271 |
|
|
const Eigen::Vector3d& normal, |
272 |
|
|
const HeuristicParam& params) { |
273 |
|
|
return fixedStepHeuristic(sample, direction, normal, params, 1.); |
274 |
|
|
} |
275 |
|
|
|
276 |
|
|
double fixedStep08Heuristic(const sampling::Sample& sample, |
277 |
|
|
const Eigen::Vector3d& direction, |
278 |
|
|
const Eigen::Vector3d& normal, |
279 |
|
|
const HeuristicParam& params) { |
280 |
|
|
return fixedStepHeuristic(sample, direction, normal, params, 0.8); |
281 |
|
|
} |
282 |
|
|
|
283 |
|
|
double fixedStep06Heuristic(const sampling::Sample& sample, |
284 |
|
|
const Eigen::Vector3d& direction, |
285 |
|
|
const Eigen::Vector3d& normal, |
286 |
|
|
const HeuristicParam& params) { |
287 |
|
|
return fixedStepHeuristic(sample, direction, normal, params, 0.6); |
288 |
|
|
} |
289 |
|
|
|
290 |
|
|
double fixedStep04Heuristic(const sampling::Sample& sample, |
291 |
|
|
const Eigen::Vector3d& direction, |
292 |
|
|
const Eigen::Vector3d& normal, |
293 |
|
|
const HeuristicParam& params) { |
294 |
|
|
return fixedStepHeuristic(sample, direction, normal, params, 0.4); |
295 |
|
|
} |
296 |
|
|
|
297 |
|
|
double BackwardHeuristic(const sampling::Sample& sample, |
298 |
|
|
const Eigen::Vector3d& direction, |
299 |
|
|
const Eigen::Vector3d& normal, |
300 |
|
|
const HeuristicParam& /*params*/) { |
301 |
|
|
return sample.staticValue_ * 10000 * Eigen::Vector3d::UnitZ().dot(normal) - |
302 |
|
|
100 * sample.effectorPosition_.dot( |
303 |
|
|
fcl::Vec3f(direction(0), direction(1), direction(2))) + |
304 |
|
|
((double)rand()) / ((double)(RAND_MAX)); |
305 |
|
|
} |
306 |
|
|
|
307 |
|
|
double DistanceToLimitHeuristic(const sampling::Sample& sample, |
308 |
|
|
const Eigen::Vector3d& /*direction*/, |
309 |
|
|
const Eigen::Vector3d& /*normal*/, |
310 |
|
|
const HeuristicParam& /*params*/) { |
311 |
|
|
return sample.configuration_.norm(); |
312 |
|
|
} |
313 |
|
|
} // namespace |
314 |
|
|
|
315 |
|
4 |
HeuristicFactory::HeuristicFactory() { |
316 |
|
4 |
unsigned int seed = (unsigned int)(time(NULL)); |
317 |
|
4 |
srand(seed); |
318 |
|
|
hppDout(notice, "SEED for heuristic = " << seed); |
319 |
|
|
/*std::ofstream fout; |
320 |
|
|
fout.open("/local/fernbac/bench_iros18/success/log_success.log",std::fstream::app); |
321 |
|
|
fout<<"seed = "<<seed<<std::endl; |
322 |
|
|
fout.close();*/ |
323 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("static", &StaticHeuristic)); |
324 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("EFORT", &EFORTHeuristic)); |
325 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("EFORT_Normal", &EFORTNormalHeuristic)); |
326 |
|
|
heuristics_.insert( |
327 |
✓✗✓✗
|
4 |
std::make_pair("manipulability", &ManipulabilityHeuristic)); |
328 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("random", &RandomHeuristic)); |
329 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("forward", &ForwardHeuristic)); |
330 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("dynamicWalk", &DynamicWalkHeuristic)); |
331 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("backward", &BackwardHeuristic)); |
332 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("jointlimits", &DistanceToLimitHeuristic)); |
333 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("dynamic", &dynamicHeuristic)); |
334 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("fixedStep1", &fixedStep1Heuristic)); |
335 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("fixedStep08", &fixedStep08Heuristic)); |
336 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("fixedStep06", &fixedStep06Heuristic)); |
337 |
✓✗✓✗
|
4 |
heuristics_.insert(std::make_pair("fixedStep04", &fixedStep04Heuristic)); |
338 |
|
4 |
} |
339 |
|
|
|
340 |
|
4 |
HeuristicFactory::~HeuristicFactory() {} |
341 |
|
|
|
342 |
|
|
bool HeuristicFactory::AddHeuristic(const std::string& name, |
343 |
|
|
const heuristic func) { |
344 |
|
|
if (heuristics_.find(name) != heuristics_.end()) return false; |
345 |
|
|
heuristics_.insert(std::make_pair(name, func)); |
346 |
|
|
return true; |
347 |
|
|
} |