Directory: | ./ |
---|---|
File: | src/path-vector.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 83 | 123 | 67.5% |
Branches: | 65 | 186 | 34.9% |
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/vector.hpp> | ||
31 | #include <boost/serialization/weak_ptr.hpp> | ||
32 | #include <hpp/core/path-vector.hh> | ||
33 | #include <hpp/util/indent.hh> | ||
34 | #include <hpp/util/serialization.hh> | ||
35 | #include <stdexcept> | ||
36 | |||
37 | namespace hpp { | ||
38 | namespace core { | ||
39 | |||
40 | 2385 | std::size_t PathVector::rankAtParam(const value_type& param, | |
41 | value_type& localParam) const { | ||
42 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 2385 times.
|
2385 | assert(!timeParameterization()); |
43 |
1/4✗ Branch 1 not taken.
✓ Branch 2 taken 2385 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
2385 | if (paths_.empty()) throw std::runtime_error("PathVector is empty."); |
44 | 2385 | std::size_t res = 0; | |
45 | 2385 | localParam = param; | |
46 | 2385 | bool finished = false; | |
47 | |||
48 |
6/6✓ Branch 1 taken 6414 times.
✓ Branch 2 taken 623 times.
✓ Branch 3 taken 4652 times.
✓ Branch 4 taken 1762 times.
✓ Branch 5 taken 4652 times.
✓ Branch 6 taken 2385 times.
|
7037 | while (res + 1 < paths_.size() && !finished) { |
49 |
2/2✓ Branch 3 taken 2890 times.
✓ Branch 4 taken 1762 times.
|
4652 | if (localParam > paths_[res]->length()) { |
50 | 2890 | localParam -= paths_[res]->length(); | |
51 | 2890 | res++; | |
52 | } else { | ||
53 | 1762 | finished = true; | |
54 | } | ||
55 | } | ||
56 |
2/2✓ Branch 3 taken 3 times.
✓ Branch 4 taken 2382 times.
|
2385 | if (localParam > paths_[res]->length()) { |
57 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
|
3 | if (res != paths_.size() - 1) { |
58 | ✗ | throw std::runtime_error("localparam out of range."); | |
59 | } | ||
60 | 3 | localParam = paths_[res]->timeRange().second; | |
61 | } else { | ||
62 | 2382 | localParam += paths_[res]->timeRange().first; | |
63 | } | ||
64 |
1/2✗ Branch 4 not taken.
✓ Branch 5 taken 2385 times.
|
2385 | assert(localParam >= paths_[res]->timeRange().first - |
65 | std::numeric_limits<float>::epsilon()); | ||
66 |
1/2✗ Branch 4 not taken.
✓ Branch 5 taken 2385 times.
|
2385 | assert(localParam <= paths_[res]->timeRange().second + |
67 | std::numeric_limits<float>::epsilon()); | ||
68 | 2385 | return res; | |
69 | } | ||
70 | |||
71 | 1852844 | void PathVector::appendPath(const PathPtr_t& path) { | |
72 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1852844 times.
|
1852844 | assert(!timeParameterization()); |
73 |
1/2✓ Branch 1 taken 1852844 times.
✗ Branch 2 not taken.
|
1852844 | paths_.push_back(path); |
74 | 1852844 | interval_t tr = timeRange(); | |
75 |
1/2✓ Branch 2 taken 1852844 times.
✗ Branch 3 not taken.
|
1852844 | tr.second += path->length(); |
76 |
1/2✓ Branch 1 taken 1852844 times.
✗ Branch 2 not taken.
|
1852844 | timeRange(tr); |
77 | 1852844 | } | |
78 | |||
79 | 926596 | PathPtr_t PathVector::pathAtRank(std::size_t rank) const { | |
80 | 926596 | PathPtr_t copy; | |
81 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 926596 times.
|
926596 | if (constraints()) { |
82 | ✗ | if (paths_[rank]->constraints()) { | |
83 | ✗ | throw std::runtime_error( | |
84 | "Attempt to extract a path from a path vector where both " | ||
85 | ✗ | "are subject to constraints. This is not supported."); | |
86 | } else { | ||
87 | ✗ | ConstraintPtr_t constraintCopy(constraints()->copy()); | |
88 | ✗ | HPP_STATIC_CAST_REF_CHECK(ConstraintSet, *constraintCopy); | |
89 | ✗ | copy = paths_[rank]->copy( | |
90 | ✗ | HPP_STATIC_PTR_CAST(ConstraintSet, constraintCopy)); | |
91 | } | ||
92 | } else { | ||
93 |
1/2✓ Branch 3 taken 926596 times.
✗ Branch 4 not taken.
|
926596 | copy = paths_[rank]->copy(); |
94 | } | ||
95 | 926596 | return copy; | |
96 | } | ||
97 | |||
98 | 64 | void PathVector::concatenate(const PathVectorPtr_t& path) { | |
99 |
2/2✓ Branch 2 taken 926296 times.
✓ Branch 3 taken 64 times.
|
926360 | for (std::size_t i = 0; i < path->numberPaths(); ++i) { |
100 |
2/4✓ Branch 4 taken 926296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 926296 times.
✗ Branch 8 not taken.
|
926296 | appendPath(path->pathAtRank(i)->copy()); |
101 | } | ||
102 | 64 | } | |
103 | |||
104 | 15 | void PathVector::flatten(PathVectorPtr_t p) const { | |
105 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 15 times.
|
15 | assert(!timeParameterization()); |
106 |
2/2✓ Branch 1 taken 51 times.
✓ Branch 2 taken 15 times.
|
66 | for (std::size_t i = 0; i < numberPaths(); ++i) { |
107 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
51 | PathPtr_t path = pathAtRank(i); |
108 | 51 | PathVectorPtr_t pv = HPP_DYNAMIC_PTR_CAST(PathVector, path); | |
109 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 51 times.
|
51 | if (pv) |
110 | ✗ | pv->flatten(p); | |
111 | else | ||
112 |
1/2✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
|
51 | p->appendPath(path); |
113 | 51 | } | |
114 | 15 | } | |
115 | |||
116 | 720 | bool PathVector::impl_compute(ConfigurationOut_t result, | |
117 | value_type param) const { | ||
118 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 720 times.
|
720 | assert(!timeParameterization()); |
119 | // Find direct path in vector corresponding to parameter. | ||
120 | size_t rank; | ||
121 | value_type localParam; | ||
122 |
1/2✓ Branch 1 taken 720 times.
✗ Branch 2 not taken.
|
720 | rank = rankAtParam(param, localParam); |
123 | |||
124 | 720 | PathPtr_t subpath = paths_[rank]; | |
125 |
2/4✓ Branch 2 taken 720 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 720 times.
✗ Branch 6 not taken.
|
1440 | return subpath->eval(result, localParam); |
126 | 720 | } | |
127 | |||
128 | 1653 | void PathVector::impl_derivative(vectorOut_t result, const value_type& param, | |
129 | size_type order) const { | ||
130 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1653 times.
|
1653 | assert(!timeParameterization()); |
131 | // Find direct path in vector corresponding to parameter. | ||
132 | size_t rank; | ||
133 | value_type localParam; | ||
134 |
1/2✓ Branch 1 taken 1653 times.
✗ Branch 2 not taken.
|
1653 | rank = rankAtParam(param, localParam); |
135 | |||
136 | 1653 | PathPtr_t subpath = paths_[rank]; | |
137 |
2/4✓ Branch 2 taken 1653 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1653 times.
✗ Branch 6 not taken.
|
1653 | subpath->derivative(result, localParam, order); |
138 | 1653 | } | |
139 | |||
140 | 4 | inline const value_type& Iinit(const interval_t& I, bool reverse) { | |
141 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
|
4 | return (reverse ? I.second : I.first); |
142 | } | ||
143 | 4 | inline const value_type& Iend(const interval_t& I, bool reverse) { | |
144 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
|
4 | return (reverse ? I.first : I.second); |
145 | } | ||
146 | |||
147 | ✗ | void PathVector::impl_velocityBound(vectorOut_t bound, const value_type& param0, | |
148 | const value_type& param1) const { | ||
149 | ✗ | assert(!timeParameterization()); | |
150 | |||
151 | ✗ | bool reversed = param0 > param1 ? true : false; | |
152 | value_type localtinit, localtend; | ||
153 | ✗ | std::size_t iinit = rankAtParam(param0, localtinit), | |
154 | ✗ | iend = rankAtParam(param1, localtend); | |
155 | ✗ | if (iinit == iend) { | |
156 | ✗ | paths_[iinit]->velocityBound(bound, localtinit, localtend); | |
157 | } else { | ||
158 | ✗ | paths_[iinit]->velocityBound(bound, localtinit, | |
159 | ✗ | Iend(paths_[iinit]->timeRange(), reversed)); | |
160 | ✗ | int one = (reversed ? -1 : 1); | |
161 | ✗ | vector_t localbound(vector_t::Zero(bound.size())); | |
162 | ✗ | for (std::size_t i = iinit + one; (reversed && i > iend) || i < iend; | |
163 | ✗ | i += one) { | |
164 | ✗ | paths_[i]->velocityBound(localbound, paths_[i]->timeRange().first, | |
165 | ✗ | paths_[i]->timeRange().second); | |
166 | ✗ | bound = bound.cwiseMax(localbound); | |
167 | } | ||
168 | ✗ | paths_[iend]->velocityBound( | |
169 | ✗ | localbound, Iinit(paths_[iend]->timeRange(), reversed), localtend); | |
170 | ✗ | bound = bound.cwiseMax(localbound); | |
171 | } | ||
172 | } | ||
173 | |||
174 | 6 | PathPtr_t PathVector::impl_extract(const interval_t& subInterval) const { | |
175 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
|
6 | assert(!timeParameterization()); |
176 |
1/2✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
6 | PathVectorPtr_t path = create(outputSize(), outputDerivativeSize()); |
177 | 6 | bool reversed = subInterval.first > subInterval.second ? true : false; | |
178 | |||
179 | value_type localtinit, localtend; | ||
180 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | std::size_t iinit = rankAtParam(subInterval.first, localtinit), |
181 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
6 | iend = rankAtParam(subInterval.second, localtend); |
182 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 4 times.
|
6 | if (iinit == iend) { |
183 |
2/4✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
2 | path->appendPath(paths_[iinit]->extract(localtinit, localtend)); |
184 | } else { | ||
185 |
2/4✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
|
8 | path->appendPath(paths_[iinit]->extract( |
186 | 4 | localtinit, Iend(paths_[iinit]->timeRange(), reversed))); | |
187 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
|
4 | int one = (reversed ? -1 : 1); |
188 |
4/6✓ Branch 0 taken 1 times.
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
|
4 | for (std::size_t i = iinit + one; (reversed && i > iend) || i < iend; |
189 | ✗ | i += one) | |
190 | ✗ | path->appendPath(reversed ? paths_[i]->reverse() : paths_[i]); | |
191 |
2/4✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
|
8 | path->appendPath(paths_[iend]->extract( |
192 | 4 | Iinit(paths_[iend]->timeRange(), reversed), localtend)); | |
193 | } | ||
194 | 12 | return path; | |
195 | 6 | } | |
196 | |||
197 | 3 | PathPtr_t PathVector::reverse() const { | |
198 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
|
3 | assert(!timeParameterization()); |
199 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | PathVectorPtr_t ret = create(outputSize(), outputDerivativeSize()); |
200 |
1/2✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
3 | std::for_each(paths_.rbegin(), paths_.rend(), [&ret](const PathPtr_t& path) { |
201 |
1/2✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
|
8 | ret->appendPath(path->reverse()); |
202 | 8 | }); | |
203 | 6 | return ret; | |
204 | 3 | } | |
205 | |||
206 | ✗ | std::ostream& PathVector::print(std::ostream& os) const { | |
207 | ✗ | Path::print(os << "PathVector:") << incendl; | |
208 | ✗ | for (Paths_t::const_iterator itPath = paths_.begin(); itPath != paths_.end(); | |
209 | ✗ | ++itPath) { | |
210 | ✗ | os << (**itPath) << iendl; | |
211 | } | ||
212 | ✗ | return os << decindent; | |
213 | } | ||
214 | |||
215 | template <class Archive> | ||
216 | ✗ | void PathVector::serialize(Archive& ar, const unsigned int version) { | |
217 | using namespace boost::serialization; | ||
218 | (void)version; | ||
219 | ✗ | ar& make_nvp("base", base_object<Path>(*this)); | |
220 | ✗ | ar& BOOST_SERIALIZATION_NVP(paths_); | |
221 | ✗ | ar& BOOST_SERIALIZATION_NVP(weak_); | |
222 | } | ||
223 | |||
224 | HPP_SERIALIZATION_IMPLEMENT(PathVector); | ||
225 | } // namespace core | ||
226 | } // namespace hpp | ||
227 | |||
228 | BOOST_CLASS_EXPORT_IMPLEMENT(hpp::core::PathVector) | ||
229 |