GCC Code Coverage Report

Directory: ./
File: tests/test-constraintgraph.cc
Date: 2025-03-07 11:10:46
Exec Total Coverage
Lines: 66 67 98.5%
Branches: 184 382 48.2%
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 <boost/test/unit_test.hpp>
30 #include <hpp/constraints/generic-transformation.hh>
31 #include <hpp/constraints/locked-joint.hh>
32 #include <hpp/constraints/relative-com.hh>
33 #include <hpp/core/path-validation-report.hh>
34 #include <hpp/core/steering-method/straight.hh>
35 #include <hpp/manipulation/constraint-set.hh>
36 #include <hpp/manipulation/steering-method/graph.hh>
37 #include <hpp/pinocchio/liegroup-element.hh>
38 #include <hpp/pinocchio/urdf/util.hh>
39
40 #include "hpp/manipulation/device.hh"
41 #include "hpp/manipulation/graph-path-validation.hh"
42 #include "hpp/manipulation/graph/edge.hh"
43 #include "hpp/manipulation/graph/graph.hh"
44 #include "hpp/manipulation/graph/state-selector.hh"
45 #include "hpp/manipulation/graph/state.hh"
46 #include "hpp/manipulation/problem.hh"
47
48 using hpp::core::SteeringMethodPtr_t;
49 using hpp::core::steeringMethod::Straight;
50
51 typedef std::vector<hpp::manipulation::graph::GraphComponentPtr_t>
52 GraphComponents_t;
53
54 namespace hpp_test {
55 using hpp::core::Configuration_t;
56 using hpp::manipulation::graph::EdgePtr_t;
57 using hpp::manipulation::graph::Edges_t;
58 using hpp::manipulation::graph::Graph;
59 using hpp::manipulation::graph::GraphComponent;
60 using hpp::manipulation::graph::GraphPtr_t;
61 using hpp::manipulation::graph::StatePtr_t;
62 using hpp::manipulation::graph::StateSelectorPtr_t;
63
64 hpp::manipulation::DevicePtr_t robot;
65 Configuration_t q1, q2;
66
67 GraphComponents_t components;
68 GraphPtr_t graph_;
69 StateSelectorPtr_t ns;
70 StatePtr_t n1;
71 StatePtr_t n2;
72 EdgePtr_t e11;
73 EdgePtr_t e21;
74 EdgePtr_t e12;
75 EdgePtr_t e22;
76
77 2 void initialize(bool ur5) {
78 2 components.clear();
79
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 robot = hpp::manipulation::Device::create("test-robot");
80 hpp::manipulation::ProblemPtr_t problem(
81
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 hpp::manipulation::Problem::create(robot));
82
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
 2 if (ur5) {
83
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
 1 hpp::pinocchio::urdf::loadModel(robot, 0, "ur5/", "anchor",
84 "package://ur_description/urdf/"
85 "ur5_joint_limited_robot.urdf",
86 "package://ur_description/srdf/"
87 "ur5_joint_limited_robot.srdf");
88 }
89 SteeringMethodPtr_t sm(
90
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 hpp::manipulation::steeringMethod::Graph::create(problem));
91 2 hpp::core::ProblemPtr_t pb(problem);
92 2 pb->steeringMethod(sm);
93
94
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 graph_ = Graph::create("manipulation-graph", robot, problem);
95
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 components.push_back(graph_);
96
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 graph_->maxIterations(20);
97
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 graph_->errorThreshold(1e-4);
98
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 ns = graph_->createStateSelector("node-selector");
99
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 n1 = ns->createState("node 1");
100
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 components.push_back(n1);
101
2/4
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
 2 n2 = ns->createState("node 2");
102
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 components.push_back(n2);
103
2/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 e11 = n1->linkTo("edge 11", n1);
104
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 components.push_back(e11);
105
2/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 e21 = n2->linkTo("edge 21", n1);
106
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 components.push_back(e21);
107
2/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 e12 = n1->linkTo("edge 12", n2);
108
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 components.push_back(e12);
109
2/4
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 e22 = n2->linkTo("edge 22", n2);
110
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 components.push_back(e22);
111
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 graph_->initialize();
112
113
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 q1 = Configuration_t::Zero(6);
114
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 q2 = Configuration_t::Zero(6);
115
6/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 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.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
 2 q1 << 1, 1, 1, 0, 2.5, -1.9;
116
6/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 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.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
 2 q2 << 2, 0, 1, 0, 2.5, -1.9;
117 2 }
118 } // namespace hpp_test
119
120
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(GraphStructure) {
121 using namespace hpp_test;
122 using hpp_test::graph_;
123
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 initialize(false);
124
125 // Check that GraphComponent keeps track of all object properly.
126 2 size_t index = 0;
127 2 for (GraphComponents_t::iterator it = components.begin();
128
2/2
✓ Branch 2 taken 7 times.
✓ Branch 3 taken 1 times.
 16 it != components.end(); ++it) {
129
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 7 times.
 14 BOOST_CHECK_MESSAGE(*it == graph_->get(index).lock(),
130 "GraphComponent class do not track properly "
131 "GraphComponents_t inherited objects");
132 14 index++;
133 }
134
135 // Test function Graph::getEdge
136 2 StatePtr_t from(n1), to(n2);
137
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 Edges_t checkPossibleEdges, possibleEdges = graph_->getEdges(from, to);
138
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 checkPossibleEdges.push_back(e12);
139
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
 4 for (size_t j = 0; j < possibleEdges.size(); j++)
140
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
 2 BOOST_CHECK_MESSAGE(possibleEdges[j] == checkPossibleEdges[j],
141 "Possible edge j = " << j);
142
143
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 Configuration_t cfg;
144
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 4 StatePtr_t node = graph_->getState(cfg);
145
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
 2 BOOST_CHECK(node == n1);
146 2 }
147
148
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(Initialization) {
149 using namespace hpp_test;
150 using hpp::constraints::ComparisonTypes_t;
151 using hpp::constraints::Equality;
152 using hpp::constraints::EqualToZero;
153 using hpp::constraints::ImplicitPtr_t;
154 using hpp::constraints::LockedJoint;
155 using hpp::manipulation::graph::Edge;
156 using hpp::manipulation::graph::EdgePtr_t;
157 using hpp::pinocchio::LiegroupElement;
158 using hpp::pinocchio::LiegroupSpace;
159 using hpp_test::graph_;
160
161
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 initialize(true);
162
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 hpp::manipulation::DevicePtr_t robot(graph_->robot());
163
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 4 ImplicitPtr_t constraint(LockedJoint::create(
164
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 6 robot->jointAt(1), LiegroupElement(LiegroupSpace::R1(true))));
165
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 graph_->addNumericalConstraint(constraint);
166 // Check that states refuse parameterizable constraints
167
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 2 constraint->comparisonType(ComparisonTypes_t(1, Equality));
168
11/34
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 39 not taken.
✓ Branch 40 taken 1 times.
✓ Branch 43 taken 1 times.
✗ Branch 44 not taken.
✓ Branch 47 taken 1 times.
✗ Branch 48 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✗ Branch 63 not taken.
✓ Branch 64 taken 1 times.
 4 BOOST_CHECK_THROW(n1->addNumericalConstraint(constraint), std::logic_error);
169
170
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 1 times.
 16 for (std::size_t i = 0; i < components.size(); ++i) {
171 14 EdgePtr_t edge(HPP_DYNAMIC_PTR_CAST(Edge, components[i]));
172
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 3 times.
 14 if (edge) {
173 try {
174
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 8 edge->targetConstraint();
175 ✗ BOOST_CHECK(false && "should have thrown.");
176
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 8 } catch (const std::logic_error& exc) {
177
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 16 std::string msg(exc.what());
178
7/14
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 4 times.
 8 BOOST_CHECK(
179 msg ==
180 std::string("The graph should have been initialized first."));
181 8 }
182 }
183 14 }
184 2 }
185