GCC Code Coverage Report

Directory: ./
File: src/graph/graph.cc
Date: 2025-03-07 11:10:46
Exec Total Coverage
Lines: 59 129 45.7%
Branches: 28 148 18.9%
Line Branch Exec Source
1 // Copyright (c) 2014, LAAS-CNRS
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/manipulation/graph/graph.hh"
30
31 #include <hpp/manipulation/constraint-set.hh>
32 #include <hpp/util/assertion.hh>
33
34 #include "hpp/manipulation/graph/edge.hh"
35 #include "hpp/manipulation/graph/state-selector.hh"
36 #include "hpp/manipulation/graph/state.hh"
37 #include "hpp/manipulation/graph/statistics.hh"
38
39 namespace hpp {
40 namespace manipulation {
41 namespace graph {
42 typedef constraints::Implicit Implicit;
43 typedef constraints::ImplicitPtr_t ImplicitPtr_t;
44 2 GraphPtr_t Graph::create(const std::string& name, DevicePtr_t robot,
45 const ProblemPtr_t& problem) {
46
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 Graph* ptr = new Graph(name, problem);
47 2 GraphPtr_t shPtr(ptr);
48
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 ptr->init(shPtr, robot);
49
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 shPtr->createStateSelector(name);
50 2 return shPtr;
51 }
52
53 2 void Graph::init(const GraphWkPtr_t& weak, DevicePtr_t robot) {
54
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 GraphComponent::init(weak);
55 2 robot_ = robot;
56 2 wkPtr_ = weak;
57 2 parentGraph(wkPtr_);
58
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 insertHistogram(
59
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 4 graph::HistogramPtr_t(new graph::StateHistogram(wkPtr_.lock())));
60 2 }
61
62 2 void Graph::initialize() {
63 2 hists_.clear();
64
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 4 assert(components_.size() >= 1 && components_[0].lock() == wkPtr_.lock());
65
2/2
✓ Branch 1 taken 12 times.
✓ Branch 2 taken 2 times.
 14 for (std::size_t i = 1; i < components_.size(); ++i)
66
1/2
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 components_[i].lock()->initialize();
67 2 isInit_ = true;
68 2 }
69
70 9 void Graph::invalidate() {
71
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 9 times.
 15 for (std::size_t i = 1; i < components_.size(); ++i) {
72
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
 6 assert(components_[i].lock());
73
1/2
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 6 components_[i].lock()->invalidate();
74 }
75 9 isInit_ = false;
76 9 }
77
78 4 StateSelectorPtr_t Graph::createStateSelector(const std::string& name) {
79 4 invalidate();
80 4 stateSelector_ = StateSelector::create(name);
81 4 stateSelector_->parentGraph(wkPtr_);
82 4 return stateSelector_;
83 }
84
85 ✗ void Graph::stateSelector(StateSelectorPtr_t ns) {
86 ✗ invalidate();
87 ✗ stateSelector_ = ns;
88 ✗ stateSelector_->parentGraph(wkPtr_);
89 }
90
91 2 void Graph::maxIterations(size_type iterations) {
92 2 invalidate();
93 2 maxIterations_ = iterations;
94 2 }
95
96 20 size_type Graph::maxIterations() const { return maxIterations_; }
97
98 2 void Graph::errorThreshold(const value_type& threshold) {
99 2 invalidate();
100 2 errorThreshold_ = threshold;
101 2 }
102
103 20 value_type Graph::errorThreshold() const { return errorThreshold_; }
104
105 65 const DevicePtr_t& Graph::robot() const { return robot_; }
106
107 ✗ void Graph::problem(const ProblemPtr_t& problem) {
108 ✗ if (problem_ != problem) {
109 ✗ problem_ = problem;
110 ✗ invalidate();
111 }
112 }
113
114 8 const ProblemPtr_t& Graph::problem() const { return problem_; }
115
116 1 StatePtr_t Graph::getState(ConfigurationIn_t config) const {
117
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (!stateSelector_)
118 ✗ throw std::runtime_error("No StateSelector in Constraint Graph.");
119
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 return stateSelector_->getState(config);
120 }
121
122 ✗ StatePtr_t Graph::getState(RoadmapNodePtr_t coreNode) const {
123 ✗ return stateSelector_->getState(coreNode);
124 }
125
126 1 Edges_t Graph::getEdges(const StatePtr_t& from, const StatePtr_t& to) const {
127 1 Edges_t edges;
128 1 for (Neighbors_t::const_iterator it = from->neighbors().begin();
129
2/2
✓ Branch 5 taken 2 times.
✓ Branch 6 taken 1 times.
 3 it != from->neighbors().end(); ++it) {
130
4/6
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1 times.
✓ Branch 8 taken 1 times.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 if (it->second->stateTo() == to) edges.push_back(it->second);
131 }
132 1 return edges;
133 }
134
135 ✗ EdgePtr_t Graph::chooseEdge(RoadmapNodePtr_t from) const {
136 ✗ return stateSelector_->chooseEdge(from);
137 }
138
139 ✗ void Graph::clearConstraintsAndComplement() {
140 ✗ constraintsAndComplements_.clear();
141 }
142
143 ✗ void Graph::registerConstraints(const ImplicitPtr_t& constraint,
144 const ImplicitPtr_t& complement,
145 const ImplicitPtr_t& both) {
146 ✗ for (ConstraintsAndComplements_t::const_iterator it(
147 ✗ constraintsAndComplements_.begin());
148 ✗ it != constraintsAndComplements_.end(); ++it) {
149 ✗ assert(it->constraint != constraint);
150 }
151 ✗ constraintsAndComplements_.push_back(
152 ✗ ConstraintAndComplement_t(constraint, complement, both));
153 }
154
155 ✗ bool Graph::isComplement(const ImplicitPtr_t& constraint,
156 const ImplicitPtr_t& complement,
157 ImplicitPtr_t& combinationOfBoth) const {
158 ✗ for (ConstraintsAndComplements_t::const_iterator it =
159 ✗ constraintsAndComplements_.begin();
160 ✗ it != constraintsAndComplements_.end(); ++it) {
161 ✗ if ((it->constraint->functionPtr() == constraint->functionPtr()) &&
162 ✗ (it->complement->functionPtr() == complement->functionPtr())) {
163 ✗ combinationOfBoth = it->both;
164 ✗ return true;
165 }
166 }
167 ✗ return false;
168 }
169
170 ✗ const ConstraintsAndComplements_t& Graph::constraintsAndComplements() const {
171 ✗ return constraintsAndComplements_;
172 }
173
174 ✗ ConstraintSetPtr_t Graph::configConstraint(const StatePtr_t& state) const {
175 ✗ return state->configConstraint();
176 }
177
178 ✗ bool Graph::getConfigErrorForState(ConfigurationIn_t config,
179 const StatePtr_t& state,
180 vector_t& error) const {
181 ✗ return configConstraint(state)->isSatisfied(config, error);
182 }
183
184 ✗ bool Graph::getConfigErrorForEdge(ConfigurationIn_t config,
185 const EdgePtr_t& edge,
186 vector_t& error) const {
187 ✗ ConstraintSetPtr_t cs(pathConstraint(edge));
188 ✗ ConfigProjectorPtr_t cp(cs->configProjector());
189 ✗ if (cp) cp->rightHandSideFromConfig(config);
190 ✗ return cs->isSatisfied(config, error);
191 }
192
193 ✗ bool Graph::getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig,
194 ConfigurationIn_t config,
195 const EdgePtr_t& edge,
196 vector_t& error) const {
197 ✗ ConstraintSetPtr_t cs(targetConstraint(edge));
198 ✗ ConfigProjectorPtr_t cp(cs->configProjector());
199 ✗ if (cp) cp->rightHandSideFromConfig(leafConfig);
200 ✗ return cs->isSatisfied(config, error);
201 }
202
203 ✗ bool Graph::getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig,
204 ConfigurationIn_t config,
205 const EdgePtr_t& edge,
206 vector_t& error) const {
207 ✗ ConstraintSetPtr_t cs(pathConstraint(edge));
208 ✗ ConfigProjectorPtr_t cp(cs->configProjector());
209 ✗ if (cp) cp->rightHandSideFromConfig(leafConfig);
210 ✗ return cs->isSatisfied(config, error);
211 }
212
213 ✗ ConstraintSetPtr_t Graph::targetConstraint(const EdgePtr_t& edge) const {
214 ✗ return edge->targetConstraint();
215 }
216
217 ✗ ConstraintSetPtr_t Graph::pathConstraint(const EdgePtr_t& edge) const {
218 ✗ return edge->pathConstraint();
219 }
220
221 7 GraphComponentWkPtr_t Graph::get(std::size_t id) const {
222
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 7 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 7 if (id >= components_.size()) throw std::out_of_range("ID out of range.");
223 7 return components_[id];
224 }
225
226 28 GraphComponents_t& Graph::components() { return components_; }
227
228 2 Graph::Graph(const std::string& name, const ProblemPtr_t& problem)
229 2 : GraphComponent(name), problem_(problem) {}
230
231 ✗ std::ostream& Graph::dotPrint(std::ostream& os,
232 dot::DrawingAttributes da) const {
233 ✗ da.separator = "; ";
234 ✗ da.openSection = "\n";
235 ✗ da.closeSection = ";\n";
236 ✗ dot::Tooltip tp;
237 ✗ tp.addLine("Graph constains:");
238 ✗ populateTooltip(tp);
239 ✗ da.insertWithQuote("tooltip", tp.toStr());
240 ✗ os << "digraph " << id() << " {" << da;
241 ✗ stateSelector_->dotPrint(os);
242 ✗ os << "}" << std::endl;
243 ✗ return os;
244 }
245
246 ✗ std::ostream& Graph::print(std::ostream& os) const {
247 ✗ return GraphComponent::print(os) << std::endl << *stateSelector_;
248 }
249 } // namespace graph
250 } // namespace manipulation
251
252 } // namespace hpp
253