GCC Code Coverage Report

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