GCC Code Coverage Report

Directory: ./
File: include/hpp/core/path/spline.hh
Date: 2024-08-10 11:29:48
Exec Total Coverage
Lines: 53 70 75.7%
Branches: 40 108 37.0%
Line Branch Exec Source
1 // Copyright (c) 2017 CNRS
2 // Authors: Joseph Mirabel
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 #ifndef HPP_CORE_PATH_SPLINE_HH
30 #define HPP_CORE_PATH_SPLINE_HH
31
32 #include <hpp/core/config.hh>
33 #include <hpp/core/fwd.hh>
34 #include <hpp/core/path.hh>
35 #include <hpp/core/steering-method/fwd.hh>
36 #include <hpp/pinocchio/device.hh>
37 #include <hpp/pinocchio/liegroup-element.hh>
38
39 namespace hpp {
40 namespace core {
41 namespace path {
42 /// \addtogroup path
43 /// \{
44
45 enum PolynomeBasisType { CanonicalPolynomeBasis, BernsteinBasis };
46
47 /// \cond
48 namespace internal {
49 template <int SplineType, int Degree>
50 struct spline_basis_function;
51 template <int SplineType, int Degree>
52 struct sbf_traits {
53 enum { NbCoeffs = Degree + 1 };
54 typedef Eigen::Matrix<value_type, NbCoeffs, 1> Coeffs_t;
55 typedef Eigen::Matrix<value_type, NbCoeffs, NbCoeffs> IntegralCoeffs_t;
56 };
57 } // namespace internal
58 /// \endcond
59
60 /// Base class for spline paths
61 ///
62 /// Splines are polynomials with various possible representations.
63 /// \param _PolynomeBasis basis of polynomials used among
64 /// \li CanonicalPolynomeBasis for the canonical basis,
65 /// \li BernsteinBasis for Bernstein basis
66 /// \param _Order degree of the polynomial representation.
67 /// \sa hpp::core::path::PolynomeBasisType.
68 ///
69 /// Splines represent a curve in the tangent space of a given
70 /// robot (hpp::core::Device) at a configuration called \b base.
71 ///
72 /// \f{eqnarray*}
73 /// spline (u) &=& base + PM^{T} B (u)
74 /// \f}
75 ///
76 /// where*
77 /// \li \f$u\in [0,1]\f$,
78 /// \li operator "+" should be understood as Lie group integration,
79 /// \li \f$PM\f$ is the matrix of parameters the rows of
80 /// which are the spline control points. This matrix is
81 /// accessible via setter and getter Spline::parameters,
82 /// \li \f$B (t)\f$ is the vector containing the values of the basis
83 /// functions at parameter \f$t\f$.
84 ///
85 /// The dimension of control points, corresponding to the robot number of
86 /// degrees of freedom can be retrieved by getter Spline::parameterSize.
87 ///
88 /// \warning Velocities for robots with non-vector configuration space are
89 /// not correctly handled.
90 template <int _PolynomeBasis, int _Order>
91 class HPP_CORE_DLLAPI Spline : public Path {
92 public:
93 enum {
94 PolynomeBasis = _PolynomeBasis,
95 Order = _Order,
96 NbCoeffs = _Order + 1,
97 NbPowerOfT = 2 * NbCoeffs + 1
98 };
99
100 typedef internal::sbf_traits<PolynomeBasis, Order> sbf_traits;
101 typedef internal::spline_basis_function<PolynomeBasis, Order> BasisFunction_t;
102 typedef Eigen::Matrix<value_type, NbPowerOfT, 1> PowersOfT_t;
103 typedef typename sbf_traits::Coeffs_t BasisFunctionVector_t;
104 typedef typename sbf_traits::IntegralCoeffs_t BasisFunctionIntegralMatrix_t;
105
106 typedef Eigen::Matrix<value_type, NbCoeffs, Eigen::Dynamic, Eigen::RowMajor>
107 ParameterMatrix_t;
108 typedef Eigen::Map<const vector_t, Eigen::Aligned> ConstParameterVector_t;
109 typedef Eigen::Map<vector_t, Eigen::Aligned> ParameterVector_t;
110
111 typedef shared_ptr<Spline> Ptr_t;
112 typedef weak_ptr<Spline> WkPtr_t;
113
114 ✗ size_type parameterSize() const { return parameterSize_; }
115
116 /** The partial derivative with respects to the parameters is of the form
117 * \f{eqnarray*}{
118 * \frac{\partial S}{\partial p_{k}} (q, p, t) &=& B_k(t) \times I \\
119 * \frac{\partial S}{\partial q_{base}} (q, p, t) &=& I
120 * \f}
121 * This method returns the coefficients \f$ (B_k(t))_{k} \f$
122 **/
123 ✗ void parameterDerivativeCoefficients(vectorOut_t res,
124 const value_type& t) const {
125 ✗ assert(res.size() == NbCoeffs);
126 ✗ impl_paramDerivative(res, t);
127 }
128
129 /// Adds dParam to the parameters
130 ✗ void parameterIntegrate(vectorIn_t dParam) {
131 ✗ assert(dParam.size() == NbCoeffs * parameterSize_);
132 ✗ impl_paramIntegrate(dParam);
133 }
134
135 /// Returns \f$ \int S^{(k)}(t)^T \times S^{(k)}(t) dt \f$
136 ///
137 /// where k is the argument
138 value_type squaredNormIntegral(const size_type order) const;
139
140 /// Returns the derivative of \ref squaredNormIntegral wrt the parameters.
141 ///
142 /// \f[ res(j) \gets 2 \sum_i P_i^T \times m_{i,j} \f]
143 void squaredNormIntegralDerivative(const size_type order,
144 vectorOut_t res) const;
145
146 /** Returns a vector \f$ (v_i) \f$ as
147 * \f[
148 * v_i = b_i^{(k)}(u)
149 * \f]
150 **/
151 static void timeFreeBasisFunctionDerivative(const size_type order,
152 const value_type& u,
153 BasisFunctionVector_t& res);
154
155 ✗ static void timeFreeBasisFunctionDerivative(const size_type order,
156 const value_type& u,
157 vectorOut_t res) {
158 ✗ assert(res.size() == NbCoeffs);
159 ✗ BasisFunctionVector_t tmp;
160 ✗ timeFreeBasisFunctionDerivative(order, u, tmp);
161 ✗ res = tmp;
162 }
163
164 /** Returns a vector \f$ (v_i) \f$ as
165 * \f[
166 * v_i = T^{-k} b_i^{(k)}(u)
167 * \f]
168 **/
169 void basisFunctionDerivative(const size_type order, const value_type& u,
170 BasisFunctionVector_t& res) const;
171
172 630 void basisFunctionDerivative(const size_type order, const value_type& u,
173 vectorOut_t res) const {
174
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 313 times.
 630 assert(res.size() == NbCoeffs);
175
1/2
✓ Branch 1 taken 313 times.
✗ Branch 2 not taken.
 626 BasisFunctionVector_t tmp;
176
1/2
✓ Branch 1 taken 311 times.
✗ Branch 2 not taken.
 626 basisFunctionDerivative(order, u, tmp);
177
1/2
✓ Branch 1 taken 315 times.
✗ Branch 2 not taken.
 622 res = tmp;
178 630 }
179
180 /// Returns an upper bound of the velocity on the complete interval.
181 /// \sa Path::velocityBound
182 void maxVelocity(vectorOut_t res) const;
183
184 /** Returns a matrix \f$ (m_{i,j}) \f$ as
185 * \f[
186 * m_{i,j} = T^{1-2k} \int_0^1 b_i^{(k)}(u) b_j^{(k)}(u) du
187 * \f]
188 **/
189 void squaredNormBasisFunctionIntegral(
190 const size_type order, BasisFunctionIntegralMatrix_t& res) const;
191
192 ✗ void squaredNormBasisFunctionIntegral(const size_type order,
193 matrixOut_t res) const {
194 // assert (res.size() == NbCoeffs);
195 ✗ BasisFunctionIntegralMatrix_t tmp;
196 ✗ squaredNormBasisFunctionIntegral(order, tmp);
197 ✗ res = tmp;
198 }
199
200 57 virtual Configuration_t initial() const {
201
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
 57 Configuration_t q(outputSize());
202
2/4
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 6 not taken.
 57 bool res = eval(q, timeRange().first);
203
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 29 times.
 57 assert(res);
204 (void)res;
205 57 return q;
206 }
207
208 57 virtual Configuration_t end() const {
209
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
 57 Configuration_t q(outputSize());
210
2/4
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 6 not taken.
 57 bool res = eval(q, timeRange().second);
211
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 29 times.
 57 assert(res);
212 (void)res;
213 57 return q;
214 }
215
216 /// Get the base configuration.
217 /// The parameters are velocities to be integrated from this
218 /// configuration.
219 364 const Configuration_t& base() const { return base_.vector(); }
220
221 /// \sa base() const
222 3705272 void base(const Configuration_t& q) { base_.vector() = q; }
223
224 /// Each row corresponds to a velocity of the robot.
225 204 const ParameterMatrix_t& parameters() const { return parameters_; }
226
227 /// Returns the \f$ (P_i^T) \f$.
228 /// Each row contains one parameter.
229 135 void parameters(const ParameterMatrix_t& m) { parameters_ = m; }
230
231 /// Concatenate the parameters as one vector (P_0^T, ..., P_n^T).
232 684 ConstParameterVector_t rowParameters() const {
233
2/4
✓ Branch 3 taken 342 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 342 times.
✗ Branch 7 not taken.
 684 return ConstParameterVector_t(parameters_.data(), parameters_.size());
234 }
235
236 /// Set the parameters
237 324 void rowParameters(vectorIn_t p) {
238
3/6
✓ Branch 3 taken 162 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 162 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 162 times.
✗ Branch 10 not taken.
 324 ParameterVector_t(parameters_.data(), parameters_.size()) = p;
239 324 }
240
241 396 PathPtr_t copy() const {
242
3/6
✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 198 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 198 times.
✗ Branch 8 not taken.
 396 Ptr_t other(new Spline(*this));
243
1/2
✓ Branch 2 taken 198 times.
✗ Branch 3 not taken.
 396 other->init(other);
244 792 return other;
245 396 }
246
247 48 PathPtr_t copy(const ConstraintSetPtr_t& constraints) const {
248
3/6
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24 times.
✗ Branch 8 not taken.
 48 Ptr_t other(new Spline(*this, constraints));
249
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 48 other->init(other);
250 96 return other;
251 }
252
253 7411494 virtual ~Spline() {}
254
255 135 static Ptr_t create(const DevicePtr_t& robot, const interval_t& interval,
256 const ConstraintSetPtr_t& constraints) {
257
2/4
✓ Branch 2 taken 71 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 71 times.
✗ Branch 6 not taken.
 135 Ptr_t shPtr(new Spline(robot, interval, constraints));
258
1/2
✓ Branch 2 taken 71 times.
✗ Branch 3 not taken.
 135 shPtr->init(shPtr);
259 135 return shPtr;
260 }
261
262 /// Evaluate a spline.
263 /// \param base the base configuration.
264 /// \param params concatenation of row vectors representing the
265 /// velocity interpolation points.
266 /// \param u the ratio, between 0 and 1.
267 /// \retval config the output configuration
268 /// \retval velocity the interpolated velocity
269 static void value(pinocchio::LiegroupElementConstRef base,
270 Eigen::Ref<const ParameterMatrix_t> params,
271 const value_type& u, ConfigurationOut_t config,
272 vectorOut_t velocity);
273
274 protected:
275 3705272 Spline(const DevicePtr_t& robot, const interval_t& interval,
276 const ConstraintSetPtr_t& constraints)
277 : Path(interval, robot->configSize(), robot->numberDof(), constraints),
278
1/2
✓ Branch 1 taken 1852640 times.
✗ Branch 2 not taken.
 3705272 parameterSize_(robot->numberDof()),
279 3705272 robot_(robot),
280
2/4
✓ Branch 4 taken 1852639 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1852640 times.
✗ Branch 8 not taken.
 3705272 base_(robot->RnxSOnConfigSpace()->vectorSpacesMerged()),
281
1/2
✓ Branch 1 taken 1852640 times.
✗ Branch 2 not taken.
 3705272 parameters_((int)NbCoeffs, parameterSize_),
282
2/4
✓ Branch 8 taken 1852640 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1852638 times.
✗ Branch 12 not taken.
 7410544 velocity_(parameterSize_) {
283
1/2
✓ Branch 1 taken 1852640 times.
✗ Branch 2 not taken.
 3705268 powersOfT_(0) = 1;
284
2/2
✓ Branch 0 taken 14821230 times.
✓ Branch 1 taken 1852640 times.
 33347668 for (size_type i = 1; i < NbPowerOfT; ++i)
285
2/4
✓ Branch 1 taken 14821230 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 14821230 times.
✗ Branch 6 not taken.
 29642396 powersOfT_(i) = powersOfT_(i - 1) * length();
286 3705272 }
287
288 Spline(const Spline& path);
289
290 Spline(const Spline& path, const ConstraintSetPtr_t& constraints);
291
292 7410900 void init(const Ptr_t& self) {
293
1/2
✓ Branch 2 taken 3705454 times.
✗ Branch 3 not taken.
 7410900 Path::init(self);
294 7410900 weak_ = self;
295 7410900 }
296
297 std::ostream& print(std::ostream& os) const;
298
299 bool impl_compute(ConfigurationOut_t configuration, value_type t) const;
300
301 void impl_derivative(vectorOut_t res, const value_type& t,
302 size_type order) const;
303
304 void impl_paramDerivative(vectorOut_t res, const value_type& t) const;
305
306 void impl_paramIntegrate(vectorIn_t dParam);
307
308 void impl_velocityBound(vectorOut_t result, const value_type& t0,
309 const value_type& t1) const;
310
311 /// Robot number of degrees of freedom.
312 size_type parameterSize_;
313 DevicePtr_t robot_;
314 /// Base of the spline path.
315 /// The spline is a curve in the tangent space of the robot at this
316 /// configuration.
317 LiegroupElement base_;
318 /// Parameters of the spline are stored in a matrix
319 /// number of rows = degree of polynomial + 1
320 /// number of columns = robot number of degrees of freedom.
321 ParameterMatrix_t parameters_;
322
323 private:
324 WkPtr_t weak_;
325
326 mutable vector_t velocity_;
327 mutable PowersOfT_t powersOfT_;
328
329 friend class steeringMethod::Spline<_PolynomeBasis, _Order>;
330
331 private:
332 ✗ Spline() {}
333 HPP_SERIALIZABLE();
334 }; // class Spline
335 /// \}
336 } // namespace path
337 } // namespace core
338 } // namespace hpp
339 #endif // HPP_CORE_PATH_SPLINE_HH
340