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 |