Directory: | ./ |
---|---|
File: | src/path.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 102 | 160 | 63.8% |
Branches: | 62 | 267 | 23.2% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2014 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 | #include <boost/serialization/utility.hpp> | ||
31 | #include <boost/serialization/weak_ptr.hpp> | ||
32 | #include <hpp/core/config-projector.hh> | ||
33 | #include <hpp/core/path.hh> | ||
34 | #include <hpp/core/time-parameterization.hh> | ||
35 | #include <hpp/pinocchio/configuration.hh> | ||
36 | #include <hpp/util/debug.hh> | ||
37 | #include <hpp/util/indent.hh> | ||
38 | #include <hpp/util/serialization.hh> | ||
39 | |||
40 | #include "extracted-path.hh" | ||
41 | |||
42 | namespace hpp { | ||
43 | namespace core { | ||
44 | namespace timeParameterization { | ||
45 | class HPP_CORE_LOCAL Shift : public TimeParameterization { | ||
46 | public: | ||
47 | typedef shared_ptr<Shift> Ptr_t; | ||
48 | |||
49 | 2 | static TimeParameterizationPtr_t createWithCheck(TimeParameterizationPtr_t tp, | |
50 | value_type t, value_type s) { | ||
51 |
2/4✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | if (t == 0 && s == 0) |
52 | 2 | return tp; | |
53 | else | ||
54 | ✗ | return create(tp, t, s); | |
55 | } | ||
56 | |||
57 | ✗ | static Ptr_t create(TimeParameterizationPtr_t tp, value_type t, | |
58 | value_type s) { | ||
59 | ✗ | Ptr_t shift = HPP_DYNAMIC_PTR_CAST(Shift, tp); | |
60 | ✗ | if (shift) | |
61 | ✗ | return Ptr_t(new Shift(shift->tp_, shift->t_ + t, shift->s_ + s)); | |
62 | else | ||
63 | ✗ | return Ptr_t(new Shift(tp, t, s)); | |
64 | } | ||
65 | |||
66 | ✗ | Shift(TimeParameterizationPtr_t tp, value_type t, value_type s) | |
67 | ✗ | : tp_(tp), t_(t), s_(s) {} | |
68 | |||
69 | ✗ | value_type value(const value_type& t) const { | |
70 | ✗ | return tp_->value(t + t_) + s_; | |
71 | } | ||
72 | ✗ | value_type derivative(const value_type& t, const size_type& order) const { | |
73 | ✗ | return tp_->derivative(t + t_, order); | |
74 | } | ||
75 | value_type impl_derivativeBound(const value_type& l, | ||
76 | const value_type& u) const { | ||
77 | return tp_->derivativeBound(l + t_, u + t_); | ||
78 | } | ||
79 | |||
80 | ✗ | TimeParameterizationPtr_t copy() const { return create(tp_->copy(), t_, s_); } | |
81 | |||
82 | TimeParameterizationPtr_t tp_; | ||
83 | value_type t_; | ||
84 | value_type s_; | ||
85 | }; | ||
86 | } // namespace timeParameterization | ||
87 | |||
88 | // Constructor with constraints | ||
89 | 1873088 | Path::Path(const interval_t& interval, size_type outputSize, | |
90 | size_type outputDerivativeSize, | ||
91 | 1873088 | const ConstraintSetPtr_t& constraints) | |
92 | 1873088 | : paramRange_(interval), | |
93 | 1873088 | timeRange_(interval), | |
94 | 1873088 | outputSize_(outputSize), | |
95 | 1873088 | outputDerivativeSize_(outputDerivativeSize), | |
96 | 1873088 | constraints_() { | |
97 |
2/2✓ Branch 1 taken 1853012 times.
✓ Branch 2 taken 20076 times.
|
1873088 | if (constraints) { |
98 |
1/2✓ Branch 2 taken 1853012 times.
✗ Branch 3 not taken.
|
1853012 | constraints_ = HPP_STATIC_PTR_CAST(ConstraintSet, constraints->copy()); |
99 | } | ||
100 | 1873088 | } | |
101 | |||
102 | // Constructor without constraints | ||
103 | 6506 | Path::Path(const interval_t& interval, size_type outputSize, | |
104 | 6506 | size_type outputDerivativeSize) | |
105 | 6506 | : paramRange_(interval), | |
106 | 6506 | timeRange_(interval), | |
107 | 6506 | outputSize_(outputSize), | |
108 | 6506 | outputDerivativeSize_(outputDerivativeSize), | |
109 | 6506 | constraints_() {} | |
110 | |||
111 | // Copy constructor | ||
112 | 1864006 | Path::Path(const Path& path) | |
113 | 1864006 | : paramRange_(path.paramRange_), | |
114 | 1864006 | timeRange_(path.timeRange_), | |
115 | 1864006 | outputSize_(path.outputSize_), | |
116 | 1864006 | outputDerivativeSize_(path.outputDerivativeSize_), | |
117 | 1864006 | constraints_(), | |
118 | 1864006 | timeParam_() { | |
119 |
2/2✓ Branch 1 taken 1852843 times.
✓ Branch 2 taken 11163 times.
|
1864006 | if (path.constraints_) { |
120 | constraints_ = | ||
121 |
1/2✓ Branch 2 taken 1852843 times.
✗ Branch 3 not taken.
|
1852843 | HPP_STATIC_PTR_CAST(ConstraintSet, path.constraints_->copy()); |
122 | } | ||
123 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 1864006 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
1864006 | if (path.timeParam_) timeParam_ = path.timeParam_->copy(); |
124 | 1864006 | } | |
125 | |||
126 | 24 | Path::Path(const Path& path, const ConstraintSetPtr_t& constraints) | |
127 | 24 | : paramRange_(path.paramRange_), | |
128 | 24 | timeRange_(path.timeRange_), | |
129 | 24 | outputSize_(path.outputSize_), | |
130 | 24 | outputDerivativeSize_(path.outputDerivativeSize_), | |
131 | 24 | constraints_(constraints), | |
132 | 24 | timeParam_() { | |
133 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
|
24 | assert(!path.constraints_); |
134 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 24 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
24 | if (path.timeParam_) timeParam_ = path.timeParam_->copy(); |
135 | 24 | } | |
136 | |||
137 | // Initialization after creation | ||
138 | 3743624 | void Path::init(const PathWkPtr_t& self) { weak_ = self; } | |
139 | |||
140 | 2090122 | bool Path::applyConstraints(ConfigurationOut_t result, | |
141 | const value_type& param) const { | ||
142 |
2/2✓ Branch 1 taken 231204 times.
✓ Branch 2 taken 1858916 times.
|
2090122 | if (!constraints_) return true; |
143 |
2/2✓ Branch 4 taken 1852680 times.
✓ Branch 5 taken 6236 times.
|
1858916 | if (constraints_->configProjector()) |
144 |
1/2✓ Branch 4 taken 1852680 times.
✗ Branch 5 not taken.
|
1852680 | constraints_->configProjector()->rightHandSideAt(param); |
145 |
1/2✓ Branch 3 taken 1858916 times.
✗ Branch 4 not taken.
|
1858916 | return constraints_->apply(result); |
146 | } | ||
147 | |||
148 | 929649 | void Path::derivative(vectorOut_t result, const value_type& time, | |
149 | size_type order) const { | ||
150 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 929649 times.
|
929649 | if (timeParam_) { |
151 | ✗ | switch (order) { | |
152 | ✗ | case 1: | |
153 | ✗ | impl_derivative(result, timeParam_->value(time), 1); | |
154 | ✗ | result *= timeParam_->derivative(time, 1); | |
155 | ✗ | break; | |
156 | ✗ | case 2: { | |
157 | ✗ | vector_t tmp(outputDerivativeSize()); | |
158 | ✗ | impl_derivative(tmp, timeParam_->value(time), 2); | |
159 | ✗ | value_type der = timeParam_->derivative(time, 1); | |
160 | ✗ | result.noalias() = tmp * (der * der); | |
161 | |||
162 | ✗ | impl_derivative(tmp, timeParam_->value(time), 1); | |
163 | ✗ | result.noalias() += tmp * timeParam_->derivative(time, 2); | |
164 | ✗ | break; | |
165 | } | ||
166 | ✗ | default: | |
167 | ✗ | throw std::invalid_argument( | |
168 | ✗ | "Cannot compute the derivative of order greater than 2."); | |
169 | } | ||
170 | } else { | ||
171 |
1/2✓ Branch 2 taken 929649 times.
✗ Branch 3 not taken.
|
929649 | impl_derivative(result, time, order); |
172 | } | ||
173 | 929649 | } | |
174 | |||
175 | 20288 | PathPtr_t Path::extract(const interval_t& subInterval) const { | |
176 | 20288 | PathPtr_t res; | |
177 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 20286 times.
|
20288 | if (timeParam_) { |
178 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | interval_t paramInterval(timeParam_->value(subInterval.first), |
179 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
4 | timeParam_->value(subInterval.second)); |
180 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | res = this->impl_extract(paramInterval); |
181 | // TODO Child class that reimplement impl_extract may return | ||
182 | // a path whose paramRange has been shifted to 0. We must then shift | ||
183 | // the time parameterization. | ||
184 | value_type shift_t, shift_s; | ||
185 | 2 | interval_t timeInterval; | |
186 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
|
2 | if (subInterval.first > subInterval.second) { |
187 | 1 | shift_t = 0; | |
188 | 1 | shift_s = res->paramRange().first - paramInterval.second; | |
189 | |||
190 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (shift_s != 0) { |
191 | ✗ | shift_t = subInterval.second; | |
192 | ✗ | timeInterval.first = 0; | |
193 | ✗ | timeInterval.second = subInterval.first - subInterval.second; | |
194 | } else { | ||
195 | 1 | shift_t = 0; | |
196 | 1 | timeInterval = interval_t(subInterval.second, subInterval.first); | |
197 | } | ||
198 | } else { | ||
199 | 1 | shift_t = 0; | |
200 | 1 | shift_s = res->paramRange().first - paramInterval.first; | |
201 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (shift_s != 0) { |
202 | ✗ | shift_t = subInterval.first; | |
203 | ✗ | timeInterval.first = 0; | |
204 | ✗ | timeInterval.second = subInterval.second - subInterval.first; | |
205 | } else { | ||
206 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
|
1 | assert(res->paramRange() == paramInterval); |
207 | 1 | shift_t = 0; | |
208 | 1 | timeInterval = subInterval; | |
209 | } | ||
210 | } | ||
211 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | timeParameterization::Shift::createWithCheck(timeParam_, shift_t, shift_s); |
212 | #ifndef NDEBUG | ||
213 | 2 | interval_t pr = res->paramRange(); | |
214 | #endif // NDEBUG | ||
215 |
2/4✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
|
2 | res->timeParameterization(timeParam_->copy(), timeInterval); |
216 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
|
2 | assert(pr == res->paramRange()); |
217 | } else { | ||
218 |
1/2✓ Branch 1 taken 20286 times.
✗ Branch 2 not taken.
|
20286 | res = this->impl_extract(subInterval); |
219 | } | ||
220 | 20288 | return res; | |
221 | } | ||
222 | |||
223 | 45 | PathPtr_t Path::impl_extract(const interval_t& paramInterval) const { | |
224 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 45 times.
|
45 | if (paramInterval == paramRange_) return this->copy(); |
225 |
1/2✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
|
45 | return ExtractedPath::create(weak_.lock(), paramInterval); |
226 | } | ||
227 | |||
228 | 53 | PathPtr_t Path::reverse() const { | |
229 | 53 | interval_t interval; | |
230 | 53 | interval.first = this->timeRange_.second; | |
231 | 53 | interval.second = this->timeRange_.first; | |
232 |
1/2✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
|
106 | return this->extract(interval); |
233 | } | ||
234 | |||
235 | 3824824 | void Path::checkPath() const { | |
236 | using pinocchio::displayConfig; | ||
237 |
2/2✓ Branch 2 taken 3706015 times.
✓ Branch 3 taken 118809 times.
|
3824824 | if (constraints()) { |
238 |
2/2✓ Branch 4 taken 3705410 times.
✓ Branch 5 taken 605 times.
|
3706015 | if (constraints_->configProjector()) |
239 |
1/2✓ Branch 4 taken 3705410 times.
✗ Branch 5 not taken.
|
3705410 | constraints_->configProjector()->rightHandSideAt(paramRange_.first); |
240 |
3/6✓ Branch 4 taken 3706015 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3706015 times.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✓ Branch 12 taken 3706015 times.
|
3706015 | if (!constraints()->isSatisfied(initial())) { |
241 | ✗ | std::stringstream oss; | |
242 | hppDout(error, *constraints()); | ||
243 | hppDout(error, initial().transpose()); | ||
244 | oss << "Initial configuration of path does not satisfy the path " | ||
245 | "constraints: q=" | ||
246 | ✗ | << displayConfig(initial()) << "; error="; | |
247 | ✗ | vector_t error; | |
248 | ✗ | constraints()->isSatisfied(initial(), error); | |
249 | ✗ | oss << displayConfig(error) << "."; | |
250 | ✗ | throw projection_error(oss.str().c_str()); | |
251 | } | ||
252 |
2/2✓ Branch 4 taken 3705410 times.
✓ Branch 5 taken 605 times.
|
3706015 | if (constraints_->configProjector()) |
253 |
1/2✓ Branch 4 taken 3705410 times.
✗ Branch 5 not taken.
|
3705410 | constraints_->configProjector()->rightHandSideAt(paramRange_.second); |
254 |
8/20✓ Branch 2 taken 3706015 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 3706015 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3706015 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3706015 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✓ Branch 16 taken 3706015 times.
✓ Branch 17 taken 3706015 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 3706015 times.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 3706015 times.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
|
3706015 | if (constraints() && !constraints()->isSatisfied(end())) { |
255 | ✗ | std::stringstream oss; | |
256 | hppDout(error, *constraints()); | ||
257 | hppDout(error, displayConfig(end())); | ||
258 | oss << "End configuration of path does not satisfy the path " | ||
259 | "constraints: q=" | ||
260 | ✗ | << displayConfig(end()) << "; error="; | |
261 | ✗ | vector_t error; | |
262 | ✗ | constraints()->isSatisfied(end(), error); | |
263 | ✗ | Configuration_t q = end(); | |
264 | ✗ | constraints()->apply(q); | |
265 | ✗ | oss << displayConfig(error) << "; qproj=" << displayConfig(q) << ".\n" | |
266 | ✗ | << *constraints(); | |
267 | ✗ | throw projection_error(oss.str().c_str()); | |
268 | } | ||
269 | } | ||
270 | 3824824 | } | |
271 | |||
272 | ✗ | std::ostream& Path::print(std::ostream& os) const { | |
273 | ✗ | os << "time in [ " << timeRange().first << ", " << timeRange().second << " ]"; | |
274 | ✗ | if (timeParam_) | |
275 | ✗ | os << ", param in [ " << paramRange().first << ", " << paramRange().second | |
276 | ✗ | << " ]"; | |
277 | ✗ | os << iendl; | |
278 | ✗ | return os; | |
279 | } | ||
280 | |||
281 | template <class Archive> | ||
282 | 60 | void Path::serialize(Archive& ar, const unsigned int version) { | |
283 | (void)version; | ||
284 |
1/2✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
|
60 | ar& BOOST_SERIALIZATION_NVP(timeRange_); |
285 |
1/2✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
|
60 | ar& BOOST_SERIALIZATION_NVP(outputSize_); |
286 |
1/2✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
|
60 | ar& BOOST_SERIALIZATION_NVP(outputDerivativeSize_); |
287 |
1/2✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
|
60 | ar& BOOST_SERIALIZATION_NVP(constraints_); |
288 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
|
30 | if (Archive::is_saving::value && timeParam_) |
289 | ✗ | throw std::logic_error( | |
290 | "At the moment, it is not possible to serialize " | ||
291 | "a Path with time parameterization."); | ||
292 | // ar & BOOST_SERIALIZATION_NVP(timeParam_); | ||
293 | if (!Archive::is_saving::value) { // handles paramRange_ | ||
294 | 30 | timeRange(timeRange_); | |
295 | } | ||
296 |
1/2✓ Branch 2 taken 30 times.
✗ Branch 3 not taken.
|
60 | ar& BOOST_SERIALIZATION_NVP(weak_); |
297 | 60 | } | |
298 | |||
299 | HPP_SERIALIZATION_IMPLEMENT(Path); | ||
300 | } // namespace core | ||
301 | } // namespace hpp | ||
302 |