| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
|||
2 |
// Copyright (C) 2018 LAAS-CNRS |
||
3 |
// Author: Pierre Fernbach |
||
4 |
// |
||
5 |
// This file is part of the hpp-rbprm. |
||
6 |
// |
||
7 |
// hpp-rbprm is free software: you can redistribute it and/or modify |
||
8 |
// it under the terms of the GNU Lesser General Public License as published by |
||
9 |
// the Free Software Foundation, either version 3 of the License, or |
||
10 |
// (at your option) any later version. |
||
11 |
// |
||
12 |
// test-hpp is distributed in the hope that it will be useful, |
||
13 |
// but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
14 |
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
15 |
// GNU Lesser General Public License for more details. |
||
16 |
// |
||
17 |
// You should have received a copy of the GNU Lesser General Public License |
||
18 |
// along with hpp-core. If not, see <http://www.gnu.org/licenses/>. |
||
19 |
|||
20 |
#ifndef TOOLSOBSTACLE_HH |
||
21 |
#define TOOLSOBSTACLE_HH |
||
22 |
|||
23 |
#include <hpp/affordance/affordance-extraction.hh> |
||
24 |
#include <hpp/affordance/operations.hh> |
||
25 |
#include <hpp/core/config-validations.hh> |
||
26 |
#include <hpp/core/problem-solver.hh> |
||
27 |
#include <hpp/core/problem.hh> |
||
28 |
#include <hpp/pinocchio/device.hh> |
||
29 |
#include <hpp/pinocchio/fwd.hh> |
||
30 |
#include <hpp/pinocchio/urdf/util.hh> |
||
31 |
#include <hpp/rbprm/dynamic/dynamic-path-validation.hh> |
||
32 |
#include <hpp/rbprm/planner/dynamic-planner.hh> |
||
33 |
#include <hpp/rbprm/planner/oriented-path-optimizer.hh> |
||
34 |
#include <hpp/rbprm/planner/random-shortcut-dynamic.hh> |
||
35 |
#include <hpp/rbprm/planner/rbprm-steering-kinodynamic.hh> |
||
36 |
#include <hpp/rbprm/rbprm-device.hh> |
||
37 |
#include <hpp/rbprm/rbprm-path-validation.hh> |
||
38 |
#include <hpp/rbprm/rbprm-shooter.hh> |
||
39 |
#include <pinocchio/parsers/urdf.hpp> |
||
40 |
|||
41 |
#if BOOST_VERSION / 100 % 1000 >= 60 |
||
42 |
#include <boost/bind/bind.hpp> |
||
43 |
using namespace boost::placeholders; |
||
44 |
#else |
||
45 |
#include <boost/bind.hpp> |
||
46 |
#endif |
||
47 |
|||
48 |
using namespace hpp; |
||
49 |
using namespace hpp::core; |
||
50 |
|||
51 |
13 |
void addAffObjects(hpp::core::ProblemSolver& problemSolver, |
|
52 |
const affordance::OperationBases_t& ops, |
||
53 |
const std::vector<affordance::CollisionObjects_t>& affObjs, |
||
54 |
const std::string obstacleNameNonAff) { |
||
55 |
✓✗ | 26 |
const std::string affSuffix = "aff"; |
56 |
✓✗ | 26 |
std::string obstacleName(obstacleNameNonAff); |
57 |
✓✗ | 13 |
obstacleName += affSuffix; |
58 |
✓✓ | 52 |
for (unsigned int opIdx = 0; opIdx < ops.size(); opIdx++) { |
59 |
78 |
AffordanceObjects_t objs; |
|
60 |
✓✗ | 39 |
affordance::CollisionObjects_t affs = affObjs[opIdx]; |
61 |
✓✓ | 209 |
for (unsigned int objIdx = 0; objIdx < affs.size(); objIdx++) { |
62 |
✓✗ | 340 |
std::stringstream ss; |
63 |
✓✗✓✗ ✓✗ |
170 |
ss << opIdx << "_" << objIdx; |
64 |
✓✗✓✗ |
340 |
std::string ig = obstacleName + ss.str(); |
65 |
✓✗ | 170 |
problemSolver.addObstacle(ig, *(affs[objIdx]), false, false); |
66 |
✓✗ | 170 |
hpp::pinocchio::CollisionObjectPtr_t obj = problemSolver.obstacle(ig); |
67 |
✓✗✓✗ |
170 |
objs.push_back(std::make_pair(ig, obj)); |
68 |
} |
||
69 |
✓✗✓✗ ✗✓ |
39 |
if (problemSolver.affordanceObjects.has(ops[opIdx]->affordance_)) { |
70 |
AffordanceObjects_t mapObjs = |
||
71 |
problemSolver.affordanceObjects.get(ops[opIdx]->affordance_); |
||
72 |
objs.insert(objs.begin() + objs.size(), mapObjs.begin(), mapObjs.end()); |
||
73 |
} |
||
74 |
✓✗✓✗ |
39 |
problemSolver.affordanceObjects.erase(ops[opIdx]->affordance_); |
75 |
✓✗✓✗ |
39 |
problemSolver.affordanceObjects.add(ops[opIdx]->affordance_, objs); |
76 |
} |
||
77 |
13 |
} |
|
78 |
|||
79 |
13 |
void affordanceAnalysis(ProblemSolver& problemSolver, |
|
80 |
const std::string& obstacleName, |
||
81 |
const affordance::OperationBases_t& operations, |
||
82 |
std::vector<double> reduceSizes) { |
||
83 |
✓✗ | 26 |
std::list<std::string> obstacles = problemSolver.obstacleNames(true, true); |
84 |
std::list<std::string>::iterator objIt = |
||
85 |
✓✗ | 13 |
std::find(obstacles.begin(), obstacles.end(), obstacleName); |
86 |
✗✓✗✗ |
13 |
while (reduceSizes.size() < operations.size()) reduceSizes.push_back(0.); |
87 |
✗✓ | 13 |
if (objIt == obstacles.end()) |
88 |
throw std::runtime_error( |
||
89 |
"No obstacle by given name found. Unable to analyse."); |
||
90 |
try { |
||
91 |
affordance::SemanticsDataPtr_t aff = affordance::affordanceAnalysis( |
||
92 |
✓✗✓✗ ✓✗ |
26 |
(problemSolver.obstacle(obstacleName)->fcl()), operations); |
93 |
std::vector<std::vector<fcl::CollisionObjectPtr_t> > affObjs = |
||
94 |
✓✗✓✗ |
13 |
affordance::getReducedAffordanceObjects(aff, reduceSizes); |
95 |
// add fcl::CollisionObstacles to problemSolver |
||
96 |
✓✗✓✗ |
13 |
addAffObjects(problemSolver, operations, affObjs, obstacleName); |
97 |
} catch (const std::exception& exc) { |
||
98 |
throw std::runtime_error(exc.what()); |
||
99 |
} |
||
100 |
13 |
} |
|
101 |
|||
102 |
13 |
affordance::OperationBases_t createOperations( |
|
103 |
hpp::core::ProblemSolver& pSolver) { |
||
104 |
✓✗✓✗ ✓✗ |
13 |
pSolver.affordanceConfigs.add("Support", vector3_t(0.3, 0.3, 0.05)); |
105 |
✓✗✓✗ ✓✗ |
13 |
pSolver.affordanceConfigs.add("Lean", vector3_t(0.1, 0.3, 0.05)); |
106 |
✓✗✓✗ ✓✗ |
13 |
pSolver.affordanceConfigs.add("Support45", vector3_t(0.1, 0.3, 0.05)); |
107 |
✓✗✓✗ ✗✓ |
13 |
if (!pSolver.affordanceConfigs.has("Support")) |
108 |
throw std::runtime_error( |
||
109 |
"No 'Support' affordance type found Afford::createOperations ()"); |
||
110 |
const hpp::pinocchio::vector3_t& sconf = |
||
111 |
✓✗✓✗ |
13 |
pSolver.affordanceConfigs.get("Support"); |
112 |
✓✗✓✗ ✗✓ |
13 |
if (!pSolver.affordanceConfigs.has("Lean")) |
113 |
throw std::runtime_error( |
||
114 |
"No 'Lean' affordance type found in Afford::createOperations ()"); |
||
115 |
const hpp::pinocchio::vector3_t& lconf = |
||
116 |
✓✗✓✗ |
13 |
pSolver.affordanceConfigs.get("Lean"); |
117 |
✓✗✓✗ ✗✓ |
13 |
if (!pSolver.affordanceConfigs.has("Support45")) |
118 |
throw std::runtime_error( |
||
119 |
"No 'Support45' affordance type found in Afford::createOperations ()"); |
||
120 |
const hpp::pinocchio::vector3_t& s45conf = |
||
121 |
✓✗✓✗ |
13 |
pSolver.affordanceConfigs.get("Support45"); |
122 |
affordance::SupportOperationPtr_t support( |
||
123 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
26 |
new affordance::SupportOperation(sconf[0], sconf[1], sconf[2])); |
124 |
affordance::LeanOperationPtr_t lean( |
||
125 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
26 |
new affordance::LeanOperation(lconf[0], lconf[1], lconf[2])); |
126 |
affordance::Support45OperationPtr_t support45( |
||
127 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
26 |
new affordance::Support45Operation(s45conf[0], s45conf[1], s45conf[2])); |
128 |
|||
129 |
13 |
affordance::OperationBases_t operations; |
|
130 |
✓✗ | 13 |
operations.push_back(support); |
131 |
✓✗ | 13 |
operations.push_back(lean); |
132 |
✓✗ | 13 |
operations.push_back(support45); |
133 |
|||
134 |
26 |
return operations; |
|
135 |
} |
||
136 |
|||
137 |
13 |
void loadObstacleWithAffordance(hpp::core::ProblemSolver& pSolver, |
|
138 |
std::string packagename, std::string filename, |
||
139 |
std::string prefix, |
||
140 |
const affordance::OperationBases_t& operations, |
||
141 |
std::vector<double> reduceSizes) { |
||
142 |
✓✗ | 26 |
DevicePtr_t device(hpp::pinocchio::Device::create(prefix)); |
143 |
✗✓ | 13 |
if (packagename.empty()) |
144 |
hpp::pinocchio::urdf::loadModelFromString(device, 0, "", "anchor", filename, |
||
145 |
""); |
||
146 |
else |
||
147 |
✓✗✓✗ |
13 |
hpp::pinocchio::urdf::loadUrdfModel(device, "anchor", packagename, |
148 |
filename); |
||
149 |
✓✗ | 13 |
device->controlComputation(hpp::pinocchio::JOINT_POSITION); |
150 |
✓✗ | 13 |
pSolver.addObstacle(device, true, true); |
151 |
✓✗ | 26 |
std::list<std::string> obstacles = pSolver.obstacleNames(true, false); |
152 |
26 |
for (std::list<std::string>::const_iterator cit = obstacles.begin(); |
|
153 |
✓✓ | 39 |
cit != obstacles.end(); ++cit) { |
154 |
✓✗ | 13 |
if ((*cit).find(prefix) == 0) |
155 |
✓✗✓✗ |
13 |
affordanceAnalysis(pSolver, *cit, operations, reduceSizes); |
156 |
} |
||
157 |
13 |
} |
|
158 |
|||
159 |
13 |
void loadObstacleWithAffordance(hpp::core::ProblemSolver& pSolver, |
|
160 |
std::string packagename, std::string filename, |
||
161 |
std::string prefix) { |
||
162 |
✓✗ | 26 |
affordance::OperationBases_t operations = createOperations(pSolver); |
163 |
26 |
std::vector<double> reduceSizes; |
|
164 |
✓✓✓✗ |
52 |
while (reduceSizes.size() < operations.size()) reduceSizes.push_back(0.); |
165 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
13 |
return loadObstacleWithAffordance(pSolver, packagename, filename, prefix, |
166 |
26 |
operations, reduceSizes); |
|
167 |
} |
||
168 |
|||
169 |
void loadDarpa(hpp::core::ProblemSolver& pSolver) { |
||
170 |
loadObstacleWithAffordance(pSolver, std::string("hpp_environments"), |
||
171 |
std::string("multicontact/darpa"), |
||
172 |
std::string("planning")); |
||
173 |
} |
||
174 |
|||
175 |
typedef hpp::core::Container<hpp::core::AffordanceObjects_t> affMap_t; |
||
176 |
struct BindShooter { |
||
177 |
13 |
BindShooter(const std::size_t shootLimit = 10000, |
|
178 |
const std::size_t displacementLimit = 100) |
||
179 |
13 |
: shootLimit_(shootLimit), displacementLimit_(displacementLimit) {} |
|
180 |
|||
181 |
37 |
hpp::rbprm::RbPrmShooterPtr_t create( |
|
182 |
/*const hpp::pinocchio::DevicePtr_t& robot,*/ core::ProblemConstPtr_t |
||
183 |
problem, |
||
184 |
const hpp::core::ProblemSolverPtr_t problemSolver) { |
||
185 |
hpp::core::Container<hpp::core::AffordanceObjects_t> affMap_ = |
||
186 |
✓✗ | 74 |
problemSolver->affordanceObjects; |
187 |
✓✗ | 37 |
affMap_ = problemSolver->affordanceObjects; |
188 |
hpp::pinocchio::RbPrmDevicePtr_t robotcast = |
||
189 |
74 |
static_pointer_cast<hpp::pinocchio::RbPrmDevice>(problem->robot()); |
|
190 |
✗✓ | 37 |
if (affMap_.map.empty()) { |
191 |
throw std::runtime_error( |
||
192 |
"No affordances found. Unable to create shooter object."); |
||
193 |
} |
||
194 |
rbprm::RbPrmShooterPtr_t shooter = hpp::rbprm::RbPrmShooter::create( |
||
195 |
robotcast, problemSolver->problem()->collisionObstacles(), affMap_, |
||
196 |
✓✗✓✗ |
37 |
romFilter_, affFilter_, shootLimit_, displacementLimit_); |
197 |
✓✗✓✗ |
37 |
if (!so3Bounds_.empty()) shooter->BoundSO3(so3Bounds_); |
198 |
74 |
return shooter; |
|
199 |
} |
||
200 |
|||
201 |
13 |
hpp::core::PathValidationPtr_t createPathValidation( |
|
202 |
const hpp::pinocchio::DevicePtr_t& robot, |
||
203 |
const hpp::pinocchio::value_type& val, |
||
204 |
const hpp::core::ProblemSolverPtr_t problemSolver) { |
||
205 |
hpp::pinocchio::RbPrmDevicePtr_t robotcast = |
||
206 |
26 |
static_pointer_cast<hpp::pinocchio::RbPrmDevice>(robot); |
|
207 |
hpp::core::Container<hpp::core::AffordanceObjects_t> affMap_ = |
||
208 |
✓✗ | 26 |
problemSolver->affordanceObjects; |
209 |
✗✓ | 13 |
if (affMap_.map.empty()) { |
210 |
throw std::runtime_error( |
||
211 |
"No affordances found. Unable to create Path Validaton object."); |
||
212 |
} |
||
213 |
hpp::rbprm::RbPrmValidationPtr_t validation( |
||
214 |
13 |
hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, affFilter_, |
|
215 |
✓✗ | 26 |
affMap_)); |
216 |
hpp::rbprm::RbPrmPathValidationPtr_t collisionChecking = |
||
217 |
✓✗ | 26 |
hpp::rbprm::RbPrmPathValidation::create(robot, val); |
218 |
✓✗ | 13 |
collisionChecking->add(validation); |
219 |
26 |
problemSolver->problem()->configValidation( |
|
220 |
✓✗ | 26 |
core::ConfigValidations::create()); |
221 |
✓✗ | 13 |
problemSolver->problem()->configValidations()->add(validation); |
222 |
26 |
return collisionChecking; |
|
223 |
} |
||
224 |
|||
225 |
hpp::core::PathValidationPtr_t createDynamicPathValidation( |
||
226 |
const hpp::pinocchio::DevicePtr_t& robot, |
||
227 |
const hpp::pinocchio::value_type& val, |
||
228 |
const hpp::core::ProblemSolverPtr_t problemSolver) { |
||
229 |
hpp::pinocchio::RbPrmDevicePtr_t robotcast = |
||
230 |
static_pointer_cast<hpp::pinocchio::RbPrmDevice>(robot); |
||
231 |
hpp::core::Container<hpp::core::AffordanceObjects_t> affMap_ = |
||
232 |
problemSolver->affordanceObjects; |
||
233 |
if (affMap_.map.empty()) { |
||
234 |
throw std::runtime_error( |
||
235 |
"No affordances found. Unable to create Path Validaton object."); |
||
236 |
} |
||
237 |
hpp::rbprm::RbPrmValidationPtr_t validation( |
||
238 |
hpp::rbprm::RbPrmValidation::create(robotcast, romFilter_, affFilter_, |
||
239 |
affMap_)); |
||
240 |
hpp::rbprm::DynamicPathValidationPtr_t collisionChecking = |
||
241 |
hpp::rbprm::DynamicPathValidation::create(robot, val); |
||
242 |
collisionChecking->add(validation); |
||
243 |
problemSolver->problem()->configValidation( |
||
244 |
core::ConfigValidations::create()); |
||
245 |
problemSolver->problem()->configValidations()->add(validation); |
||
246 |
// build the dynamicValidation : |
||
247 |
double sizeFootX, sizeFootY, mass, mu; |
||
248 |
bool rectangularContact; |
||
249 |
sizeFootX = problemSolver->problem() |
||
250 |
->getParameter(std::string("DynamicPlanner/sizeFootX")) |
||
251 |
.floatValue() / |
||
252 |
2.; |
||
253 |
sizeFootY = problemSolver->problem() |
||
254 |
->getParameter(std::string("DynamicPlanner/sizeFootY")) |
||
255 |
.floatValue() / |
||
256 |
2.; |
||
257 |
if (sizeFootX > 0. && sizeFootY > 0.) |
||
258 |
rectangularContact = 1; |
||
259 |
else |
||
260 |
rectangularContact = 0; |
||
261 |
mass = robot->mass(); |
||
262 |
mu = problemSolver->problem() |
||
263 |
->getParameter(std::string("DynamicPlanner/friction")) |
||
264 |
.floatValue(); |
||
265 |
hppDout(notice, "mu define in python : " << mu); |
||
266 |
DynamicValidationPtr_t dynamicVal = DynamicValidation::create( |
||
267 |
rectangularContact, sizeFootX, sizeFootY, mass, mu, robot); |
||
268 |
collisionChecking->addDynamicValidator(dynamicVal); |
||
269 |
|||
270 |
return collisionChecking; |
||
271 |
} |
||
272 |
|||
273 |
std::vector<std::string> romFilter_; |
||
274 |
std::map<std::string, std::vector<std::string> > affFilter_; |
||
275 |
std::size_t shootLimit_; |
||
276 |
std::size_t displacementLimit_; |
||
277 |
std::vector<double> so3Bounds_; |
||
278 |
}; |
||
279 |
|||
280 |
std::vector<double> addSo3LimitsHyQ() { |
||
281 |
std::vector<double> res; |
||
282 |
res.push_back(-0.4); |
||
283 |
res.push_back(0.4); |
||
284 |
res.push_back(-0.3); |
||
285 |
res.push_back(0.3); |
||
286 |
res.push_back(-0.3); |
||
287 |
res.push_back(0.3); |
||
288 |
return res; |
||
289 |
} |
||
290 |
|||
291 |
13 |
hpp::core::ProblemSolverPtr_t configureRbprmProblemSolverForSupportLimbs( |
|
292 |
const DevicePtr_t& robot, BindShooter& bShooter) { |
||
293 |
✓✗ | 13 |
hpp::core::ProblemSolverPtr_t ps = hpp::core::ProblemSolver::create(); |
294 |
|||
295 |
/*bind shooter init*/ |
||
296 |
hpp::pinocchio::RbPrmDevicePtr_t robotcast = |
||
297 |
26 |
static_pointer_cast<hpp::pinocchio::RbPrmDevice>(robot); |
|
298 |
13 |
std::vector<std::string> affNames; |
|
299 |
✓✗✓✗ |
13 |
affNames.push_back(std::string("Support")); |
300 |
30 |
for (std::map<std::string, DevicePtr_t>::const_iterator cit = |
|
301 |
13 |
robotcast->robotRoms_.begin(); |
|
302 |
✓✓ | 73 |
cit != robotcast->robotRoms_.end(); ++cit) { |
303 |
✓✗✓✗ |
30 |
bShooter.affFilter_.insert(std::make_pair(cit->first, affNames)); |
304 |
✓✗ | 30 |
bShooter.romFilter_.push_back(cit->first); |
305 |
} |
||
306 |
/*END bind shoorther init*/ |
||
307 |
|||
308 |
✓✗ | 13 |
ps->robot(robot); |
309 |
✓✗✓✗ ✓✗✓✗ |
26 |
ps->configurationShooters.add( |
310 |
"RbprmShooter", |
||
311 |
boost::bind(&BindShooter::create, boost::ref(bShooter), _1, ps)); |
||
312 |
✓✗✓✗ ✓✗✓✗ |
26 |
ps->pathValidations.add("RbprmPathValidation", |
313 |
boost::bind(&BindShooter::createPathValidation, |
||
314 |
boost::ref(bShooter), _1, _2, ps)); |
||
315 |
✓✗✓✗ ✓✗✓✗ |
26 |
ps->pathValidations.add("RbprmDynamicPathValidation", |
316 |
boost::bind(&BindShooter::createDynamicPathValidation, |
||
317 |
boost::ref(bShooter), _1, _2, ps)); |
||
318 |
✓✗✓✗ ✓✗ |
13 |
ps->pathPlanners.add("DynamicPlanner", DynamicPlanner::createWithRoadmap); |
319 |
✓✗✓✗ ✓✗ |
13 |
ps->steeringMethods.add("RBPRMKinodynamic", |
320 |
SteeringMethodKinodynamic::create); |
||
321 |
✓✗✓✗ ✓✗ |
13 |
ps->pathOptimizers.add("RandomShortcutDynamic", |
322 |
RandomShortcutDynamic::create); |
||
323 |
✓✗✓✗ ✓✗ |
13 |
ps->pathOptimizers.add("OrientedPathOptimizer", |
324 |
OrientedPathOptimizer::create); |
||
325 |
26 |
return ps; |
|
326 |
} |
||
327 |
#endif // TOOLSOBSTACLE_HH |
| Generated by: GCOVR (Version 4.2) |