GCC Code Coverage Report


Directory: ./
File: src/problem-target/task-target.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 0 36 0.0%
Branches: 0 68 0.0%

Line Branch Exec Source
1 // Copyright (c) 2016, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28
29 #include <hpp/core/config-projector.hh>
30 #include <hpp/core/connected-component.hh>
31 #include <hpp/core/constraint-set.hh>
32 #include <hpp/core/node.hh>
33 #include <hpp/core/problem-target/task-target.hh>
34 #include <hpp/core/problem.hh>
35 #include <hpp/core/roadmap.hh>
36 #include <stdexcept>
37
38 #include "../astar.hh"
39
40 namespace hpp {
41 namespace core {
42 namespace problemTarget {
43 TaskTargetPtr_t TaskTarget::create(const ProblemPtr_t& problem) {
44 TaskTarget* tt = new TaskTarget(problem);
45 TaskTargetPtr_t shPtr(tt);
46 tt->init(shPtr);
47 return shPtr;
48 }
49
50 void TaskTarget::check(const RoadmapPtr_t&) const {
51 if (!constraints_) {
52 std::string msg("No constraints: task not specified.");
53 hppDout(error, msg);
54 throw std::runtime_error(msg);
55 }
56 }
57
58 bool TaskTarget::reached(const RoadmapPtr_t& roadmap) const {
59 assert(roadmap->goalNodes().empty());
60 if (!roadmap->initNode()) return false;
61 const ConnectedComponentPtr_t ccInit =
62 roadmap->initNode()->connectedComponent(); // TODO
63 bool res(false);
64 for (auto node : ccInit->nodes()) {
65 if ((*constraints_).isSatisfied(node->configuration())) {
66 roadmap->addGoalNode(
67 node->configuration()); // temporarily add goal node to compute path
68 res = true;
69 }
70 }
71 return res;
72 }
73
74 NumericalConstraints_t TaskTarget::constraints() const {
75 if ((!constraints_) || (!constraints_->configProjector())) {
76 return NumericalConstraints_t();
77 }
78 return constraints_->configProjector()->numericalConstraints();
79 }
80
81 PathVectorPtr_t TaskTarget::computePath(const RoadmapPtr_t& roadmap) const {
82 ProblemPtr_t problem(problem_.lock());
83 assert(problem);
84 Astar astar(roadmap, problem->distance());
85 PathVectorPtr_t sol = PathVector::create(problem->robot()->configSize(),
86 problem->robot()->numberDof());
87 astar.solution(sol);
88 // This happens when q_init already satisfies the task.
89 if (sol->numberPaths() == 0) {
90 Configuration_t q(roadmap->initNode()->configuration());
91 sol->appendPath((*problem->steeringMethod())(q, q));
92 }
93 roadmap->resetGoalNodes(); // remove the temporary goal node
94 return sol;
95 }
96 } // namespace problemTarget
97 } // namespace core
98 } // namespace hpp
99