GCC Code Coverage Report

Directory: ./
File: src/problem-solver.cc
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 274 616 44.5%
Branches: 252 1186 21.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
4 //
5
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // 1. Redistributions of source code must retain the above copyright
11 // notice, this list of conditions and the following disclaimer.
12 //
13 // 2. Redistributions in binary form must reproduce the above copyright
14 // notice, this list of conditions and the following disclaimer in the
15 // documentation and/or other materials provided with the distribution.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28 // DAMAGE.
29
30 #include <hpp/fcl/collision_utility.h>
31
32 #include <hpp/constraints/differentiable-function.hh>
33 #include <hpp/constraints/implicit.hh>
34 #include <hpp/constraints/locked-joint.hh>
35 #include <hpp/constraints/solver/by-substitution.hh>
36 #include <hpp/core/bi-rrt-planner.hh>
37 #include <hpp/core/collision-validation.hh>
38 #include <hpp/core/config-projector.hh>
39 #include <hpp/core/configuration-shooter/gaussian.hh>
40 #include <hpp/core/configuration-shooter/uniform-tpl.hh>
41 #include <hpp/core/configuration-shooter/uniform.hh>
42 #include <hpp/core/constraint-set.hh>
43 #include <hpp/core/continuous-validation/dichotomy.hh>
44 #include <hpp/core/continuous-validation/progressive.hh>
45 #include <hpp/core/diffusing-planner.hh>
46 #include <hpp/core/distance-between-objects.hh>
47 #include <hpp/core/distance/reeds-shepp.hh>
48 #include <hpp/core/joint-bound-validation.hh>
49 #include <hpp/core/kinodynamic-distance.hh>
50 #include <hpp/core/path-optimization/partial-shortcut.hh>
51 #include <hpp/core/path-optimization/random-shortcut.hh>
52 #include <hpp/core/path-optimization/rs-time-parameterization.hh>
53 #include <hpp/core/path-optimization/simple-shortcut.hh>
54 #include <hpp/core/path-optimization/simple-time-parameterization.hh>
55 #include <hpp/core/path-planner/bi-rrt-star.hh>
56 #include <hpp/core/path-planner/k-prm-star.hh>
57 #include <hpp/core/path-projector/dichotomy.hh>
58 #include <hpp/core/path-projector/global.hh>
59 #include <hpp/core/path-projector/progressive.hh>
60 #include <hpp/core/path-projector/recursive-hermite.hh>
61 #include <hpp/core/path-validation-report.hh>
62 #include <hpp/core/path-validation/discretized-collision-checking.hh>
63 #include <hpp/core/path-validation/discretized-joint-bound.hh>
64 #include <hpp/core/path-vector.hh>
65 #include <hpp/core/problem-solver.hh>
66 #include <hpp/core/problem-target/goal-configurations.hh>
67 #include <hpp/core/problem-target/task-target.hh>
68 #include <hpp/core/roadmap.hh>
69 #include <hpp/core/steering-method/dubins.hh>
70 #include <hpp/core/steering-method/hermite.hh>
71 #include <hpp/core/steering-method/reeds-shepp.hh>
72 #include <hpp/core/steering-method/snibud.hh>
73 #include <hpp/core/steering-method/spline.hh>
74 #include <hpp/core/steering-method/steering-kinodynamic.hh>
75 #include <hpp/core/steering-method/straight.hh>
76 #include <hpp/core/visibility-prm-planner.hh>
77 #include <hpp/core/weighed-distance.hh>
78 #include <hpp/pinocchio/collision-object.hh>
79 #include <hpp/pinocchio/joint-collection.hh>
80 #include <hpp/util/debug.hh>
81 #include <hpp/util/exception-factory.hh>
82 #include <iterator>
83 #include <pinocchio/algorithm/frames.hpp>
84 #include <pinocchio/multibody/fcl.hpp>
85 #include <pinocchio/multibody/geometry.hpp>
86
87 #include "../src/path-validation/no-validation.hh"
88
89 namespace hpp {
90 namespace core {
91 using pinocchio::GeomIndex;
92
93 using pinocchio::CollisionObject;
94 using pinocchio::Data;
95 using pinocchio::DataPtr_t;
96 using pinocchio::GeomData;
97 using pinocchio::GeomDataPtr_t;
98 using pinocchio::GeomModel;
99 using pinocchio::GeomModelPtr_t;
100 using pinocchio::Model;
101 using pinocchio::ModelPtr_t;
102
103 namespace {
104 template <typename Container>
105 ✗ void remove(Container& vector, std::size_t pos) {
106 ✗ assert(pos < vector.size());
107 ✗ vector.erase(std::next(vector.begin(), pos));
108 }
109
110 ✗ void remove(ObjectStdVector_t& vector, const GeomIndex& i) {
111 ✗ ObjectStdVector_t::iterator it = std::find_if(
112 vector.begin(), vector.end(),
113 ✗ [i](const CollisionObjectPtr_t& co) { return co->indexInModel() == i; });
114 ✗ if (it != vector.end()) vector.erase(it);
115 }
116 } // namespace
117
118 pathValidation::DiscretizedPtr_t
119 ✗ createDiscretizedJointBoundAndCollisionChecking(const DevicePtr_t& robot,
120 const value_type& stepSize) {
121 using namespace pathValidation;
122 ✗ return Discretized::create(stepSize, {
123 ✗ JointBoundValidation::create(robot),
124 ✗ CollisionValidation::create(robot),
125 ✗ });
126 }
127
128 ✗ configurationShooter::GaussianPtr_t createGaussianConfigShooter(
129 const ProblemConstPtr_t& p) {
130 configurationShooter::GaussianPtr_t ptr(
131 ✗ configurationShooter::Gaussian::create(p->robot()));
132 ✗ static const std::string center = "ConfigurationShooter/Gaussian/center";
133 static const std::string useVel =
134 ✗ "ConfigurationShooter/Gaussian/useRobotVelocity";
135 static const std::string stdDev =
136 ✗ "ConfigurationShooter/Gaussian/standardDeviation";
137 ✗ ptr->center(p->getParameter(center).vectorValue());
138 ✗ if (p->getParameter(useVel).boolValue()) {
139 ✗ ptr->sigmas(p->robot()->currentVelocity());
140 ✗ } else if (p->parameters.has(stdDev))
141 ✗ ptr->sigma(p->getParameter(stdDev).floatValue());
142 ✗ return ptr;
143 }
144
145 7 configurationShooter::UniformPtr_t createUniformConfigShooter(
146 const ProblemConstPtr_t& p) {
147 configurationShooter::UniformPtr_t ptr(
148 7 configurationShooter::Uniform::create(p->robot()));
149 7 ptr->sampleExtraDOF(
150
3/6
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
 7 p->getParameter("ConfigurationShooter/sampleExtraDOF").boolValue());
151 7 return ptr;
152 }
153
154 template <class generator_t>
155 ✗ typename configurationShooter::UniformTpl<generator_t>::Ptr_t createUniformTpl(
156 const ProblemConstPtr_t& p) {
157 typedef configurationShooter::UniformTpl<generator_t> shooter_t;
158 ✗ typename shooter_t::Ptr_t ptr(shooter_t::create(p->robot()));
159 ✗ return ptr;
160 }
161
162 11 ProblemSolverPtr_t ProblemSolver::create() {
163
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 return ProblemSolverPtr_t(new ProblemSolver());
164 }
165
166 11 ProblemSolver::ProblemSolver()
167 11 : constraints_(),
168 11 robot_(),
169 11 problem_(),
170 11 pathPlanner_(),
171 11 roadmap_(),
172 11 paths_(),
173
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 pathProjectorType_("None"),
174 11 pathProjectorTolerance_(0.2),
175
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 pathPlannerType_("DiffusingPlanner"),
176
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 target_(problemTarget::GoalConfigurations::create(ProblemPtr_t())),
177
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 initConf_(),
178 11 goalConfigurations_(),
179
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 robotType_("hpp::pinocchio::Device"),
180
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 configurationShooterType_("Uniform"),
181
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 distanceType_("WeighedDistance"),
182
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 steeringMethodType_("Straight"),
183 11 pathOptimizerTypes_(),
184 11 pathOptimizers_(),
185
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 pathValidationType_("Discretized"),
186 11 pathValidationTolerance_(0.05),
187 11 configValidationTypes_(),
188 11 collisionObstacles_(),
189 11 distanceObstacles_(),
190
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 11 obstacleRModel_(new Model()),
191 11 obstacleRData_(),
192
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 11 obstacleModel_(new GeomModel()),
193
3/6
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
 11 obstacleData_(new GeomData(*obstacleModel_)),
194 11 errorThreshold_(1e-4),
195 11 maxIterProjection_(20),
196 11 maxIterPathPlanning_(std::numeric_limits<unsigned long int>::max()),
197 11 timeOutPathPlanning_(std::numeric_limits<double>::infinity()),
198
199 11 passiveDofsMap_(),
200 11 comcMap_(),
201 22 distanceBetweenObjects_() {
202
4/8
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 11 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 11 times.
✗ Branch 13 not taken.
 22 obstacleRModel_->addFrame(::pinocchio::Frame(
203
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 "obstacle_frame", 0, 0, Transform3f::Identity(), ::pinocchio::BODY));
204
3/6
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
 11 obstacleRData_.reset(new Data(*obstacleRModel_));
205
206
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 robots.add(robotType_, Device_t::create);
207
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathPlanners.add("DiffusingPlanner", DiffusingPlanner::createWithRoadmap);
208
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathPlanners.add("VisibilityPrmPlanner",
209 VisibilityPrmPlanner::createWithRoadmap);
210
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathPlanners.add("BiRRTPlanner", BiRRTPlanner::createWithRoadmap);
211
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathPlanners.add("kPRM*", pathPlanner::kPrmStar::createWithRoadmap);
212
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathPlanners.add("BiRRT*", pathPlanner::BiRrtStar::createWithRoadmap);
213
214
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 configurationShooters.add("Uniform", createUniformConfigShooter);
215
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 configurationShooters.add("Gaussian", createGaussianConfigShooter);
216
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 configurationShooters.add("UniformSeedable",
217 createUniformTpl<std::default_random_engine>);
218
219
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 distances.add("Weighed", WeighedDistance::createFromProblem);
220
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 11 times.
✗ Branch 9 not taken.
 11 distances.add(
221 "ReedsShepp",
222 ✗ std::bind(
223
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 static_cast<distance::ReedsSheppPtr_t (*)(const ProblemConstPtr_t&)>(
224 distance::ReedsShepp::create),
225 std::placeholders::_1));
226
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 distances.add("Kinodynamic", KinodynamicDistance::createFromProblem);
227
228
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 steeringMethods.add("Straight", steeringMethod::Straight::create);
229
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 steeringMethods.add("ReedsShepp",
230 steeringMethod::ReedsShepp::createWithGuess);
231
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 steeringMethods.add("Kinodynamic", steeringMethod::Kinodynamic::create);
232
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 steeringMethods.add("Dubins", steeringMethod::Dubins::createWithGuess);
233
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 steeringMethods.add("Snibud", steeringMethod::Snibud::createWithGuess);
234
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 steeringMethods.add("Hermite", steeringMethod::Hermite::create);
235
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 steeringMethods.add("SplineBezier3",
236 steeringMethod::Spline<path::BernsteinBasis, 3>::create);
237
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 steeringMethods.add("SplineBezier5",
238 steeringMethod::Spline<path::BernsteinBasis, 5>::create);
239
240 // Store path optimization methods in map.
241
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathOptimizers.add("RandomShortcut",
242 pathOptimization::RandomShortcut::create);
243
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathOptimizers.add("SimpleShortcut",
244 pathOptimization::SimpleShortcut::create);
245
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathOptimizers.add("PartialShortcut",
246 pathOptimization::PartialShortcut::create);
247
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathOptimizers.add("SimpleTimeParameterization",
248 pathOptimization::SimpleTimeParameterization::create);
249
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathOptimizers.add("RSTimeParameterization",
250 pathOptimization::RSTimeParameterization::create);
251
252 // Store path validation methods in map.
253
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathValidations.add("NoValidation", pathValidation::NoValidation::create);
254
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathValidations.add("Discretized",
255 pathValidation::createDiscretizedCollisionChecking);
256
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathValidations.add("DiscretizedCollision",
257 pathValidation::createDiscretizedCollisionChecking);
258
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathValidations.add("DiscretizedJointBound",
259 pathValidation::createDiscretizedJointBound);
260
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathValidations.add("DiscretizedCollisionAndJointBound",
261 createDiscretizedJointBoundAndCollisionChecking);
262
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathValidations.add("Progressive", continuousValidation::Progressive::create);
263
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathValidations.add("Dichotomy", continuousValidation::Dichotomy::create);
264
265 // Store config validation methods in map.
266
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 configValidations.add("CollisionValidation", CollisionValidation::create);
267
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 configValidations.add("JointBoundValidation", JointBoundValidation::create);
268
269 // Set default config validation methods.
270
2/4
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
 11 configValidationTypes_.push_back("CollisionValidation");
271
2/4
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11 times.
✗ Branch 6 not taken.
 11 configValidationTypes_.push_back("JointBoundValidation");
272
273 // Store path projector methods in map.
274
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathProjectors.add("None", [](const ProblemConstPtr_t&, const value_type&) {
275 7 return PathProjectorPtr_t();
276 });
277
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathProjectors.add("Progressive",
278 ✗ [](const ProblemConstPtr_t& p, const value_type& v) {
279 ✗ return pathProjector::Progressive ::create(p, v);
280 });
281
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathProjectors.add("Dichotomy",
282 ✗ [](const ProblemConstPtr_t& p, const value_type& v) {
283 ✗ return pathProjector::Dichotomy ::create(p, v);
284 });
285
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathProjectors.add("Global",
286 ✗ [](const ProblemConstPtr_t& p, const value_type& v) {
287 ✗ return pathProjector::Global ::create(p, v);
288 });
289
2/4
✓ Branch 3 taken 11 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 11 times.
✗ Branch 7 not taken.
 11 pathProjectors.add("RecursiveHermite",
290 ✗ [](const ProblemConstPtr_t& p, const value_type& v) {
291 ✗ return pathProjector::RecursiveHermite::create(p, v);
292 });
293 11 }
294
295 8 ProblemSolver::~ProblemSolver() {}
296
297 7 void ProblemSolver::distanceType(const std::string& type) {
298
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (!distances.has(type)) {
299 ✗ throw std::runtime_error(std::string("No distance method with name ") +
300 ✗ type);
301 }
302 7 distanceType_ = type;
303 // TODO
304 // initDistance ();
305 7 }
306
307 7 void ProblemSolver::steeringMethodType(const std::string& type) {
308
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (!steeringMethods.has(type)) {
309 ✗ throw std::runtime_error(std::string("No steering method with name ") +
310 ✗ type);
311 }
312 7 steeringMethodType_ = type;
313
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 if (problem_) initSteeringMethod();
314 7 }
315
316 ✗ void ProblemSolver::pathPlannerType(const std::string& type) {
317 ✗ if (!pathPlanners.has(type)) {
318 ✗ throw std::runtime_error(std::string("No path planner with name ") + type);
319 }
320 ✗ pathPlannerType_ = type;
321 }
322
323 ✗ void ProblemSolver::configurationShooterType(const std::string& type) {
324 ✗ if (!configurationShooters.has(type)) {
325 ✗ throw std::runtime_error(
326 ✗ std::string("No configuration shooter with name ") + type);
327 }
328 ✗ configurationShooterType_ = type;
329 ✗ if (robot_ && problem_) {
330 ✗ problem_->configurationShooter(
331 ✗ configurationShooters.get(configurationShooterType_)(problem_));
332 }
333 }
334
335 ✗ void ProblemSolver::addPathOptimizer(const std::string& type) {
336 ✗ if (!pathOptimizers.has(type)) {
337 ✗ throw std::runtime_error(std::string("No path optimizer with name ") +
338 ✗ type);
339 }
340 ✗ pathOptimizerTypes_.push_back(type);
341 }
342
343 ✗ void ProblemSolver::clearPathOptimizers() {
344 ✗ pathOptimizerTypes_.clear();
345 ✗ pathOptimizers_.clear();
346 }
347
348 7 void ProblemSolver::optimizePath(PathVectorPtr_t path) {
349 7 createPathOptimizers();
350 7 for (PathOptimizers_t::const_iterator it = pathOptimizers_.begin();
351
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 7 times.
 7 it != pathOptimizers_.end(); ++it) {
352 ✗ path = (*it)->optimize(path);
353 ✗ paths_.push_back(path);
354 }
355 7 }
356
357 7 void ProblemSolver::pathValidationType(const std::string& type,
358 const value_type& tolerance) {
359
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (!pathValidations.has(type)) {
360 ✗ throw std::runtime_error(std::string("No path validation method with "
361 ✗ "name ") +
362 ✗ type);
363 }
364 7 pathValidationType_ = type;
365 7 pathValidationTolerance_ = tolerance;
366 // If a robot is present, set path validation method
367
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 if (robot_ && problem_) {
368 7 initPathValidation();
369 }
370 7 }
371
372 // Initialize path validation with obstacles of problem
373 17 static void setObstaclesToPathValidation(const ProblemPtr_t& problem,
374 const PathValidationPtr_t& pv) {
375 // Insert obstacles in path validation object
376 shared_ptr<ObstacleUserInterface> oui =
377 17 HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, pv);
378
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 if (oui) {
379 17 for (ObjectStdVector_t::const_iterator it =
380
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 problem->collisionObstacles().begin();
381
2/4
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 17 times.
 17 it != problem->collisionObstacles().end(); ++it)
382 ✗ oui->addObstacle(*it);
383 }
384 17 }
385
386 17 void ProblemSolver::initPathValidation() {
387
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 17 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 17 if (!problem_) throw std::runtime_error("The problem is not defined.");
388
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 PathValidationPtr_t pathValidation = pathValidations.get(pathValidationType_)(
389
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 robot_, pathValidationTolerance_);
390
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 setObstaclesToPathValidation(problem_, pathValidation);
391
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 problem_->pathValidation(pathValidation);
392 17 }
393
394 11 void ProblemSolver::initConfigValidation() {
395
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 11 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 11 if (!robot_) throw std::logic_error("You must provide a robot first");
396
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 11 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 11 if (!problem_) throw std::runtime_error("The problem is not defined.");
397 11 problem_->clearConfigValidations();
398 11 for (ConfigValidationTypes_t::const_iterator it =
399 11 configValidationTypes_.begin();
400
2/2
✓ Branch 3 taken 21 times.
✓ Branch 4 taken 11 times.
 32 it != configValidationTypes_.end(); ++it) {
401
2/4
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
 21 ConfigValidationPtr_t configValidation = configValidations.get(*it)(robot_);
402
1/2
✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
 21 problem_->addConfigValidation(configValidation);
403 21 }
404 11 }
405
406 ✗ void ProblemSolver::initValidations() {
407 ✗ initPathValidation();
408 ✗ initConfigValidation();
409 ✗ problem_->collisionObstacles(collisionObstacles_);
410 }
411
412 ✗ void ProblemSolver::pathProjectorType(const std::string& type,
413 const value_type& tolerance) {
414 ✗ if (!pathProjectors.has(type)) {
415 ✗ throw std::runtime_error(std::string("No path projector method with "
416 ✗ "name ") +
417 ✗ type);
418 }
419 ✗ pathProjectorType_ = type;
420 ✗ pathProjectorTolerance_ = tolerance;
421 // If a robot is present, set path projector method
422 ✗ if (robot_ && problem_) {
423 ✗ initPathProjector();
424 }
425 }
426
427 ✗ void ProblemSolver::addConfigValidationBuilder(
428 const std::string& type, const ConfigValidationBuilder_t& builder) {
429 ✗ configValidations.add(type, builder);
430 }
431
432 1 void ProblemSolver::addConfigValidation(const std::string& type) {
433
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!configValidations.has(type)) {
434 ✗ throw std::runtime_error(std::string("No config validation method with "
435 ✗ "name ") +
436 ✗ type);
437 }
438 1 configValidationTypes_.push_back(type);
439
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 1 if (!problem_) throw std::runtime_error("The problem is not defined.");
440 // If a robot is present, set config validation methods
441 1 initConfigValidation();
442 // Set obstacles
443 1 problem_->collisionObstacles(collisionObstacles_);
444 1 }
445
446 1 void ProblemSolver::clearConfigValidations() {
447 1 configValidationTypes_.clear();
448 1 problem_->clearConfigValidations();
449 1 }
450
451 ✗ void ProblemSolver::robotType(const std::string& type) { robotType_ = type; }
452
453 ✗ const std::string& ProblemSolver::robotType() const { return robotType_; }
454
455 2 DevicePtr_t ProblemSolver::createRobot(const std::string& name) {
456
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 RobotBuilder_t factory(robots.get(robotType_));
457
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 assert(factory);
458
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 return factory(name);
459 2 }
460
461 10 void ProblemSolver::robot(const DevicePtr_t& robot) {
462 10 robot_ = robot;
463
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 constraints_ = ConstraintSet::create(robot_, "Default constraint set");
464 // Reset obstacles
465
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 obstacleRModel_.reset(new Model());
466
3/6
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 20 obstacleRModel_->addFrame(::pinocchio::Frame(
467
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 "obstacle_frame", 0, 0, Transform3f::Identity(), ::pinocchio::BODY));
468
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 10 obstacleRData_.reset(new Data(*obstacleRModel_));
469
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 obstacleModel_.reset(new GeomModel());
470
1/2
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
 10 obstacleData_.reset(new GeomData(*obstacleModel_));
471 10 resetProblem();
472 10 }
473
474 1 const DevicePtr_t& ProblemSolver::robot() const { return robot_; }
475
476 7 void ProblemSolver::initConfig(ConfigurationIn_t config) { initConf_ = config; }
477
478 ✗ const Configurations_t& ProblemSolver::goalConfigs() const {
479 ✗ return goalConfigurations_;
480 }
481
482 7 void ProblemSolver::addGoalConfig(ConfigurationIn_t config) {
483
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 goalConfigurations_.push_back(config);
484 7 }
485
486 ✗ void ProblemSolver::resetGoalConfigs() { goalConfigurations_.clear(); }
487
488 ✗ void ProblemSolver::setGoalConstraints(
489 const NumericalConstraints_t& constraints) {
490 problemTarget::TaskTargetPtr_t tt(
491 ✗ problemTarget::TaskTarget::create(problem_));
492 ConstraintSetPtr_t cs(
493 ✗ ConstraintSet::create(problem_->robot(), "goalConstraints"));
494 ConfigProjectorPtr_t cp(ConfigProjector::create(
495 ✗ robot_, "Goal ConfigProjector", errorThreshold_, maxIterProjection_));
496 ✗ cs->addConstraint(cp);
497 ✗ for (auto c : constraints) {
498 ✗ cp->add(c);
499 }
500 ✗ tt->constraints(cs);
501 ✗ target_ = tt;
502 }
503
504 ✗ void ProblemSolver::resetGoalConstraints() { target_.reset(); }
505
506 ✗ void ProblemSolver::addConstraint(const ConstraintPtr_t& constraint) {
507 ✗ if (robot_)
508 ✗ constraints_->addConstraint(constraint);
509 else
510 ✗ throw std::logic_error("Cannot add constraint while robot is not set");
511 }
512
513 ✗ void ProblemSolver::resetConstraints() {
514 ✗ if (robot_) {
515 ✗ constraints_ = ConstraintSet::create(robot_, "Default constraint set");
516 ✗ if (problem_) {
517 ✗ problem_->constraints(constraints_);
518 }
519 }
520 }
521
522 ✗ void ProblemSolver::addNumericalConstraintToConfigProjector(
523 const std::string& configProjName, const std::string& constraintName,
524 const std::size_t priority) {
525 ✗ if (!robot_) {
526 hppDout(error, "Cannot add constraint while robot is not set");
527 }
528 ✗ ConfigProjectorPtr_t configProjector = constraints_->configProjector();
529 ✗ if (!configProjector) {
530 ✗ configProjector = ConfigProjector::create(
531 ✗ robot_, configProjName, errorThreshold_, maxIterProjection_);
532 ✗ constraints_->addConstraint(configProjector);
533 }
534 ✗ if (!numericalConstraints.has(constraintName)) {
535 ✗ std::stringstream ss;
536 ✗ ss << "Function " << constraintName << " does not exists";
537 ✗ throw std::invalid_argument(ss.str());
538 }
539 ✗ configProjector->add(numericalConstraints.get(constraintName), priority);
540 }
541
542 ✗ void ProblemSolver::comparisonType(const std::string& name,
543 const ComparisonTypes_t types) {
544 ✗ constraints::ImplicitPtr_t nc;
545 ✗ if (numericalConstraints.has(name))
546 ✗ nc = numericalConstraints.get(name);
547 else
548 ✗ throw std::runtime_error(name + std::string(" is not a numerical "
549 ✗ "constraint."));
550 ✗ nc->comparisonType(types);
551 }
552
553 ✗ void ProblemSolver::comparisonType(const std::string& name,
554 const ComparisonType& type) {
555 ✗ constraints::ImplicitPtr_t nc;
556 ✗ if (numericalConstraints.has(name))
557 ✗ nc = numericalConstraints.get(name);
558 else
559 ✗ throw std::runtime_error(name + std::string(" is not a numerical "
560 ✗ "constraint."));
561 ✗ ComparisonTypes_t eqtypes(nc->function().outputDerivativeSize(), type);
562 ✗ nc->comparisonType(eqtypes);
563 }
564
565 ✗ ComparisonTypes_t ProblemSolver::comparisonType(const std::string& name) const {
566 ✗ constraints::ImplicitPtr_t nc;
567 ✗ if (numericalConstraints.has(name))
568 ✗ nc = numericalConstraints.get(name);
569 else
570 ✗ throw std::runtime_error(name + std::string(" is not a numerical "
571 ✗ "constraint."));
572 ✗ return nc->comparisonType();
573 }
574
575 ✗ void ProblemSolver::computeValueAndJacobian(
576 const Configuration_t& configuration, vector_t& value,
577 matrix_t& jacobian) const {
578 ✗ if (!robot()) throw std::runtime_error("No robot loaded");
579 ✗ ConfigProjectorPtr_t configProjector(constraints()->configProjector());
580 ✗ if (!configProjector) {
581 ✗ throw std::runtime_error("No constraints have assigned.");
582 }
583 // resize value and Jacobian
584 ✗ value.resize(configProjector->solver().dimension());
585 ✗ size_type rows = configProjector->solver().reducedDimension();
586 ✗ jacobian.resize(rows, configProjector->numberFreeVariables());
587 ✗ configProjector->computeValueAndJacobian(configuration, value, jacobian);
588 }
589
590 ✗ void ProblemSolver::maxIterProjection(size_type iterations) {
591 ✗ maxIterProjection_ = iterations;
592 ✗ if (constraints_ && constraints_->configProjector()) {
593 ✗ constraints_->configProjector()->maxIterations(iterations);
594 }
595 }
596
597 7 void ProblemSolver::maxIterPathPlanning(size_type iterations) {
598 7 maxIterPathPlanning_ = iterations;
599
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
 7 if (pathPlanner_) pathPlanner_->maxIterations(maxIterPathPlanning_);
600 7 }
601
602 ✗ void ProblemSolver::errorThreshold(const value_type& threshold) {
603 ✗ errorThreshold_ = threshold;
604 ✗ if (constraints_ && constraints_->configProjector()) {
605 ✗ constraints_->configProjector()->errorThreshold(threshold);
606 }
607 }
608
609 10 void ProblemSolver::resetProblem() {
610
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 initializeProblem(Problem::create(robot_));
611 10 }
612
613 10 void ProblemSolver::initializeProblem(ProblemPtr_t problem) {
614 10 problem_ = problem;
615 10 resetRoadmap();
616 // Set constraints
617 10 problem_->constraints(constraints_);
618 // Set path validation method
619 10 initPathValidation();
620 // Set config validation methods
621 10 initConfigValidation();
622 // Set obstacles
623 10 problem_->collisionObstacles(collisionObstacles_);
624 // Distance to obstacles
625 distanceBetweenObjects_ =
626
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 DistanceBetweenObjectsPtr_t(new DistanceBetweenObjects(robot_));
627 10 distanceBetweenObjects_->obstacles(distanceObstacles_);
628 10 }
629
630 7 void ProblemSolver::initProblem() {
631
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 7 if (!problem_) throw std::runtime_error("The problem is not defined.");
632 // Set shooter
633
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 14 problem_->configurationShooter(
634
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 14 configurationShooters.get(configurationShooterType_)(problem_));
635 // Set steeringMethod
636
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 initSteeringMethod();
637
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 PathPlannerBuilder_t createPlanner = pathPlanners.get(pathPlannerType_);
638
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 pathPlanner_ = createPlanner(problem_, roadmap_);
639
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 pathPlanner_->maxIterations(maxIterPathPlanning_);
640
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 pathPlanner_->timeOut(timeOutPathPlanning_);
641
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 roadmap_ = pathPlanner_->roadmap();
642 /// create Path projector
643
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 initPathProjector();
644 /// create Path optimizer
645 // Reset init and goal configurations
646
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
 7 problem_->initConfig(initConf_);
647 problemTarget::TaskTargetPtr_t tt(
648 7 HPP_DYNAMIC_PTR_CAST(problemTarget::TaskTarget, target_));
649
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 7 times.
 7 if (!goalConfigurations_.empty() && tt) {
650 ✗ throw std::logic_error(
651 "The goal is defined by goal configurations and"
652 " by a task at the same time. This is not"
653 ✗ " valid.");
654 }
655
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 if (!tt) {
656 // Create a goal with configurations
657 problemTarget::GoalConfigurationsPtr_t gc(
658
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 problemTarget::GoalConfigurations::create(problem_));
659 7 for (Configurations_t::const_iterator itConfig =
660 7 goalConfigurations_.begin();
661
2/2
✓ Branch 2 taken 7 times.
✓ Branch 3 taken 7 times.
 14 itConfig != goalConfigurations_.end(); ++itConfig) {
662
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 gc->addConfiguration(*itConfig);
663 }
664 7 target_ = gc;
665 7 }
666
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 initProblemTarget();
667 7 }
668
669 ✗ void ProblemSolver::problem(ProblemPtr_t problem) { problem_ = problem; }
670
671 17 void ProblemSolver::resetRoadmap() {
672
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 17 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 17 if (!problem_) throw std::runtime_error("The problem is not defined.");
673 17 roadmap_ = Roadmap::create(problem_->distance(), problem_->robot());
674 17 }
675
676 7 void ProblemSolver::createPathOptimizers() {
677
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 7 if (!problem_) throw std::runtime_error("The problem is not defined.");
678 7 pathOptimizers_.clear();
679 7 for (PathOptimizerTypes_t::const_iterator it = pathOptimizerTypes_.begin();
680
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 7 times.
 7 it != pathOptimizerTypes_.end(); ++it) {
681 ✗ PathOptimizerBuilder_t createOptimizer = pathOptimizers.get(*it);
682 ✗ pathOptimizers_.push_back(createOptimizer(problem_));
683 }
684 7 }
685
686 ✗ void ProblemSolver::initDistance() {
687 ✗ if (!problem_) throw std::runtime_error("The problem is not defined.");
688 ✗ DistancePtr_t dist(distances.get(distanceType_)(problem_));
689 ✗ problem_->distance(dist);
690 }
691
692 14 void ProblemSolver::initSteeringMethod() {
693
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 14 if (!problem_) throw std::runtime_error("The problem is not defined.");
694
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14 times.
✗ Branch 6 not taken.
 14 SteeringMethodPtr_t sm(steeringMethods.get(steeringMethodType_)(problem_));
695 14 problem_->steeringMethod(sm);
696 14 }
697
698 7 void ProblemSolver::initPathProjector() {
699
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 7 if (!problem_) throw std::runtime_error("The problem is not defined.");
700 PathProjectorBuilder_t createProjector =
701
2/4
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 pathProjectors.get(pathProjectorType_);
702 // The PathProjector will store a copy of the current steering method.
703 // This means:
704 // - when constraints are relevant, they should have been added before.
705 // TODO The path projector should update the constraint according to the
706 // path they project.
707 // - the steering method type must match the path projector type.
708 PathProjectorPtr_t pathProjector_ =
709
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 createProjector(problem_, pathProjectorTolerance_);
710 7 problem_->pathProjector(pathProjector_);
711 7 }
712
713 7 void ProblemSolver::initProblemTarget() {
714
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 7 if (!problem_) throw std::runtime_error("The problem is not defined.");
715 7 target_->problem(problem_);
716 7 problem_->target(target_);
717 7 }
718
719 ✗ bool ProblemSolver::prepareSolveStepByStep() {
720 ✗ initProblem();
721
722 ✗ pathPlanner_->startSolve();
723 ✗ pathPlanner_->tryConnectInitAndGoals();
724 ✗ return roadmap_->pathExists();
725 }
726
727 ✗ bool ProblemSolver::executeOneStep() {
728 ✗ pathPlanner_->oneStep();
729 ✗ return roadmap_->pathExists();
730 }
731
732 ✗ void ProblemSolver::finishSolveStepByStep() {
733 ✗ if (!roadmap_->pathExists()) throw std::logic_error("No path exists.");
734 ✗ PathVectorPtr_t planned = pathPlanner_->computePath();
735 ✗ paths_.push_back(pathPlanner_->finishSolve(planned));
736 }
737
738 7 void ProblemSolver::solve() {
739
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 initProblem();
740
741
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 PathVectorPtr_t path = pathPlanner_->solve();
742
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 paths_.push_back(path);
743
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 optimizePath(path);
744 7 }
745
746 ✗ bool ProblemSolver::directPath(ConfigurationIn_t start, ConfigurationIn_t end,
747 bool validate, std::size_t& pathId,
748 std::string& report) {
749 ✗ report = "";
750 ✗ if (!problem_) throw std::runtime_error("The problem is not defined.");
751
752 // Get steering method from problem
753 ✗ SteeringMethodPtr_t sm(problem_->steeringMethod());
754 ✗ PathPtr_t dp = (*sm)(start, end);
755 ✗ if (!dp) {
756 ✗ report = "Steering method failed to build a path.";
757 ✗ pathId = -1;
758 ✗ return false;
759 }
760 ✗ PathPtr_t dp1, dp2;
761 ✗ PathValidationReportPtr_t r;
762 ✗ bool projValid = true, projected = true, pathValid = true;
763 ✗ if (validate) {
764 ✗ PathProjectorPtr_t proj(problem()->pathProjector());
765 ✗ if (proj) {
766 ✗ projected = proj->apply(dp, dp1);
767 } else {
768 ✗ dp1 = dp;
769 }
770 ✗ projValid = problem()->pathValidation()->validate(dp1, false, dp2, r);
771 ✗ pathValid = projValid && projected;
772 ✗ if (!projValid) {
773 ✗ if (r) {
774 ✗ std::ostringstream oss;
775 ✗ oss << *r;
776 ✗ report = oss.str();
777 ✗ } else {
778 ✗ report = "No path validation report.";
779 }
780 }
781 ✗ } else {
782 ✗ dp2 = dp;
783 }
784 // Add Path in problem
785 PathVectorPtr_t path(
786 ✗ core::PathVector::create(dp2->outputSize(), dp2->outputDerivativeSize()));
787 ✗ path->appendPath(dp2);
788 ✗ pathId = addPath(path);
789 ✗ return pathValid;
790 }
791
792 ✗ void ProblemSolver::addConfigToRoadmap(ConfigurationIn_t config) {
793 ✗ roadmap_->addNode(config);
794 }
795
796 ✗ void ProblemSolver::addEdgeToRoadmap(ConfigurationIn_t config1,
797 ConfigurationIn_t config2,
798 const PathPtr_t& path) {
799 NodePtr_t node1, node2;
800 ✗ value_type accuracy = 10e-6;
801 value_type distance1, distance2;
802 ✗ node1 = roadmap_->nearestNode(config1, distance1);
803 ✗ node2 = roadmap_->nearestNode(config2, distance2);
804 ✗ if (distance1 >= accuracy) {
805 ✗ throw std::runtime_error("No node of the roadmap contains config1");
806 }
807 ✗ if (distance2 >= accuracy) {
808 ✗ throw std::runtime_error("No node of the roadmap contains config2");
809 }
810 ✗ roadmap_->addEdge(node1, node2, path);
811 }
812
813 ✗ void ProblemSolver::interrupt() {
814 ✗ if (pathPlanner()) pathPlanner()->interrupt();
815 ✗ for (PathOptimizers_t::iterator it = pathOptimizers_.begin();
816 ✗ it != pathOptimizers_.end(); ++it) {
817 ✗ (*it)->interrupt();
818 }
819 }
820
821 ✗ void ProblemSolver::addObstacle(const DevicePtr_t& device, bool collision,
822 bool distance) {
823 ✗ device->computeForwardKinematics(pinocchio::JOINT_POSITION);
824 ✗ device->computeFramesForwardKinematics();
825 ✗ device->updateGeometryPlacements();
826 ✗ const std::string& prefix = device->name();
827
828 ✗ const Model& m = device->model();
829 ✗ const Data& d = device->data();
830 ✗ for (std::size_t i = 1; i < m.frames.size(); ++i) {
831 ✗ const ::pinocchio::Frame& frame = m.frames[i];
832 // ::pinocchio::FrameType type = (frame.type == ::pinocchio::JOINT ?
833 // ::pinocchio::FIXED_JOINT : frame.type);
834 ✗ obstacleRModel_->addFrame(
835 ✗ ::pinocchio::Frame(prefix + frame.name, 0,
836 0, // TODO Keep frame hierarchy
837 ✗ d.oMf[i],
838 // type
839 ✗ frame.type));
840 }
841 ✗ obstacleRData_.reset(new Data(*obstacleRModel_));
842 ✗ ::pinocchio::framesForwardKinematics(*obstacleRModel_, *obstacleRData_,
843 ✗ vector_t::Zero(0).eval());
844
845 // Detach objects from joints
846 ✗ for (size_type i = 0; i < device->nbObjects(); ++i) {
847 ✗ CollisionObjectPtr_t obj = device->objectAt(i);
848 ✗ addObstacle(prefix + obj->name(), obj->geometry(), obj->getTransform(),
849 collision, distance);
850 }
851 }
852
853 ✗ void ProblemSolver::addObstacle(const CollisionObjectPtr_t& object,
854 bool collision, bool distance) {
855 // FIXME propagate object->mesh_path ?
856 ✗ addObstacle(object->name(), object->geometry(), object->getTransform(),
857 collision, distance);
858 }
859
860 7 void ProblemSolver::addObstacle(const std::string& name,
861 const CollisionGeometryPtr_t& inObject,
862 const Transform3f& pose, bool collision,
863 bool distance) {
864
2/4
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 7 times.
 7 if (obstacleModel_->existGeometryName(name)) {
865 ✗ HPP_THROW(std::runtime_error,
866 "object with name "
867 << name << " already added! Choose another name (prefix).");
868 }
869
870
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 14 ::pinocchio::GeomIndex id = obstacleModel_->addGeometryObject(
871
6/12
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 7 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 7 times.
✗ Branch 19 not taken.
 28 ::pinocchio::GeometryObject(name, 1, 0, inObject, pose, "",
872
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 vector3_t::Ones()),
873 7 *obstacleRModel_);
874 // Update obstacleData_
875 // FIXME This should be done in Pinocchio
876 {
877 7 ::pinocchio::GeometryModel& model = *obstacleModel_;
878 7 ::pinocchio::GeometryData& data = *obstacleData_;
879
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 data.oMg.resize(model.ngeoms);
880 // data.activeCollisionPairs.resize(model.collisionPairs.size(), true)
881 // data.distance_results(model.collisionPairs.size())
882 // data.collision_results(model.collisionPairs.size())
883 // data.radius()
884
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 data.oMg[id] = model.geometryObjects[id].placement;
885 }
886 CollisionObjectPtr_t object(
887
3/6
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
 14 new CollisionObject(obstacleModel_, obstacleData_, id));
888
889
1/2
✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
 7 if (collision) {
890
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 collisionObstacles_.push_back(object);
891
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 resetRoadmap();
892 }
893
2/4
✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 if (distance) distanceObstacles_.push_back(object);
894
2/4
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 7 times.
✗ Branch 9 not taken.
 7 if (problem()) problem()->addObstacle(object);
895
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 if (distanceBetweenObjects_) {
896
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 7 distanceBetweenObjects_->addObstacle(object);
897 }
898 7 }
899
900 ✗ void ProblemSolver::addObstacle(const std::string& name,
901 /*const*/ FclCollisionObject& inObject,
902 bool collision, bool distance) {
903 ✗ if (obstacleModel_->existGeometryName(name)) {
904 ✗ HPP_THROW(std::runtime_error,
905 "object with name "
906 << name << " already added! Choose another name (prefix).");
907 }
908
909 ✗ ::pinocchio::GeomIndex id = obstacleModel_->addGeometryObject(
910 ✗ ::pinocchio::GeometryObject(
911 ✗ name, 1, 0, inObject.collisionGeometry(),
912 ✗ ::pinocchio::toPinocchioSE3(inObject.getTransform()), "",
913 ✗ vector3_t::Ones()),
914 ✗ *obstacleRModel_);
915 // Update obstacleData_
916 // FIXME This should be done in Pinocchio
917 {
918 ✗ ::pinocchio::GeometryModel& model = *obstacleModel_;
919 ✗ ::pinocchio::GeometryData& data = *obstacleData_;
920 ✗ data.oMg.resize(model.ngeoms);
921 // data.activeCollisionPairs.resize(model.collisionPairs.size(), true)
922 // data.distance_results(model.collisionPairs.size())
923 // data.collision_results(model.collisionPairs.size())
924 // data.radius()
925 ✗ data.oMg[id] = model.geometryObjects[id].placement;
926 }
927 CollisionObjectPtr_t object(
928 ✗ new CollisionObject(obstacleModel_, obstacleData_, id));
929
930 ✗ if (collision) {
931 ✗ collisionObstacles_.push_back(object);
932 ✗ resetRoadmap();
933 }
934 ✗ if (distance) distanceObstacles_.push_back(object);
935 ✗ if (problem()) problem()->addObstacle(object);
936 ✗ if (distanceBetweenObjects_) {
937 ✗ distanceBetweenObjects_->addObstacle(object);
938 }
939 }
940
941 ✗ void ProblemSolver::removeObstacle(const std::string& name) {
942 ✗ if (!obstacleModel_->existGeometryName(name)) {
943 ✗ HPP_THROW(std::invalid_argument, "No obstacle with name " << name);
944 }
945 ✗ ::pinocchio::GeomIndex id = obstacleModel_->getGeometryId(name);
946
947 // Update obstacle model
948 ✗ remove(obstacleModel_->geometryObjects, id);
949 ✗ obstacleModel_->ngeoms--;
950 ✗ remove(obstacleData_->oMg, id);
951
952 ✗ remove(collisionObstacles_, id);
953 ✗ remove(distanceObstacles_, id);
954 ✗ for (ObjectStdVector_t::iterator _o = collisionObstacles_.begin();
955 ✗ _o != collisionObstacles_.end(); ++_o) {
956 ✗ pinocchio::GeomIndex oid = (*_o)->indexInModel();
957 ✗ if (oid > id)
958 ✗ _o->reset(new CollisionObject(obstacleModel_, obstacleData_, oid - 1));
959 }
960 ✗ for (ObjectStdVector_t::iterator _o = distanceObstacles_.begin();
961 ✗ _o != distanceObstacles_.end(); ++_o) {
962 ✗ pinocchio::GeomIndex oid = (*_o)->indexInModel();
963 ✗ if (oid > id)
964 ✗ _o->reset(new CollisionObject(obstacleModel_, obstacleData_, oid - 1));
965 }
966 ✗ resetProblem(); // resets problem_ and distanceBetweenObjects_
967 ✗ resetRoadmap();
968 }
969
970 ✗ void ProblemSolver::cutObstacle(const std::string& name,
971 const fcl::AABB& aabb) {
972 ✗ if (!obstacleModel_->existGeometryName(name)) {
973 ✗ HPP_THROW(std::invalid_argument, "No obstacle with name " << name);
974 }
975 ✗ ::pinocchio::GeomIndex id = obstacleModel_->getGeometryId(name);
976
977 ✗ fcl::Transform3f oMg = ::pinocchio::toFclTransform3f(obstacleData_->oMg[id]);
978 ✗ CollisionGeometryPtr_t fclgeom = obstacleModel_->geometryObjects[id].geometry;
979 ✗ CollisionGeometryPtr_t newgeom(extract(fclgeom.get(), oMg, aabb));
980 ✗ if (!newgeom) {
981 // No intersection. Geom should be removed.
982 ✗ removeObstacle(name);
983 } else {
984 ✗ obstacleModel_->geometryObjects[id].geometry = newgeom;
985 }
986 }
987
988 ✗ void ProblemSolver::removeObstacleFromJoint(const std::string& obstacleName,
989 const std::string& jointName) {
990 ✗ if (!robot_) {
991 ✗ throw std::runtime_error("No robot defined.");
992 }
993 ✗ JointPtr_t joint = robot_->getJointByName(jointName);
994 ✗ if (!joint) {
995 ✗ std::ostringstream oss;
996 ✗ oss << "Robot has no joint with name " << jointName << ".";
997 ✗ throw std::runtime_error(oss.str().c_str());
998 }
999 ✗ const CollisionObjectPtr_t& object = obstacle(obstacleName);
1000 ✗ if (!object) {
1001 ✗ std::ostringstream oss;
1002 ✗ oss << "No obstacle with with name " << obstacleName << ".";
1003 ✗ throw std::runtime_error(oss.str().c_str());
1004 }
1005 ✗ problem()->removeObstacleFromJoint(joint, object);
1006 }
1007
1008 ✗ void ProblemSolver::filterCollisionPairs() {
1009 ✗ problem()->filterCollisionPairs();
1010 }
1011
1012 ✗ CollisionObjectPtr_t ProblemSolver::obstacle(const std::string& name) const {
1013 ✗ if (obstacleModel_->existGeometryName(name)) {
1014 ✗ ::pinocchio::GeomIndex id = obstacleModel_->getGeometryId(name);
1015 return CollisionObjectPtr_t(
1016 ✗ new CollisionObject(obstacleModel_, obstacleData_, id));
1017 }
1018 ✗ HPP_THROW(std::invalid_argument, "No obstacle with name " << name);
1019 }
1020
1021 ✗ const Transform3f& ProblemSolver::obstacleFramePosition(
1022 const std::string& name) const {
1023 ✗ if (!obstacleRModel_->existFrame(name)) {
1024 ✗ HPP_THROW(std::invalid_argument, "No obstacle frame with name " << name);
1025 }
1026 ✗ ::pinocchio::FrameIndex id = obstacleRModel_->getFrameId(name);
1027 ✗ return obstacleRData_->oMf[id];
1028 }
1029
1030 ✗ std::list<std::string> ProblemSolver::obstacleNames(bool collision,
1031 bool distance) const {
1032 ✗ std::list<std::string> res;
1033 ✗ if (collision) {
1034 ✗ for (ObjectStdVector_t::const_iterator it = collisionObstacles_.begin();
1035 ✗ it != collisionObstacles_.end(); ++it) {
1036 ✗ res.push_back((*it)->name());
1037 }
1038 }
1039 ✗ if (distance) {
1040 ✗ for (ObjectStdVector_t::const_iterator it = distanceObstacles_.begin();
1041 ✗ it != distanceObstacles_.end(); ++it) {
1042 ✗ res.push_back((*it)->name());
1043 }
1044 }
1045 ✗ return res;
1046 }
1047
1048 ✗ const ObjectStdVector_t& ProblemSolver::collisionObstacles() const {
1049 ✗ return collisionObstacles_;
1050 }
1051
1052 ✗ const ObjectStdVector_t& ProblemSolver::distanceObstacles() const {
1053 ✗ return distanceObstacles_;
1054 }
1055
1056 // ----------- Declare parameters ------------------------------------- //
1057
1058 18 HPP_START_PARAMETER_DECLARATION(ProblemSolver)
1059
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
1060 Parameter::VECTOR, "ConfigurationShooter/Gaussian/center",
1061
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 "Center of gaussian random distribution.", Parameter(vector_t())));
1062
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
1063 Parameter::BOOL, "ConfigurationShooter/Gaussian/useRobotVelocity",
1064
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 "Use robot current velocity as standard velocity.", Parameter(false)));
1065
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
1066 Parameter::FLOAT, "ConfigurationShooter/Gaussian/standardDeviation",
1067
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 "Scale the default standard deviation with this factor.", Parameter(0.25)));
1068
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
1069 Parameter::BOOL, "ConfigurationShooter/sampleExtraDOF",
1070 "If false, the value of the random configuration extraDOF are set to 0.",
1071
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(true)));
1072
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
1073 Parameter::FLOAT, "SteeringMethod/Carlike/turningRadius",
1074 "Turning radius of a carlike robot for Reeds and Shepp and Dubins "
1075 "distances and steering methods.",
1076
1/2
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
 36 Parameter(1.)));
1077
4/8
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 18 times.
✗ Branch 13 not taken.
 18 Problem::declareParameter(ParameterDescription(
1078 Parameter::STRING, "SteeringMethod/Carlike/wheels",
1079 "Names of revolute joints that hold directional wheels separated by "
1080 "commas.",
1081
2/4
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 18 times.
✗ Branch 6 not taken.
 36 Parameter(std::string(""))));
1082 18 HPP_END_PARAMETER_DECLARATION(ProblemSolver)
1083 } // namespace core
1084 } // namespace hpp
1085