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 |