GCC Code Coverage Report

Directory: ./
File: src/interpolated-path.cc
Date: 2025-03-10 11:18:21
Exec Total Coverage
Lines: 23 148 15.5%
Branches: 24 286 8.4%
Line Branch Exec Source
1 // Copyright (c) 2015, 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/interpolated-path.hh>
31 #include <hpp/core/projection-error.hh>
32 #include <hpp/pinocchio/configuration.hh>
33 #include <hpp/pinocchio/device.hh>
34 #include <hpp/pinocchio/liegroup.hh>
35 #include <hpp/util/debug.hh>
36
37 namespace hpp {
38 namespace core {
39 ✗ InterpolatedPath::InterpolatedPath(const DevicePtr_t& device,
40 ConfigurationIn_t init,
41 ✗ ConfigurationIn_t end, interval_t timeRange)
42 : parent_t(timeRange, device->configSize(), device->numberDof()),
43 ✗ device_(device) {
44 ✗ assert(init.size() == device_->configSize());
45 ✗ insert(timeRange.first, init);
46 ✗ insert(timeRange.second, end);
47 ✗ assert(device);
48 ✗ assert(length() >= 0);
49 ✗ assert(!constraints());
50 }
51
52 106 InterpolatedPath::InterpolatedPath(const DevicePtr_t& device,
53 ConfigurationIn_t init,
54 ConfigurationIn_t end, interval_t timeRange,
55 106 ConstraintSetPtr_t constraints)
56 : parent_t(timeRange, device->configSize(), device->numberDof(),
57 constraints),
58
1/2
✓ Branch 7 taken 106 times.
✗ Branch 8 not taken.
 106 device_(device) {
59
2/4
✓ Branch 3 taken 106 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 106 times.
 106 assert(init.size() == device_->configSize());
60
2/4
✓ Branch 1 taken 106 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 106 times.
✗ Branch 5 not taken.
 106 insert(timeRange.first, init);
61
2/4
✓ Branch 1 taken 106 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 106 times.
✗ Branch 5 not taken.
 106 insert(timeRange.second, end);
62
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 106 times.
 106 assert(device);
63
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 106 times.
 106 assert(length() >= 0);
64 106 }
65
66 ✗ InterpolatedPath::InterpolatedPath(const PathPtr_t& path,
67 const DevicePtr_t& device,
68 ✗ const std::size_t& nbSamples)
69 : parent_t(path->timeRange(), device->configSize(), device->numberDof(),
70 path->constraints()),
71 ✗ device_(device)
72
73 {
74 ✗ assert(path->initial().size() == device_->configSize());
75 ✗ insert(timeRange().first, path->initial());
76 ✗ insert(timeRange().second, path->end());
77 ✗ assert(device);
78 ✗ assert(path->length() >= 0);
79
80 ✗ const value_type dl = path->length() / (value_type)(nbSamples + 1);
81 ✗ Configuration_t q(device->configSize());
82 ✗ for (std::size_t iS = 0; iS < nbSamples; ++iS) {
83 ✗ const value_type u = timeRange().first + dl * (value_type)(iS + 1);
84 ✗ if (!path->eval(q, u))
85 ✗ throw projection_error("could not build InterpolatedPath");
86 ✗ insert(u, q);
87 }
88 }
89
90 ✗ InterpolatedPath::InterpolatedPath(const InterpolatedPath& path)
91 ✗ : parent_t(path), device_(path.device_), configs_(path.configs_) {
92 ✗ assert(initial().size() == device_->configSize());
93 }
94
95 ✗ InterpolatedPath::InterpolatedPath(const InterpolatedPath& path,
96 ✗ const ConstraintSetPtr_t& constraints)
97 : parent_t(path, constraints),
98 ✗ device_(path.device_),
99 ✗ configs_(path.configs_) {}
100
101 ✗ InterpolatedPathPtr_t InterpolatedPath::create(const PathPtr_t& path,
102 const DevicePtr_t& device,
103 const std::size_t& nbSamples) {
104 // TODO: If it is a path vector, should we get the waypoints and build
105 // a interpolated path from those waypoints ?
106 ✗ InterpolatedPath* ptr = new InterpolatedPath(path, device, nbSamples);
107 ✗ InterpolatedPathPtr_t shPtr(ptr);
108 ✗ ptr->init(shPtr);
109 ✗ return shPtr;
110 }
111
112 106 void InterpolatedPath::init(InterpolatedPathPtr_t self) {
113
1/2
✓ Branch 2 taken 106 times.
✗ Branch 3 not taken.
 106 parent_t::init(self);
114 106 weak_ = self;
115 106 checkPath();
116 106 }
117
118 ✗ void InterpolatedPath::initCopy(InterpolatedPathPtr_t self) {
119 ✗ parent_t::init(self);
120 ✗ weak_ = self;
121 ✗ checkPath();
122 }
123
124 56 bool InterpolatedPath::impl_compute(ConfigurationOut_t result,
125 value_type param) const {
126
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 56 times.
 56 assert(param >= paramRange().first);
127
3/6
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 56 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 56 times.
 56 if (param == paramRange().first || paramLength() == 0) {
128 ✗ result.noalias() = initial();
129 ✗ return true;
130 }
131
2/4
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 56 times.
 56 assert(fabs(configs_.rbegin()->first - paramRange().second) <
132 Eigen::NumTraits<value_type>::dummy_precision());
133
2/4
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 56 times.
 56 assert(param < configs_.rbegin()->first +
134 Eigen::NumTraits<value_type>::dummy_precision());
135
2/4
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
 56 if (param >= configs_.rbegin()->first) {
136
1/2
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
 56 param = configs_.rbegin()->first;
137
3/6
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 56 times.
✗ Branch 8 not taken.
 56 result.noalias() = end();
138 56 return true;
139 }
140 ✗ InterpolationPoints_t::const_iterator itA = configs_.lower_bound(param);
141 ✗ assert(itA != configs_.end());
142 ✗ InterpolationPoints_t::const_iterator itB = itA;
143 ✗ --itB;
144 ✗ const value_type T = itA->first - itB->first;
145 ✗ const value_type u = (param - itB->first) / T;
146
147 ✗ pinocchio::interpolate<hpp::pinocchio::RnxSOnLieGroupMap>(
148 ✗ device_, itB->second, itA->second, u, result);
149 ✗ return true;
150 }
151
152 ✗ void InterpolatedPath::impl_derivative(vectorOut_t result, const value_type& s,
153 size_type order) const {
154 ✗ value_type param(s);
155 ✗ assert(param >= paramRange().first);
156 ✗ if (paramRange().first == paramRange().second) {
157 ✗ result.setZero();
158 ✗ return;
159 }
160 ✗ InterpolationPoints_t::const_iterator itA;
161 ✗ InterpolationPoints_t::const_iterator itB;
162 ✗ assert(fabs(configs_.rbegin()->first - paramRange().second) <
163 Eigen::NumTraits<value_type>::dummy_precision());
164 ✗ if (param >= configs_.rbegin()->first) {
165 ✗ param = configs_.rbegin()->first;
166 ✗ itA = configs_.end();
167 ✗ --itA;
168 ✗ itB = itA;
169 ✗ --itB;
170 } else {
171 ✗ itA = configs_.upper_bound(param);
172 ✗ itB = configs_.lower_bound(param);
173 ✗ if (itB == itA) {
174 ✗ if (itB == configs_.begin()) {
175 ✗ ++itA;
176 } else {
177 ✗ --itB;
178 }
179 }
180 }
181 ✗ assert(itA != configs_.end());
182 ✗ const value_type T = itA->first - itB->first;
183 ✗ if (order > 1) {
184 ✗ result.setZero();
185 ✗ return;
186 }
187 ✗ if (order == 1) {
188 ✗ pinocchio::difference<hpp::pinocchio::RnxSOnLieGroupMap>(
189 ✗ device_, itA->second, itB->second, result);
190 ✗ result = (1 / T) * result;
191 }
192 }
193
194 ✗ void InterpolatedPath::impl_velocityBound(vectorOut_t result,
195 const value_type& t0,
196 const value_type& t1) const {
197 ✗ InterpolationPoints_t::const_iterator next = configs_.lower_bound(t0);
198 ✗ InterpolationPoints_t::const_iterator current = next;
199 ✗ ++next;
200
201 ✗ result.setZero();
202 ✗ vector_t tmp(result.size());
203 ✗ while (t1 > current->first) {
204 ✗ pinocchio::difference<hpp::pinocchio::RnxSOnLieGroupMap>(
205 ✗ device_, next->second, current->second, tmp);
206 ✗ const value_type T = next->first - current->first;
207 ✗ result.noalias() = result.cwiseMax(tmp.cwiseAbs() / T);
208 ✗ ++current;
209 ✗ ++next;
210 }
211 }
212
213 ✗ PathPtr_t InterpolatedPath::impl_extract(const interval_t& subInterval) const {
214 // Length is assumed to be proportional to interval range
215 ✗ const bool reverse = (subInterval.first > subInterval.second);
216 ✗ const value_type tmin = (reverse) ? subInterval.second : subInterval.first;
217 ✗ const value_type tmax = (reverse) ? subInterval.first : subInterval.second;
218 ✗ const value_type l = tmax - tmin;
219
220 bool success;
221 ✗ Configuration_t q1(configAtParam(subInterval.first, success));
222 ✗ if (!success)
223 ✗ throw projection_error(
224 ✗ "Failed to apply constraints in InterpolatedPath::extract");
225 ✗ Configuration_t q2(configAtParam(subInterval.second, success));
226 ✗ if (!success)
227 ✗ throw projection_error(
228 ✗ "Failed to apply constraints in InterpolatedPath::extract");
229 // TODO if the constraint as a varying right hand side, then the generated
230 // path will not be correct.
231 // In order to propagate the correct time information, one must either:
232 // - implement some extraction method for the constraints.
233 // - make class Path tolerate reversed time range, i.e. (t1, t2) with t1 > t2.
234 // The second option shouldn't be hard to implement but may be hard to
235 // propagate in all the code.
236 InterpolatedPathPtr_t result =
237 ✗ InterpolatedPath::create(device_, q1, q2, l, constraints());
238
239 ✗ InterpolationPoints_t::const_iterator it = configs_.upper_bound(tmin);
240 ✗ if (reverse)
241 ✗ for (; it->first < tmax; ++it)
242 ✗ result->insert(l - (it->first - tmin), it->second);
243 else
244 ✗ for (; it->first < tmax; ++it) result->insert(it->first - tmin, it->second);
245
246 ✗ return result;
247 }
248
249 ✗ PathPtr_t InterpolatedPath::reverse() const {
250 ✗ const value_type& l = paramLength();
251
252 InterpolatedPathPtr_t result =
253 ✗ InterpolatedPath::create(device_, end(), initial(), l, constraints());
254
255 ✗ if (configs_.size() > 2) {
256 ✗ InterpolationPoints_t::const_reverse_iterator it = configs_.rbegin();
257 ✗ ++it;
258 ✗ InterpolationPoints_t::const_reverse_iterator itEnd = configs_.rend();
259 ✗ --itEnd;
260 ✗ for (; it != itEnd; ++it) result->insert(l - it->first, it->second);
261 }
262
263 ✗ return result;
264 }
265
266 ✗ DevicePtr_t InterpolatedPath::device() const { return device_; }
267 } // namespace core
268 } // namespace hpp
269