GCC Code Coverage Report


Directory: ./
File: tests/test-path-extraction.cc
Date: 2024-12-13 16:14:03
Exec Total Coverage
Lines: 67 87 77.0%
Branches: 179 378 47.4%

Line Branch Exec Source
1 //
2 // Copyright (c) 2016 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 #define BOOST_TEST_MODULE path_extraction
31
32 #include <coal/math/transform.h>
33
34 #include <boost/test/included/unit_test.hpp>
35 #include <hpp/core/path-vector.hh>
36 #include <hpp/core/problem.hh>
37 #include <hpp/core/steering-method/straight.hh>
38 #include <hpp/core/time-parameterization/polynomial.hh>
39 #include <pinocchio/fwd.hpp>
40
41 BOOST_AUTO_TEST_SUITE(test_hpp_core)
42
43 using hpp::pinocchio::Configuration_t;
44 using hpp::pinocchio::ConfigurationOut_t;
45 using hpp::pinocchio::Device;
46 using hpp::pinocchio::DevicePtr_t;
47 using hpp::pinocchio::JointConstPtr_t;
48 using hpp::pinocchio::JointPtr_t;
49
50 using namespace hpp::core;
51
52 HPP_PREDEF_CLASS(LocalPath);
53 typedef std::shared_ptr<LocalPath> LocalPathPtr_t;
54
55 class LocalPath : public Path {
56 public:
57 32 virtual ~LocalPath() throw() {}
58 virtual bool impl_compute(ConfigurationOut_t configuration,
59 value_type t) const {
60 value_type alpha(t - timeRange().first /
61 (timeRange().second - timeRange().first));
62 configuration = (1 - alpha) * q1_ + alpha * q2_;
63 return true;
64 }
65
66 8 static LocalPathPtr_t create(const interval_t& interval) {
67
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
8 LocalPath* ptr(new LocalPath(interval));
68 8 LocalPathPtr_t shPtr(ptr);
69
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
8 ptr->init(shPtr);
70 8 return shPtr;
71 }
72
73 static LocalPathPtr_t createCopy(const LocalPathPtr_t& other) {
74 LocalPath* ptr(new LocalPath(*other));
75 LocalPathPtr_t shPtr(ptr);
76 ptr->init(shPtr);
77 return shPtr;
78 }
79
80 virtual Configuration_t initial() const { return q1_; }
81
82 virtual Configuration_t end() const { return q2_; }
83
84 virtual PathPtr_t copy() const { return createCopy(weak_.lock()); }
85
86 virtual PathPtr_t copy(const ConstraintSetPtr_t&) const {
87 return createCopy(weak_.lock());
88 }
89
90 virtual std::ostream& print(std::ostream& os) const {
91 os << "LocalPath between " << q1_[0] << " and " << q2_[0];
92 return os;
93 }
94
95 protected:
96
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
8 LocalPath(const interval_t& interval) : Path(interval, 1, 1), q1_(1), q2_(1) {
97
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 q1_[0] = interval.first;
98
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
8 q2_[0] = interval.second;
99 8 }
100
101 LocalPath(const LocalPath& other)
102 : Path(other), q1_(other.q1_), q2_(other.q2_) {}
103
104 8 void init(LocalPathWkPtr_t wkPtr) {
105
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
8 Path::init(wkPtr.lock());
106 8 weak_ = wkPtr;
107 8 }
108
109 private:
110 Configuration_t q1_, q2_;
111 LocalPathWkPtr_t weak_;
112 };
113
114
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(path_extraction_1) {
115
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 PathVectorPtr_t pv(PathVector::create(1, 1));
116 // l0 = 0.52843097738664657
117 2 interval_t inter0(std::make_pair(0, 0.52843097738664657));
118 // L0 + l1 = 2.633158413063464
119 2 interval_t inter1(std::make_pair(0, 2.1047274356768177));
120 // l0 + l1 + l2 = 3.331454231080471
121 2 interval_t inter2(std::make_pair(0.6751734883957865, 1.3734693064127934));
122 // l0 + l1 + l2 + l3 = 4.079212904814735
123 2 interval_t inter3(std::make_pair(0.83075838111776035, 1.578517054852024));
124
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 LocalPathPtr_t p0(LocalPath::create(inter0));
125
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 LocalPathPtr_t p1(LocalPath::create(inter1));
126
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 LocalPathPtr_t p2(LocalPath::create(inter2));
127
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 LocalPathPtr_t p3(LocalPath::create(inter3));
128
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pv->appendPath(p0);
129
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pv->appendPath(p1);
130
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pv->appendPath(p2);
131
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pv->appendPath(p3);
132
133 2 value_type tmin(3.7487655421722721), tmax(4.0792129048147352);
134
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 PathPtr_t extracted(pv->extract(std::make_pair(tmin, tmax)));
135
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
2 BOOST_CHECK_EQUAL(extracted->timeRange().first, 0);
136
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
2 BOOST_CHECK_CLOSE(extracted->timeRange().second, tmax - tmin,
137 std::numeric_limits<float>::epsilon());
138
7/14
✓ 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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_CLOSE(extracted->length(), tmax - tmin,
139 std::numeric_limits<float>::epsilon());
140
141
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 extracted = pv->extract(std::make_pair(tmax, tmin));
142
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
2 BOOST_CHECK_EQUAL(extracted->timeRange().first, 0);
143
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
2 BOOST_CHECK_CLOSE(extracted->timeRange().second, tmax - tmin,
144 std::numeric_limits<float>::epsilon());
145
7/14
✓ 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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_CLOSE(extracted->length(), tmax - tmin,
146 std::numeric_limits<float>::epsilon());
147 2 }
148
149
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(path_extraction_2) {
150
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 PathVectorPtr_t pv(PathVector::create(1, 1));
151 // l0 = 0.52843097738664657
152 2 interval_t inter0(std::make_pair(0, 0.52843097738664657));
153 // L0 + l1 = 2.633158413063464
154 2 interval_t inter1(std::make_pair(0, 2.1047274356768177));
155 // l0 + l1 + l2 = 3.331454231080471
156 2 interval_t inter2(std::make_pair(0.6751734883957865, 1.3734693064127934));
157 // l0 + l1 + l2 + l3 = 4.079212904814735
158 2 interval_t inter3(std::make_pair(0.83075838111776035, 1.578517054852024));
159
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 LocalPathPtr_t p0(LocalPath::create(inter0));
160
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 LocalPathPtr_t p1(LocalPath::create(inter1));
161
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 LocalPathPtr_t p2(LocalPath::create(inter2));
162
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 vector_t a(2);
163
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
2 a << 0, 0.5;
164
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
4 p2->timeParameterization(
165
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
4 TimeParameterizationPtr_t(new timeParameterization::Polynomial(a)),
166
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 interval_t(0, 2 * p2->length()));
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
2 LocalPathPtr_t p3(LocalPath::create(inter3));
168
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pv->appendPath(p0);
169
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pv->appendPath(p1);
170
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pv->appendPath(p2);
171
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
2 pv->appendPath(p3);
172
173 2 value_type tmin(3.7487655421722721), tmax(4.0792129048147352);
174
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 PathPtr_t extracted(pv->extract(std::make_pair(tmin, tmax)));
175
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
2 BOOST_CHECK_EQUAL(extracted->timeRange().first, 0);
176
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
2 BOOST_CHECK_CLOSE(extracted->timeRange().second, tmax - tmin,
177 std::numeric_limits<float>::epsilon());
178
7/14
✓ 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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_CLOSE(extracted->length(), tmax - tmin,
179 std::numeric_limits<float>::epsilon());
180
181
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
2 extracted = pv->extract(std::make_pair(tmax, tmin));
182
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 1 times.
2 BOOST_CHECK_EQUAL(extracted->timeRange().first, 0);
183
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 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
2 BOOST_CHECK_CLOSE(extracted->timeRange().second, tmax - tmin,
184 std::numeric_limits<float>::epsilon());
185
7/14
✓ 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 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
2 BOOST_CHECK_CLOSE(extracted->length(), tmax - tmin,
186 std::numeric_limits<float>::epsilon());
187 2 }
188
189 BOOST_AUTO_TEST_SUITE_END()
190