GCC Code Coverage Report

Directory: ./
File: src/path-optimization/spline-gradient-based-abstract.cc
Date: 2024-12-13 16:14:03
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