Directory: | ./ |
---|---|
File: | src/path-optimization/spline-gradient-based-abstract.cc |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 195 | 236 | 82.6% |
Branches: | 202 | 432 | 46.8% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // Copyright (c) 2017, 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/constraints/svd.hh> | ||
30 | #include <hpp/core/collision-path-validation-report.hh> | ||
31 | #include <hpp/core/config-projector.hh> | ||
32 | #include <hpp/core/interpolated-path.hh> | ||
33 | #include <hpp/core/path-optimization/spline-gradient-based-abstract.hh> | ||
34 | #include <hpp/core/path-validation.hh> | ||
35 | #include <hpp/core/problem.hh> | ||
36 | #include <hpp/core/straight-path.hh> | ||
37 | #include <hpp/pinocchio/device.hh> | ||
38 | #include <hpp/util/exception-factory.hh> | ||
39 | #include <hpp/util/timer.hh> | ||
40 | #include <path-optimization/spline-gradient-based/joint-bounds.hh> | ||
41 | |||
42 | namespace hpp { | ||
43 | namespace core { | ||
44 | using pinocchio::Device; | ||
45 | |||
46 | namespace pathOptimization { | ||
47 | typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, | ||
48 | Eigen::RowMajor> | ||
49 | RowMajorMatrix_t; | ||
50 | typedef Eigen::Map<const vector_t> ConstVectorMap_t; | ||
51 | typedef Eigen::Map<vector_t> VectorMap_t; | ||
52 | |||
53 | typedef Eigen::BlockIndex BlockIndex; | ||
54 | |||
55 | HPP_DEFINE_TIMECOUNTER(SGB_validatePath); | ||
56 | |||
57 | template <int NbRows> | ||
58 | VectorMap_t reshape(Eigen::Matrix<value_type, NbRows, Eigen::Dynamic, | ||
59 | Eigen::RowMajor>& parameters) { | ||
60 | return VectorMap_t(parameters.data(), parameters.size()); | ||
61 | } | ||
62 | |||
63 | template <int NbRows> | ||
64 | ConstVectorMap_t reshape(const Eigen::Matrix<value_type, NbRows, Eigen::Dynamic, | ||
65 | Eigen::RowMajor>& parameters) { | ||
66 | return ConstVectorMap_t(parameters.data(), parameters.size()); | ||
67 | } | ||
68 | |||
69 | template <int _PB, int _SO> | ||
70 | 30 | SplineGradientBasedAbstract<_PB, _SO>::SplineGradientBasedAbstract( | |
71 | const ProblemConstPtr_t& problem) | ||
72 | : PathOptimizer(problem), | ||
73 | 30 | steeringMethod_(SSM_t::create(problem)), | |
74 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
30 | robot_(problem->robot()) {} |
75 | |||
76 | // ----------- Convenience class -------------------------------------- // | ||
77 | |||
78 | /// \param lc the constraint to be updated. | ||
79 | /// \param row the row into \c lc.J and \c lc.b where to write. | ||
80 | /// \param col the index of the first DoF. | ||
81 | /// \param rDoF the number of DoF of the robot. | ||
82 | /// \param select the DoF indices onto which we optimize (the others | ||
83 | /// being explicitely computed.) | ||
84 | /// \param Bl, Br the Spline::basisFunctionDerivative on the left and right. | ||
85 | /// \param splineL, splineR the left and right spline. | ||
86 | /// \param Czero true for continuity of the path, false for any of its | ||
87 | /// derivative. | ||
88 | /// | ||
89 | /// \c select.nbIndices() rows will be written in \c lc.J and \c lc.b . | ||
90 | /// \c select.nbIndices() rows will be written in \c lc.J and \c lc.b . | ||
91 | template <typename SplineType> | ||
92 | 336 | static inline void setContinuityConstraint( | |
93 | LinearConstraint& lc, const size_type& row, const size_type& col, | ||
94 | const size_type& rDof, const Eigen::RowBlockIndices select, | ||
95 | const typename SplineType::BasisFunctionVector_t& Bl, | ||
96 | const typename SplineType::BasisFunctionVector_t& Br, | ||
97 | const typename SplineType::Ptr_t& splineL, | ||
98 | const typename SplineType::Ptr_t& splineR, bool Czero) { | ||
99 | 336 | const size_type& rows = select.nbIndices(); | |
100 | 336 | size_type c = col; | |
101 |
1/2✓ Branch 2 taken 168 times.
✗ Branch 3 not taken.
|
336 | lc.b.segment(row, rows).setZero(); |
102 |
2/2✓ Branch 1 taken 129 times.
✓ Branch 2 taken 39 times.
|
336 | if (splineL) { |
103 |
2/2✓ Branch 0 taken 746 times.
✓ Branch 1 taken 129 times.
|
1750 | for (size_type i = 0; i < SplineType::NbCoeffs; ++i) { |
104 |
3/6✓ Branch 1 taken 746 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 746 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 746 times.
✗ Branch 8 not taken.
|
1492 | lc.J.block(row, c, rows, rDof).noalias() = |
105 |
3/6✓ Branch 2 taken 746 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 746 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 746 times.
✗ Branch 9 not taken.
|
1492 | select.rview(-Bl(i) * matrix_t::Identity(rDof, rDof)); |
106 | 1492 | c += rDof; | |
107 | } | ||
108 |
2/2✓ Branch 0 taken 51 times.
✓ Branch 1 taken 78 times.
|
258 | if (Czero) |
109 |
3/6✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
|
102 | lc.b.segment(row, rows).noalias() = |
110 |
3/6✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
|
204 | select.rview(splineL->parameters().transpose() * (-Bl)); |
111 | } | ||
112 |
2/2✓ Branch 1 taken 129 times.
✓ Branch 2 taken 39 times.
|
336 | if (splineR) { |
113 |
2/2✓ Branch 0 taken 746 times.
✓ Branch 1 taken 129 times.
|
1750 | for (size_type i = 0; i < SplineType::NbCoeffs; ++i) { |
114 |
3/6✓ Branch 1 taken 746 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 746 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 746 times.
✗ Branch 8 not taken.
|
1492 | lc.J.block(row, c, rows, rDof).noalias() = |
115 |
3/6✓ Branch 2 taken 746 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 746 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 746 times.
✗ Branch 9 not taken.
|
1492 | select.rview(Br(i) * matrix_t::Identity(rDof, rDof)); |
116 | 1492 | c += rDof; | |
117 | } | ||
118 |
2/2✓ Branch 0 taken 51 times.
✓ Branch 1 taken 78 times.
|
258 | if (Czero) |
119 |
4/8✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 51 times.
✗ Branch 11 not taken.
|
102 | lc.b.segment(row, rows).noalias() += |
120 |
2/4✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
|
204 | select.rview(splineR->parameters().transpose() * Br).eval(); |
121 | } | ||
122 | 336 | } | |
123 | |||
124 | // ----------- Resolution steps --------------------------------------- // | ||
125 | |||
126 | template <int _PB, int _SO> | ||
127 | 30 | PathVectorPtr_t SplineGradientBasedAbstract<_PB, _SO>::cleanInput( | |
128 | const PathVectorPtr_t& input) { | ||
129 |
1/2✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
|
60 | PathVectorPtr_t flat = |
130 | 60 | PathVector::create(input->outputSize(), input->outputDerivativeSize()); | |
131 |
1/2✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
|
30 | input->flatten(flat); |
132 | // Remove zero length path | ||
133 |
1/2✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
|
60 | PathVectorPtr_t clean = |
134 | 60 | PathVector::create(input->outputSize(), input->outputDerivativeSize()); | |
135 |
2/2✓ Branch 3 taken 51 times.
✓ Branch 4 taken 15 times.
|
132 | for (std::size_t i = 0; i < flat->numberPaths(); ++i) { |
136 |
1/2✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
|
102 | PathPtr_t p = flat->pathAtRank(i); |
137 |
3/6✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 51 times.
✗ Branch 9 not taken.
|
102 | if (p->length() > 0) clean->appendPath(p); |
138 | } | ||
139 | 60 | return clean; | |
140 | 30 | } | |
141 | |||
142 | template <int _PB, int _SO> | ||
143 | typename SplineGradientBasedAbstract<_PB, _SO>::SplinePtr_t | ||
144 | 102 | SplineGradientBasedAbstract<_PB, _SO>::steer(ConfigurationIn_t q0, | |
145 | ConfigurationIn_t q1, | ||
146 | value_type length) const { | ||
147 | enum { NDerivativeConstraintPerSide = int((SplineOrder + 1 - 2) / 2) }; | ||
148 | typedef Eigen::Matrix<value_type, Eigen::Dynamic, | ||
149 | NDerivativeConstraintPerSide> | ||
150 | DerMatrix_t; | ||
151 | typedef typename DerMatrix_t::ConstantReturnType DefaultDerivatives_t; | ||
152 | |||
153 |
2/4✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
|
102 | DefaultDerivatives_t defaultDer( |
154 | 102 | DerMatrix_t::Zero(robot_->numberDof(), NDerivativeConstraintPerSide)); | |
155 |
1/2✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
|
102 | std::vector<int> orders(NDerivativeConstraintPerSide); |
156 |
2/2✓ Branch 0 taken 78 times.
✓ Branch 1 taken 43 times.
|
258 | for (std::size_t i = 0; i < NDerivativeConstraintPerSide; ++i) |
157 | 156 | orders[i] = int(i + 1); | |
158 | |||
159 |
7/14✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 51 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 51 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 51 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 51 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 51 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 51 times.
✗ Branch 21 not taken.
|
204 | PathPtr_t s = steeringMethod_->steer(q0, orders, defaultDer, q1, orders, |
160 | defaultDer, length); | ||
161 | |||
162 | 204 | return HPP_DYNAMIC_PTR_CAST(Spline, s); | |
163 | 102 | } | |
164 | |||
165 | template <int _PB, int _SO> | ||
166 | 102 | void SplineGradientBasedAbstract<_PB, _SO>::appendEquivalentSpline( | |
167 | const StraightPathPtr_t& path, Splines_t& splines) const { | ||
168 |
6/12✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 51 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 51 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 51 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 51 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 51 times.
✗ Branch 20 not taken.
|
204 | SplinePtr_t s = steer(path->initial(), path->end(), path->length()); |
169 |
2/2✓ Branch 3 taken 24 times.
✓ Branch 4 taken 27 times.
|
102 | if (path->constraints()) { |
170 |
2/4✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 24 times.
✗ Branch 7 not taken.
|
48 | splines.push_back( |
171 | 48 | HPP_DYNAMIC_PTR_CAST(Spline, s->copy(path->constraints()))); | |
172 | } else { | ||
173 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | splines.push_back(s); |
174 | } | ||
175 | 102 | } | |
176 | |||
177 | template <int _PB, int _SO> | ||
178 | ✗ | void SplineGradientBasedAbstract<_PB, _SO>::appendEquivalentSpline( | |
179 | const InterpolatedPathPtr_t& path, Splines_t& splines) const { | ||
180 | ✗ | if (path->interpolationPoints().size() > 2) { | |
181 | ✗ | HPP_THROW(std::logic_error, | |
182 | "Projected path with more than 2 IPs are not supported. " | ||
183 | << path->interpolationPoints().size() << ")."); | ||
184 | } | ||
185 | ✗ | SplinePtr_t s = steer(path->initial(), path->end(), path->length()); | |
186 | ✗ | if (path->constraints()) { | |
187 | ✗ | splines.push_back( | |
188 | ✗ | HPP_DYNAMIC_PTR_CAST(Spline, s->copy(path->constraints()))); | |
189 | } else { | ||
190 | ✗ | splines.push_back(s); | |
191 | } | ||
192 | } | ||
193 | |||
194 | template <int _PB, int _SO> | ||
195 | 30 | void SplineGradientBasedAbstract<_PB, _SO>::appendEquivalentSpline( | |
196 | const PathVectorPtr_t& path, Splines_t& splines) const { | ||
197 |
2/2✓ Branch 7 taken 51 times.
✓ Branch 8 taken 15 times.
|
132 | for (std::size_t i = 0; i < path->numberPaths(); ++i) { |
198 |
1/2✓ Branch 2 taken 51 times.
✗ Branch 3 not taken.
|
102 | PathPtr_t p = path->pathAtRank(i); |
199 | 102 | StraightPathPtr_t straight(HPP_DYNAMIC_PTR_CAST(StraightPath, p)); | |
200 | 102 | PathVectorPtr_t pvect(HPP_DYNAMIC_PTR_CAST(PathVector, p)); | |
201 | 102 | InterpolatedPathPtr_t intp(HPP_DYNAMIC_PTR_CAST(InterpolatedPath, p)); | |
202 | 102 | SplinePtr_t spline(HPP_DYNAMIC_PTR_CAST(Spline, p)); | |
203 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
102 | if (straight) |
204 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
102 | appendEquivalentSpline(straight, splines); |
205 | ✗ | else if (pvect) | |
206 | ✗ | appendEquivalentSpline(pvect, splines); | |
207 | ✗ | else if (intp) | |
208 | ✗ | appendEquivalentSpline(intp, splines); | |
209 | ✗ | else if (spline) | |
210 | ✗ | splines.push_back(HPP_STATIC_PTR_CAST(Spline, spline->copy())); | |
211 | else // if TODO check if path is another type of spline. | ||
212 | ✗ | throw std::logic_error("Unknown type of path"); | |
213 | } | ||
214 | 30 | } | |
215 | |||
216 | template <int _PB, int _SO> | ||
217 | 30 | void SplineGradientBasedAbstract<_PB, _SO>::initializePathValidation( | |
218 | const Splines_t& splines) { | ||
219 | 30 | validations_.resize(splines.size()); | |
220 |
2/2✓ Branch 1 taken 51 times.
✓ Branch 2 taken 15 times.
|
132 | for (std::size_t i = 0; i < splines.size(); ++i) { |
221 | 102 | validations_[i] = problem()->pathValidation(); | |
222 | } | ||
223 | 30 | } | |
224 | |||
225 | template <int _PB, int _SO> | ||
226 | typename SplineGradientBasedAbstract<_PB, _SO>::Reports_t | ||
227 | 24 | SplineGradientBasedAbstract<_PB, _SO>::validatePath( | |
228 | const Splines_t& splines, std::vector<std::size_t>& reordering, | ||
229 | bool stopAtFirst, bool reorder) const { | ||
230 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
|
24 | assert(validations_.size() == splines.size()); |
231 | HPP_SCOPE_TIMECOUNTER(SGB_validatePath); | ||
232 | 24 | PathPtr_t validPart; | |
233 | 24 | PathValidationReportPtr_t report; | |
234 | 24 | Reports_t reports; | |
235 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
|
24 | assert(reordering.size() == splines.size()); |
236 | #ifndef NDEBUG | ||
237 |
1/2✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
|
24 | std::vector<bool> check_ordering(reordering.size(), false); |
238 | #endif | ||
239 |
2/2✓ Branch 1 taken 42 times.
✓ Branch 2 taken 9 times.
|
102 | for (std::size_t j = 0; j < splines.size(); ++j) { |
240 | 84 | const std::size_t& i = reordering[j]; | |
241 | #ifndef NDEBUG | ||
242 |
2/4✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 42 times.
|
84 | assert(!check_ordering[i]); |
243 |
1/2✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
|
84 | check_ordering[i] = true; |
244 | #endif | ||
245 |
3/4✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✓ Branch 9 taken 39 times.
|
84 | if (!validations_[i]->validate(splines[i], false, validPart, report)) { |
246 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
6 | reports.push_back(std::make_pair(report, i)); |
247 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
6 | if (stopAtFirst) break; |
248 | } | ||
249 | } | ||
250 |
2/6✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 12 times.
|
24 | if (reorder && !reports.empty()) { |
251 | ✗ | const std::size_t k = reports.front().second; | |
252 | // Set reordering to [ k, ..., n-1, 0, ..., k-1] | ||
253 | ✗ | for (std::size_t i = 0; i < reordering.size() - k; ++i) | |
254 | ✗ | reordering[i] = k + i; | |
255 | ✗ | for (std::size_t i = 0; i < k; ++i) | |
256 | ✗ | reordering[reordering.size() - k + i] = i; | |
257 | } | ||
258 | HPP_DISPLAY_TIMECOUNTER(SGB_validatePath); | ||
259 | 48 | return reports; | |
260 | } | ||
261 | |||
262 | template <int _PB, int _SO> | ||
263 | 30 | void SplineGradientBasedAbstract<_PB, _SO>::addContinuityConstraints( | |
264 | const Splines_t& splines, const size_type maxOrder, | ||
265 | const SplineOptimizationDatas_t& sods, LinearConstraint& lc) { | ||
266 |
2/4✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
|
30 | typename Spline::BasisFunctionVector_t B0, B1; |
267 | enum { NbCoeffs = Spline::NbCoeffs }; | ||
268 | |||
269 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
30 | const size_type rDof = robot_->numberDof(); |
270 | |||
271 | // Compute which continuity constraint are necessary | ||
272 | 30 | size_type nbRows = 0; | |
273 |
1/2✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
|
30 | std::vector<RowBlockIndices> rbis(splines.size() + 1); |
274 | 30 | BlockIndex::segment_t space(0, rDof); | |
275 | |||
276 | // 1. Constrain the starting position. | ||
277 |
1/2✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
|
30 | rbis[0] = sods[0].activeParameters; |
278 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
30 | nbRows += rbis[0].nbIndices(); |
279 | |||
280 | // 2. For each consecutive splines, constrain the intersection. | ||
281 |
2/2✓ Branch 1 taken 36 times.
✓ Branch 2 taken 15 times.
|
102 | for (std::size_t i = 1; i < sods.size(); ++i) { |
282 | // Compute union between A = sods[i-1].activeParameters.indices() | ||
283 | // and B = sods[i].activeParameters.indices() | ||
284 |
2/4✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 36 times.
✗ Branch 6 not taken.
|
72 | BlockIndex::segments_t v(sods[i - 1].activeParameters.indices()); |
285 |
2/4✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 8 taken 36 times.
✗ Branch 9 not taken.
|
72 | v.insert(v.end(), sods[i].activeParameters.indices().begin(), |
286 |
1/2✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
|
72 | sods[i].activeParameters.indices().end()); |
287 |
2/4✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 36 times.
✗ Branch 6 not taken.
|
72 | rbis[i] = RowBlockIndices(v); |
288 |
1/2✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
|
72 | rbis[i].updateRows<true, true, true>(); |
289 | hppDout(info, "Optimize waypoint " << i << " over " << rbis[i]); | ||
290 |
1/2✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
|
72 | nbRows += rbis[i].nbIndices(); |
291 | } | ||
292 | |||
293 | // 3. Constrain the final position. | ||
294 |
1/2✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
|
30 | rbis[sods.size()] = sods[sods.size() - 1].activeParameters; |
295 |
1/2✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
|
30 | nbRows += rbis[sods.size()].nbIndices(); |
296 | |||
297 | 30 | nbRows *= (maxOrder + 1); | |
298 | |||
299 | // Create continuity constraint | ||
300 | 30 | size_type row = lc.J.rows(), paramSize = rDof * Spline::NbCoeffs; | |
301 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | lc.addRows(nbRows); |
302 | |||
303 |
2/2✓ Branch 0 taken 39 times.
✓ Branch 1 taken 15 times.
|
108 | for (size_type k = 0; k <= maxOrder; ++k) { |
304 | 78 | bool Czero = (k == 0); | |
305 | |||
306 | // Continuity at the beginning | ||
307 |
1/2✓ Branch 3 taken 39 times.
✗ Branch 4 not taken.
|
78 | splines[0]->basisFunctionDerivative(k, 0, B0); |
308 | 78 | size_type indexParam = 0; | |
309 | |||
310 |
2/4✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 39 times.
✗ Branch 6 not taken.
|
78 | setContinuityConstraint<Spline>(lc, row, indexParam, rDof, rbis[0], B1, B0, |
311 | 156 | SplinePtr_t(), splines[0], Czero); | |
312 |
1/2✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
|
78 | row += rbis[0].nbIndices(); |
313 | |||
314 |
2/2✓ Branch 1 taken 90 times.
✓ Branch 2 taken 39 times.
|
258 | for (std::size_t j = 0; j < splines.size() - 1; ++j) { |
315 |
1/2✓ Branch 3 taken 90 times.
✗ Branch 4 not taken.
|
180 | splines[j]->basisFunctionDerivative(k, 1, B1); |
316 |
1/2✓ Branch 3 taken 90 times.
✗ Branch 4 not taken.
|
180 | splines[j + 1]->basisFunctionDerivative(k, 0, B0); |
317 | |||
318 | // Continuity between spline i and j | ||
319 |
2/4✓ Branch 2 taken 90 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 90 times.
✗ Branch 6 not taken.
|
180 | setContinuityConstraint<Spline>(lc, row, indexParam, rDof, rbis[j + 1], |
320 | 180 | B1, B0, splines[j], splines[j + 1], | |
321 | Czero); | ||
322 | |||
323 |
1/2✓ Branch 2 taken 90 times.
✗ Branch 3 not taken.
|
180 | row += rbis[j + 1].nbIndices(); |
324 | 180 | indexParam += paramSize; | |
325 | } | ||
326 | |||
327 | // Continuity at the end | ||
328 |
1/2✓ Branch 3 taken 39 times.
✗ Branch 4 not taken.
|
78 | splines.back()->basisFunctionDerivative(k, 1, B1); |
329 |
2/4✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 39 times.
✗ Branch 6 not taken.
|
78 | setContinuityConstraint<Spline>(lc, row, indexParam, rDof, rbis.back(), B1, |
330 | 156 | B0, splines.back(), SplinePtr_t(), Czero); | |
331 |
1/2✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
|
78 | row += rbis.back().nbIndices(); |
332 | |||
333 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 39 times.
|
78 | assert(indexParam + paramSize == lc.J.cols()); |
334 | } | ||
335 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 15 times.
|
30 | assert(row == lc.J.rows()); |
336 | 30 | } | |
337 | |||
338 | template <int _PB, int _SO> | ||
339 | typename SplineGradientBasedAbstract<_PB, _SO>::Indices_t | ||
340 | 30 | SplineGradientBasedAbstract<_PB, _SO>::validateBounds( | |
341 | const Splines_t& splines, const LinearConstraint& lc) const { | ||
342 | 30 | Indices_t violated; | |
343 | |||
344 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
30 | const size_type rDof = robot_->numberDof(); |
345 | 30 | const size_type cols = rDof * Spline::NbCoeffs; | |
346 | 30 | const size_type rows = lc.J.rows() / splines.size(); | |
347 | 30 | const size_type nBoundedDof = rows / (2 * Spline::NbCoeffs); | |
348 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 15 times.
|
30 | assert(lc.J.rows() % splines.size() == 0); |
349 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 15 times.
|
30 | assert(rows % (2 * Spline::NbCoeffs) == 0); |
350 | |||
351 | 30 | const value_type thr = Eigen::NumTraits<value_type>::dummy_precision(); | |
352 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | vector_t err; |
353 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
30 | std::vector<bool> dofActive(nBoundedDof, false); |
354 | |||
355 |
2/2✓ Branch 1 taken 51 times.
✓ Branch 2 taken 15 times.
|
132 | for (std::size_t i = 0; i < splines.size(); ++i) { |
356 | 102 | const size_type row = i * rows; | |
357 | 102 | const size_type col = i * cols; | |
358 | |||
359 |
5/10✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 51 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 51 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 51 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 51 times.
✗ Branch 16 not taken.
|
102 | err = lc.J.block(row, col, rows, cols) * splines[i]->rowParameters() - |
360 |
1/2✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
|
102 | lc.b.segment(row, rows); |
361 | |||
362 | // Find the maximum per parameter | ||
363 |
2/2✓ Branch 0 taken 75 times.
✓ Branch 1 taken 51 times.
|
252 | for (size_type j = 0; j < nBoundedDof; ++j) { |
364 |
2/4✓ Branch 1 taken 75 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 75 times.
|
150 | if (dofActive[j]) continue; |
365 | 150 | value_type errM = -thr; | |
366 | 150 | size_type iErrM = -1; | |
367 |
2/2✓ Branch 0 taken 354 times.
✓ Branch 1 taken 75 times.
|
858 | for (size_type k = 0; k < Spline::NbCoeffs; ++k) { |
368 | 708 | const size_type low = 2 * j + k * 2 * nBoundedDof; | |
369 | 708 | const size_type up = 2 * j + k * 2 * nBoundedDof + 1; | |
370 |
2/4✓ Branch 1 taken 354 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 354 times.
|
708 | if (err(low) < errM) { |
371 | ✗ | iErrM = low; | |
372 | ✗ | errM = err(low); | |
373 | } | ||
374 |
2/4✓ Branch 1 taken 354 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 354 times.
|
708 | if (err(up) < errM) { |
375 | ✗ | iErrM = up; | |
376 | ✗ | errM = err(up); | |
377 | } | ||
378 | } | ||
379 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 75 times.
|
150 | if (iErrM >= 0) { |
380 | ✗ | violated.push_back(row + iErrM); | |
381 | ✗ | dofActive[j] = true; | |
382 | hppDout(info, "Bound violation at spline " | ||
383 | << i << ", param " << (iErrM - 2 * j) / nBoundedDof | ||
384 | << ", iDof " << j << ": " << errM); | ||
385 | } | ||
386 | } | ||
387 | } | ||
388 | 30 | if (!violated.empty()) { | |
389 | hppDout(info, violated.size() << " bounds violated."); | ||
390 | } | ||
391 | 60 | return violated; | |
392 | 30 | } | |
393 | |||
394 | template <int _PB, int _SO> | ||
395 | ✗ | std::size_t SplineGradientBasedAbstract<_PB, _SO>::addBoundConstraints( | |
396 | const Indices_t& bci, const LinearConstraint& bc, Bools_t& activeConstraint, | ||
397 | LinearConstraint& constraint) const { | ||
398 | ✗ | std::size_t nbC = 0; | |
399 | ✗ | for (std::size_t i = 0; i < bci.size(); i++) { | |
400 | ✗ | if (!activeConstraint[bci[i]]) { | |
401 | ✗ | ++nbC; | |
402 | ✗ | constraint.addRows(1); | |
403 | ✗ | constraint.J.template bottomRows<1>() = bc.J.row(bci[i]); | |
404 | ✗ | constraint.b(constraint.b.size() - 1) = bc.b(bci[i]); | |
405 | } | ||
406 | } | ||
407 | ✗ | return nbC; | |
408 | } | ||
409 | |||
410 | template <int _PB, int _SO> | ||
411 | 30 | PathVectorPtr_t SplineGradientBasedAbstract<_PB, _SO>::buildPathVector( | |
412 | const Splines_t& splines) const { | ||
413 | 60 | PathVectorPtr_t pv = | |
414 | 60 | PathVector::create(robot_->configSize(), robot_->numberDof()); | |
415 | |||
416 |
3/4✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 51 times.
✓ Branch 9 taken 15 times.
|
132 | for (std::size_t i = 0; i < splines.size(); ++i) pv->appendPath(splines[i]); |
417 | 30 | return pv; | |
418 | } | ||
419 | |||
420 | // ----------- Convenience functions ---------------------------------- // | ||
421 | |||
422 | template <int _PB, int _SO> | ||
423 | 30 | void SplineGradientBasedAbstract<_PB, _SO>::jointBoundConstraint( | |
424 | const Splines_t& splines, LinearConstraint& lc) const { | ||
425 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
30 | const size_type rDof = robot_->numberDof(); |
426 | 30 | const size_type cols = Spline::NbCoeffs * rDof; | |
427 | |||
428 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | matrix_t A(2 * rDof, rDof); |
429 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | vector_t b(2 * rDof); |
430 | // Only the number of inequality constraints (rows) is used after this | ||
431 | // call. Matrix A and vector b are not used, only resized. | ||
432 | const size_type rows = | ||
433 |
3/6✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 15 times.
✗ Branch 9 not taken.
|
30 | jointBoundMatrices(robot_, robot_->neutralConfiguration(), A, b); |
434 | |||
435 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | A.resize(rows, rDof); |
436 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | b.resize(rows); |
437 | |||
438 | 30 | const size_type size = Spline::NbCoeffs * rows; | |
439 |
1/2✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
|
30 | lc.J.resize(splines.size() * size, splines.size() * cols); |
440 |
1/2✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
|
30 | lc.b.resize(splines.size() * size); |
441 | |||
442 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | lc.J.setZero(); |
443 |
1/2✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
|
30 | lc.b.setZero(); |
444 | |||
445 |
2/2✓ Branch 1 taken 51 times.
✓ Branch 2 taken 15 times.
|
132 | for (std::size_t i = 0; i < splines.size(); ++i) { |
446 |
3/6✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 51 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 51 times.
✗ Branch 10 not taken.
|
102 | jointBoundMatrices(robot_, splines[i]->base(), A, b); |
447 |
2/2✓ Branch 0 taken 258 times.
✓ Branch 1 taken 51 times.
|
618 | for (size_type k = 0; k < Spline::NbCoeffs; ++k) { |
448 | 516 | const size_type row = i * size + k * rows; | |
449 | 516 | const size_type col = i * cols + k * rDof; | |
450 |
3/6✓ Branch 1 taken 258 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 258 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 258 times.
✗ Branch 8 not taken.
|
516 | lc.J.block(row, col, rows, rDof) = -A; |
451 |
3/6✓ Branch 1 taken 258 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 258 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 258 times.
✗ Branch 8 not taken.
|
516 | lc.b.segment(row, rows) = -b; |
452 | } | ||
453 | } | ||
454 | 30 | } | |
455 | |||
456 | template <int _PB, int _SO> | ||
457 | 60 | void SplineGradientBasedAbstract<_PB, _SO>::copy(const Splines_t& in, | |
458 | Splines_t& out) { | ||
459 | 60 | out.resize(in.size()); | |
460 |
2/2✓ Branch 1 taken 102 times.
✓ Branch 2 taken 30 times.
|
264 | for (std::size_t i = 0; i < in.size(); ++i) |
461 | 204 | out[i] = HPP_STATIC_PTR_CAST(Spline, in[i]->copy()); | |
462 | 60 | } | |
463 | |||
464 | template <int _PB, int _SO> | ||
465 | 36 | void SplineGradientBasedAbstract<_PB, _SO>::updateSplines( | |
466 | Splines_t& splines, const vector_t& param) const { | ||
467 | 36 | size_type row = 0, size = robot_->numberDof() * Spline::NbCoeffs; | |
468 |
2/2✓ Branch 1 taken 63 times.
✓ Branch 2 taken 18 times.
|
162 | for (std::size_t i = 0; i < splines.size(); ++i) { |
469 |
2/4✓ Branch 4 taken 63 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 63 times.
✗ Branch 8 not taken.
|
126 | splines[i]->rowParameters(param.segment(row, size)); |
470 | 126 | row += size; | |
471 | } | ||
472 | 36 | } | |
473 | |||
474 | template <int _PB, int _SO> | ||
475 | ✗ | void SplineGradientBasedAbstract<_PB, _SO>::updateParameters( | |
476 | vector_t& param, const Splines_t& splines) const { | ||
477 | ✗ | param.resize(robot_->numberDof() * Spline::NbCoeffs * splines.size()); | |
478 | ✗ | size_type row = 0, size = robot_->numberDof() * Spline::NbCoeffs; | |
479 | ✗ | for (std::size_t i = 2; i < splines.size(); ++i) { | |
480 | ✗ | param.segment(row, size) = splines[i]->rowParameters(); | |
481 | ✗ | row += size; | |
482 | } | ||
483 | } | ||
484 | |||
485 | template <int _PB, int _SO> | ||
486 | 18 | void SplineGradientBasedAbstract<_PB, _SO>::interpolate(const Splines_t& a, | |
487 | const Splines_t& b, | ||
488 | const value_type& alpha, | ||
489 | Splines_t& res) { | ||
490 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 9 times.
✗ Branch 7 not taken.
|
18 | assert(a.size() == b.size() && b.size() == res.size()); |
491 |
2/4✓ Branch 0 taken 9 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
|
18 | assert(alpha >= 0 && alpha <= 1); |
492 | |||
493 |
2/2✓ Branch 1 taken 36 times.
✓ Branch 2 taken 9 times.
|
90 | for (std::size_t i = 0; i < a.size(); ++i) |
494 |
6/12✓ Branch 5 taken 36 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 36 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 36 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 36 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 36 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 36 times.
✗ Branch 23 not taken.
|
144 | res[i]->rowParameters((1 - alpha) * a[i]->rowParameters() + |
495 | 72 | alpha * b[i]->rowParameters()); | |
496 | } | ||
497 | |||
498 | // ----------- Instanciate -------------------------------------------- // | ||
499 | |||
500 | // template class SplineGradientBased<path::CanonicalPolynomeBasis, 1>; // | ||
501 | // equivalent to StraightPath template class | ||
502 | // SplineGradientBased<path::CanonicalPolynomeBasis, 2>; template class | ||
503 | // SplineGradientBased<path::CanonicalPolynomeBasis, 3>; | ||
504 | template class SplineGradientBasedAbstract<path::BernsteinBasis, | ||
505 | 1>; // equivalent to StraightPath | ||
506 | // template class SplineGradientBased<path::BernsteinBasis, 2>; | ||
507 | template class SplineGradientBasedAbstract<path::BernsteinBasis, 3>; | ||
508 | template class SplineGradientBasedAbstract<path::BernsteinBasis, 5>; | ||
509 | template class SplineGradientBasedAbstract<path::BernsteinBasis, 7>; | ||
510 | } // namespace pathOptimization | ||
511 | } // namespace core | ||
512 | } // namespace hpp | ||
513 |