GCC Code Coverage Report


Directory: ./
File: src/interpolated-path.cc
Date: 2024-12-13 16:14:03
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