Directory: | ./ |
---|---|
File: | src/problem.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 76 | 149 | 51.0% |
Branches: | 49 | 224 | 21.9% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2005, 2006, 2007, 2008, 2009, 2010, 2011 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/core/collision-validation.hh> | ||
31 | #include <hpp/core/config-validations.hh> | ||
32 | #include <hpp/core/configuration-shooter/uniform.hh> | ||
33 | #include <hpp/core/continuous-validation/dichotomy.hh> | ||
34 | #include <hpp/core/continuous-validation/progressive.hh> | ||
35 | #include <hpp/core/joint-bound-validation.hh> | ||
36 | #include <hpp/core/path-validation/discretized-collision-checking.hh> | ||
37 | #include <hpp/core/problem-target/goal-configurations.hh> | ||
38 | #include <hpp/core/problem.hh> | ||
39 | #include <hpp/core/steering-method/straight.hh> | ||
40 | #include <hpp/core/weighed-distance.hh> | ||
41 | #include <hpp/pinocchio/configuration.hh> | ||
42 | #include <hpp/pinocchio/device.hh> | ||
43 | #include <hpp/util/debug.hh> | ||
44 | #include <iostream> | ||
45 | |||
46 | namespace hpp { | ||
47 | namespace core { | ||
48 | 1234 | Container<ParameterDescription>& pds() { | |
49 |
3/4✓ Branch 0 taken 18 times.
✓ Branch 1 taken 1216 times.
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
|
1234 | static Container<ParameterDescription> _parameterDescriptions; |
50 | 1234 | return _parameterDescriptions; | |
51 | } | ||
52 | |||
53 | // ====================================================================== | ||
54 | |||
55 | 600 | void Problem::declareParameter(const ParameterDescription& desc) { | |
56 | typedef Container<ParameterDescription> CPD_t; | ||
57 | std::pair<CPD_t::iterator, bool> ret = | ||
58 |
3/6✓ Branch 1 taken 600 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 600 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 600 times.
✗ Branch 9 not taken.
|
600 | pds().map.insert(CPD_t::value_type(desc.name(), desc)); |
59 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 600 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
600 | if (!ret.second) ret.first->second = desc; |
60 | 600 | } | |
61 | |||
62 | // ====================================================================== | ||
63 | |||
64 | ✗ | const Container<ParameterDescription>& Problem::parameterDescriptions() { | |
65 | ✗ | return pds(); | |
66 | } | ||
67 | |||
68 | // ====================================================================== | ||
69 | |||
70 | 317 | const ParameterDescription& Problem::parameterDescription( | |
71 | const std::string& name) { | ||
72 |
1/2✓ Branch 2 taken 317 times.
✗ Branch 3 not taken.
|
317 | if (pds().has(name)) return pds().get(name); |
73 | ✗ | throw std::invalid_argument("No parameter description with name " + name); | |
74 | } | ||
75 | |||
76 | // ====================================================================== | ||
77 | |||
78 | 44 | ProblemPtr_t Problem::create(DevicePtr_t robot) { | |
79 |
3/6✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 44 times.
✗ Branch 9 not taken.
|
44 | ProblemPtr_t p(new Problem(robot)); |
80 |
1/2✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
|
44 | p->init(p); |
81 | 44 | return p; | |
82 | } | ||
83 | |||
84 | ✗ | ProblemPtr_t Problem::createCopy(const ProblemConstPtr_t& other) { | |
85 | ✗ | ProblemPtr_t p(new Problem(*other)); | |
86 | ✗ | p->init(p); | |
87 | ✗ | return p; | |
88 | } | ||
89 | |||
90 | // ====================================================================== | ||
91 | 44 | Problem::Problem(DevicePtr_t robot) | |
92 |
2/4✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 44 times.
✗ Branch 11 not taken.
|
44 | : robot_(robot), configValidations_(ConfigValidations::create()) {} |
93 | |||
94 | // ====================================================================== | ||
95 | |||
96 | 44 | void Problem::init(ProblemWkPtr_t wkPtr) { | |
97 | 44 | wkPtr_ = wkPtr; | |
98 | |||
99 | 44 | distance_ = WeighedDistance::create(robot_); | |
100 |
1/2✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
|
44 | target_ = problemTarget::GoalConfigurations::create(wkPtr_.lock()); |
101 |
1/2✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
|
44 | steeringMethod_ = steeringMethod::Straight::create(wkPtr_.lock()); |
102 | pathValidation_ = | ||
103 |
1/2✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
|
44 | pathValidation::createDiscretizedCollisionChecking(robot_, 0.05); |
104 | 44 | configurationShooter_ = configurationShooter::Uniform::create(robot_); | |
105 |
2/4✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
|
44 | constraints(ConstraintSet::create(robot_, "default_constraint_set")); |
106 | 44 | } | |
107 | |||
108 | // ====================================================================== | ||
109 | |||
110 | ✗ | Problem::Problem() | |
111 | ✗ | : robot_(), | |
112 | ✗ | distance_(), | |
113 | ✗ | initConf_(), | |
114 | ✗ | target_(), | |
115 | ✗ | steeringMethod_(), | |
116 | ✗ | configValidations_(), | |
117 | ✗ | pathValidation_(), | |
118 | ✗ | collisionObstacles_(), | |
119 | ✗ | constraints_(), | |
120 | ✗ | configurationShooter_() { | |
121 | ✗ | assert(false && "This constructor should not be used."); | |
122 | } | ||
123 | |||
124 | 140 | Problem::~Problem() {} | |
125 | |||
126 | // ====================================================================== | ||
127 | |||
128 | 8 | void Problem::initConfig(ConfigurationIn_t config) { initConf_ = config; } | |
129 | |||
130 | // ====================================================================== | ||
131 | |||
132 | ✗ | const Configurations_t Problem::goalConfigs() const { | |
133 | problemTarget::GoalConfigurationsPtr_t gc( | ||
134 | ✗ | HPP_DYNAMIC_PTR_CAST(problemTarget::GoalConfigurations, target_)); | |
135 | ✗ | if (gc) { | |
136 | ✗ | return gc->configurations(); | |
137 | } | ||
138 | ✗ | return Configurations_t(); | |
139 | } | ||
140 | |||
141 | // ====================================================================== | ||
142 | |||
143 | 1 | void Problem::addGoalConfig(ConfigurationIn_t config) { | |
144 | problemTarget::GoalConfigurationsPtr_t gc( | ||
145 | 1 | HPP_DYNAMIC_PTR_CAST(problemTarget::GoalConfigurations, target_)); | |
146 | // If target is not an instance of GoalConfigurations, create new | ||
147 | // instance | ||
148 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
|
1 | if (!gc) { |
149 | ✗ | gc = problemTarget::GoalConfigurations::create(wkPtr_.lock()); | |
150 | ✗ | target_ = gc; | |
151 | } | ||
152 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | gc->addConfiguration(config); |
153 | 1 | } | |
154 | |||
155 | // ====================================================================== | ||
156 | |||
157 | ✗ | void Problem::resetGoalConfigs() { | |
158 | problemTarget::GoalConfigurationsPtr_t gc( | ||
159 | ✗ | HPP_DYNAMIC_PTR_CAST(problemTarget::GoalConfigurations, target_)); | |
160 | // If target is not an instance of GoalConfigurations, create new | ||
161 | // instance | ||
162 | ✗ | if (!gc) { | |
163 | ✗ | gc = problemTarget::GoalConfigurations::create(wkPtr_.lock()); | |
164 | ✗ | target_ = gc; | |
165 | } | ||
166 | ✗ | gc->resetConfigurations(); | |
167 | } | ||
168 | |||
169 | // ====================================================================== | ||
170 | |||
171 | 12 | void Problem::clearConfigValidations() { configValidations_->clear(); } | |
172 | |||
173 | // ====================================================================== | ||
174 | |||
175 | 34 | const ObjectStdVector_t& Problem::collisionObstacles() const { | |
176 | 34 | return collisionObstacles_; | |
177 | } | ||
178 | |||
179 | // ====================================================================== | ||
180 | |||
181 | 11 | void Problem::collisionObstacles(const ObjectStdVector_t& collisionObstacles) { | |
182 | 11 | collisionObstacles_.clear(); | |
183 | // pass the local vector of collisions object to the problem | ||
184 | 11 | for (ObjectStdVector_t::const_iterator itObj = collisionObstacles.begin(); | |
185 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 11 times.
|
11 | itObj != collisionObstacles.end(); ++itObj) { |
186 | ✗ | addObstacle(*itObj); | |
187 | } | ||
188 | 11 | } | |
189 | |||
190 | // ====================================================================== | ||
191 | |||
192 | 8 | void Problem::addObstacle(const CollisionObjectPtr_t& object) { | |
193 | // Add object in local list | ||
194 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | collisionObstacles_.push_back(object); |
195 | // Add obstacle to path validation method | ||
196 | shared_ptr<ObstacleUserInterface> oui = | ||
197 | 8 | HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, pathValidation_); | |
198 |
2/4✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 8 times.
✗ Branch 7 not taken.
|
8 | if (oui) oui->addObstacle(object); |
199 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
|
8 | assert(configValidations_); |
200 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (configValidations_) { |
201 |
1/2✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
8 | configValidations_->addObstacle(object); |
202 | } | ||
203 | 8 | } | |
204 | |||
205 | // ====================================================================== | ||
206 | |||
207 | ✗ | void Problem::removeObstacleFromJoint( | |
208 | const JointPtr_t& joint, const CollisionObjectConstPtr_t& obstacle) { | ||
209 | shared_ptr<ObstacleUserInterface> oui = | ||
210 | ✗ | HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, pathValidation_); | |
211 | ✗ | if (oui) oui->removeObstacleFromJoint(joint, obstacle); | |
212 | ✗ | assert(configValidations_); | |
213 | ✗ | if (configValidations_) { | |
214 | ✗ | configValidations_->removeObstacleFromJoint(joint, obstacle); | |
215 | } | ||
216 | } | ||
217 | |||
218 | // ====================================================================== | ||
219 | |||
220 | ✗ | void Problem::filterCollisionPairs() { | |
221 | ✗ | RelativeMotion::matrix_type matrix = RelativeMotion::matrix(robot_); | |
222 | ✗ | RelativeMotion::fromConstraint(matrix, robot_, constraints_); | |
223 | hppDout(info, "RelativeMotion matrix:\n" << matrix); | ||
224 | |||
225 | shared_ptr<ObstacleUserInterface> oui = | ||
226 | ✗ | HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, pathValidation_); | |
227 | ✗ | if (oui) oui->filterCollisionPairs(matrix); | |
228 | ✗ | assert(configValidations_); | |
229 | ✗ | if (configValidations_) { | |
230 | ✗ | configValidations_->filterCollisionPairs(matrix); | |
231 | } | ||
232 | } | ||
233 | |||
234 | // ====================================================================== | ||
235 | |||
236 | ✗ | void Problem::setSecurityMargins(const matrix_t& securityMatrix) { | |
237 | shared_ptr<ObstacleUserInterface> oui = | ||
238 | ✗ | HPP_DYNAMIC_PTR_CAST(ObstacleUserInterface, pathValidation_); | |
239 | ✗ | if (oui) oui->setSecurityMargins(securityMatrix); | |
240 | ✗ | assert(configValidations_); | |
241 | ✗ | if (configValidations_) { | |
242 | ✗ | configValidations_->setSecurityMargins(securityMatrix); | |
243 | } | ||
244 | } | ||
245 | |||
246 | // ====================================================================== | ||
247 | |||
248 | 18 | void Problem::pathValidation(const PathValidationPtr_t& pathValidation) { | |
249 | 18 | pathValidation_ = pathValidation; | |
250 | 18 | } | |
251 | |||
252 | // ====================================================================== | ||
253 | |||
254 | 21 | void Problem::addConfigValidation( | |
255 | const ConfigValidationPtr_t& configValidation) { | ||
256 | 21 | configValidations_->add(configValidation); | |
257 | 21 | } | |
258 | |||
259 | // ====================================================================== | ||
260 | |||
261 | 7 | void Problem::configurationShooter( | |
262 | const ConfigurationShooterPtr_t& configurationShooter) { | ||
263 | 7 | configurationShooter_ = configurationShooter; | |
264 | 7 | } | |
265 | |||
266 | // ====================================================================== | ||
267 | |||
268 | 8 | void Problem::checkProblem() const { | |
269 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
|
8 | if (!robot()) { |
270 | ✗ | std::string msg("No device in problem."); | |
271 | hppDout(error, msg); | ||
272 | ✗ | throw std::runtime_error(msg); | |
273 | } | ||
274 | |||
275 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
|
8 | if (initConfig().size() == 0) { |
276 | ✗ | std::string msg("No init config in problem."); | |
277 | hppDout(error, msg); | ||
278 | ✗ | throw std::runtime_error(msg); | |
279 | } | ||
280 | |||
281 | // Check that initial configuration is valid | ||
282 | 8 | ValidationReportPtr_t report; | |
283 |
2/4✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 8 times.
|
8 | if (!configValidations_->validate(initConf_, report)) { |
284 | ✗ | std::ostringstream oss; | |
285 | ✗ | oss << "init config invalid : " << *report; | |
286 | ✗ | throw std::runtime_error(oss.str()); | |
287 | } | ||
288 | // Check that initial configuration sarisfies the constraints of the | ||
289 | // problem | ||
290 |
3/6✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 8 times.
|
8 | if (!constraints_->isSatisfied(initConf_)) { |
291 | ✗ | std::ostringstream os; | |
292 | ✗ | os << "Initial configuration " << pinocchio::displayConfig(initConf_) | |
293 | ✗ | << " does not satisfy the constraints of the problem."; | |
294 | ✗ | throw std::logic_error(os.str().c_str()); | |
295 | } | ||
296 | |||
297 | // check that goal configurations satisfy are valid | ||
298 | problemTarget::GoalConfigurationsPtr_t gc( | ||
299 | 8 | HPP_DYNAMIC_PTR_CAST(problemTarget::GoalConfigurations, target_)); | |
300 |
1/2✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
|
8 | if (gc) { |
301 |
2/4✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
|
8 | const Configurations_t goals(gc->configurations()); |
302 |
4/6✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 8 times.
✓ Branch 13 taken 8 times.
|
16 | for (auto config : gc->configurations()) { |
303 |
2/4✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 8 times.
|
8 | if (!configValidations_->validate(config, report)) { |
304 | ✗ | std::ostringstream oss; | |
305 | ✗ | oss << *report; | |
306 | ✗ | throw std::runtime_error(oss.str()); | |
307 | } | ||
308 | // Check that goal configurations sarisfy the constraints of the | ||
309 | // problem | ||
310 |
3/6✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 8 times.
|
8 | if (!constraints_->isSatisfied(config)) { |
311 | ✗ | std::ostringstream os; | |
312 | ✗ | os << "Goal configuration " << pinocchio::displayConfig(config) | |
313 | ✗ | << " does not satisfy the constraints of the problem."; | |
314 | ✗ | throw std::logic_error(os.str().c_str()); | |
315 | } | ||
316 | 8 | } | |
317 | 8 | } | |
318 | 8 | } | |
319 | // ====================================================================== | ||
320 | |||
321 | 42 | void Problem::setParameter(const std::string& name, const Parameter& value) { | |
322 | 42 | const ParameterDescription& desc = parameterDescription(name); | |
323 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 42 times.
|
42 | if (desc.type() != value.type()) |
324 | ✗ | throw std::invalid_argument("value is not a " + | |
325 | ✗ | Parameter::typeName(desc.type())); | |
326 | 42 | parameters.add(name, value); | |
327 | 42 | } | |
328 | |||
329 | // ====================================================================== | ||
330 | |||
331 | } // namespace core | ||
332 | } // namespace hpp | ||
333 |