Directory: | ./ |
---|---|
File: | include/hpp/core/time-parameterization/polynomial.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 40 | 46 | 87.0% |
Branches: | 28 | 52 | 53.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 | #ifndef HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH | ||
30 | #define HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH | ||
31 | |||
32 | #include <hpp/constraints/differentiable-function.hh> | ||
33 | #include <hpp/core/config.hh> | ||
34 | #include <hpp/core/fwd.hh> | ||
35 | #include <hpp/core/path/math.hh> | ||
36 | #include <hpp/core/time-parameterization.hh> | ||
37 | |||
38 | namespace hpp { | ||
39 | namespace core { | ||
40 | namespace timeParameterization { | ||
41 | class HPP_CORE_DLLAPI Polynomial : public TimeParameterization { | ||
42 | public: | ||
43 | 6 | Polynomial(const vector_t& param) : a(param) { | |
44 |
2/2✓ Branch 1 taken 24 times.
✓ Branch 2 taken 6 times.
|
30 | for (size_type i = 0; i < a.size(); ++i) { |
45 |
2/4✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 24 times.
|
24 | assert(a[i] < std::numeric_limits<value_type>::infinity()); |
46 |
2/4✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 24 times.
|
24 | assert(a[i] > -std::numeric_limits<value_type>::infinity()); |
47 | } | ||
48 | 6 | } | |
49 | |||
50 | const vector_t& parameters() const { return a; } | ||
51 | |||
52 | 2 | TimeParameterizationPtr_t copy() const { | |
53 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | return TimeParameterizationPtr_t(new Polynomial(*this)); |
54 | } | ||
55 | |||
56 | /// Computes \f$ \sum_{i=0}^n a_i t^i \f$ | ||
57 | 19 | value_type value(const value_type& t) const { return val(t); } | |
58 | |||
59 | /// Computes \f$ \sum_{i=1}^n i a_i t^{i-1} \f$ | ||
60 | 20010 | value_type derivative(const value_type& t, const size_type& order) const { | |
61 | 20010 | return Jac(t, order); | |
62 | } | ||
63 | |||
64 | /// Compute the bound of the derivative on \f$ [ low, up ] \f$. | ||
65 | /// Three cases are handled: | ||
66 | /// \li first order: \f$ B = |a_1| \f$ | ||
67 | /// \li second order: \f$ B = \max{|J(low)|, |J(up)|} \f$ | ||
68 | /// \li third order: | ||
69 | /// Let \f$ x_m = - \frac{a_2}{3 a_3} \f$ be the extremal point | ||
70 | /// of the derivative and \f$ M = \max{|J(low)|, |J(up)|}\f$. | ||
71 | /// Then: | ||
72 | /// - if \f$ low < x_m < up \f$, | ||
73 | /// \f$ B = \max{ |a_1 - \frac{a_2}{3 a_3}|, M } \f$ | ||
74 | /// - else \f$ B = M \f$ | ||
75 | 9 | value_type derivativeBound(const value_type& low, | |
76 | const value_type& up) const { | ||
77 | using std::fabs; | ||
78 | using std::max; | ||
79 |
2/4✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
|
9 | switch (a.size()) { |
80 | 3 | case 2: | |
81 | 3 | return fabs(a[1]); | |
82 | break; | ||
83 | ✗ | case 3: | |
84 | ✗ | return max(fabs(Jac(low)), fabs(Jac(up))); | |
85 | break; | ||
86 | 6 | case 4: { | |
87 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | const value_type x_m = -a[2] / (3 * a[3]); |
88 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
6 | const value_type M = max(fabs(Jac(low)), fabs(Jac(up))); |
89 |
4/4✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 2 times.
|
6 | if (low < x_m && x_m < up) |
90 |
3/6✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
|
2 | return max(M, fabs(a[1] - a[2] / 3 * a[3])); |
91 | else | ||
92 | 4 | return M; | |
93 | } break; | ||
94 | ✗ | default: | |
95 | ✗ | throw std::logic_error("not implemented"); | |
96 | } | ||
97 | } | ||
98 | |||
99 | private: | ||
100 | 19 | value_type val(const value_type& t) const { | |
101 | 19 | value_type tn = 1; | |
102 | 19 | value_type res = a[0]; | |
103 |
2/2✓ Branch 1 taken 31 times.
✓ Branch 2 taken 19 times.
|
50 | for (size_type i = 1; i < a.size(); ++i) { |
104 | 31 | tn *= t; | |
105 | 31 | res += a[i] * tn; | |
106 | } | ||
107 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 19 times.
|
19 | assert(res == res); |
108 | 19 | return res; | |
109 | } | ||
110 | |||
111 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | value_type Jac(const value_type& t) const { return Jac(t, 1); } |
112 | |||
113 | 20022 | value_type Jac(const value_type& t, const size_type& order) const { | |
114 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 20022 times.
|
20022 | if (order >= a.size()) return 0; |
115 | 20022 | const size_type MaxOrder = 10; | |
116 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 20022 times.
|
20022 | if (a.size() > MaxOrder) |
117 | ✗ | throw std::invalid_argument( | |
118 | ✗ | "Cannot compute the derivative of order greater than 10."); | |
119 | typedef path::binomials<MaxOrder> Binomials_t; | ||
120 | 20022 | const Binomials_t::Factorials_t& factors = Binomials_t::factorials(); | |
121 | |||
122 | 20022 | value_type res = 0; | |
123 | 20022 | value_type tn = 1; | |
124 |
2/2✓ Branch 1 taken 100060 times.
✓ Branch 2 taken 20022 times.
|
120082 | for (size_type i = order; i < a.size(); ++i) { |
125 | 100060 | res += value_type(factors[i] / factors[i - order]) * a[i] * tn; | |
126 | 100060 | tn *= t; | |
127 | } | ||
128 | 20022 | return res; | |
129 | } | ||
130 | |||
131 | vector_t a; | ||
132 | }; // class Polynomial | ||
133 | } // namespace timeParameterization | ||
134 | } // namespace core | ||
135 | } // namespace hpp | ||
136 | #endif // HPP_CORE_TIME_PARAMETERIZATION_POLYNOMIAL_HH | ||
137 |