GCC Code Coverage Report

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