Directory: | ./ |
---|---|
File: | src/interpolated-path.cc |
Date: | 2024-08-10 11:29:48 |
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 |