Directory: | ./ |
---|---|
File: | include/hpp/core/path-optimization/spline-gradient-based-abstract.hh |
Date: | 2024-08-10 11:29:48 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 1 | 2 | 50.0% |
Branches: | 2 | 6 | 33.3% |
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_OPTIMIZATION_SPLINE_GRADIENT_BASED_ABSTRACT_HH | ||
30 | #define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_ABSTRACT_HH | ||
31 | |||
32 | #include <hpp/constraints/explicit-constraint-set.hh> | ||
33 | #include <hpp/constraints/solver/by-substitution.hh> | ||
34 | #include <hpp/core/path-optimization/linear-constraint.hh> | ||
35 | #include <hpp/core/path-optimizer.hh> | ||
36 | #include <hpp/core/path-vector.hh> | ||
37 | #include <hpp/core/path/spline.hh> | ||
38 | #include <hpp/core/steering-method/spline.hh> | ||
39 | |||
40 | namespace hpp { | ||
41 | namespace core { | ||
42 | /// \addtogroup path_optimization | ||
43 | /// \{ | ||
44 | namespace pathOptimization { | ||
45 | /// Common base for optimization-based path optimizer with splines. | ||
46 | template <int _PolynomeBasis, int _SplineOrder> | ||
47 | class HPP_CORE_DLLAPI SplineGradientBasedAbstract : public PathOptimizer { | ||
48 | public: | ||
49 | enum { PolynomeBasis = _PolynomeBasis, SplineOrder = _SplineOrder }; | ||
50 | typedef path::Spline<PolynomeBasis, SplineOrder> Spline; | ||
51 | typedef typename Spline::Ptr_t SplinePtr_t; | ||
52 | typedef std::vector<SplinePtr_t> Splines_t; | ||
53 | |||
54 | /// \name Spline convinience functions | ||
55 | /// \{ | ||
56 | |||
57 | /// Copy a vector of Spline | ||
58 | static void copy(const Splines_t& in, Splines_t& out); | ||
59 | |||
60 | /// Sets the parameters each spline. | ||
61 | /// \todo make this function static (currently, it only needs the | ||
62 | /// robot number dof. | ||
63 | void updateSplines(Splines_t& spline, const vector_t& param) const; | ||
64 | |||
65 | /// Gets the parameters each spline. | ||
66 | /// \todo make this function static (currently, it only needs the | ||
67 | /// robot number dof. | ||
68 | void updateParameters(vector_t& param, const Splines_t& spline) const; | ||
69 | |||
70 | /// Returns res = (1 - alpha) * a + alpha * b | ||
71 | static void interpolate(const Splines_t& a, const Splines_t& b, | ||
72 | const value_type& alpha, Splines_t& res); | ||
73 | |||
74 | /// \} | ||
75 | |||
76 | protected: | ||
77 | SplineGradientBasedAbstract(const ProblemConstPtr_t& problem); | ||
78 | |||
79 | /// \name Spline creation | ||
80 | /// \{ | ||
81 | |||
82 | /// Flatten path and remove path of zero length. | ||
83 | static PathVectorPtr_t cleanInput(const PathVectorPtr_t& input); | ||
84 | |||
85 | /// Spline steering method. | ||
86 | typedef steeringMethod::Spline<PolynomeBasis, SplineOrder> SSM_t; | ||
87 | typename SSM_t::Ptr_t steeringMethod_; | ||
88 | |||
89 | /// Convert a straight path into a spline and append to vector | ||
90 | /// | ||
91 | /// \param path straight path, | ||
92 | /// \retval splines vector of splines | ||
93 | /// | ||
94 | /// Build a spline starting from straight path initial configuration | ||
95 | /// and ending at straight path final configuration. | ||
96 | /// derivatives (up to an order depending on the spline degree) at | ||
97 | /// start and end are set to zero. | ||
98 | void appendEquivalentSpline(const StraightPathPtr_t& path, | ||
99 | Splines_t& splines) const; | ||
100 | |||
101 | /// Convert an interpolated path into a spline and append to vector | ||
102 | /// | ||
103 | /// \param path interpolated path, | ||
104 | /// \retval splines vector of splines | ||
105 | /// | ||
106 | /// Build a spline starting from interpolated path initial | ||
107 | /// configuration and ending at interpolated path final | ||
108 | /// configuration. derivatives (up to an order depending on | ||
109 | /// the spline degree) at start and end are set to zero. If | ||
110 | /// the interpolated path as only two waypoint, treat it as | ||
111 | /// a straight path. Otherwise, throw an exception. | ||
112 | void appendEquivalentSpline(const InterpolatedPathPtr_t& path, | ||
113 | Splines_t& splines) const; | ||
114 | |||
115 | /// For each subpath of path, cast it into a known path and calls | ||
116 | /// appropriate appendEquivalentSpline. | ||
117 | /// \param splines the output will be pushed back into this vector. | ||
118 | void appendEquivalentSpline(const PathVectorPtr_t& path, | ||
119 | Splines_t& splines) const; | ||
120 | |||
121 | /// \} | ||
122 | |||
123 | /// \name Path validation | ||
124 | /// \{ | ||
125 | |||
126 | /// Path validation | ||
127 | /// Its size is the number of spline paths. | ||
128 | std::vector<PathValidationPtr_t> validations_; | ||
129 | |||
130 | /// Initialize validations_. | ||
131 | /// Store a pointer to the path validation of the problem for | ||
132 | /// each spline. | ||
133 | virtual void initializePathValidation(const Splines_t& splines); | ||
134 | |||
135 | typedef std::vector<std::pair<PathValidationReportPtr_t, std::size_t> > | ||
136 | Reports_t; | ||
137 | /// Calls each validations_ on the corresponding spline. | ||
138 | /// \param reordering order in which the path validation is run. | ||
139 | /// It is assumed that reordering is a permutation | ||
140 | /// of [0, splines.size()[. | ||
141 | /// \param stopAtFirst if true, then return only at most one report | ||
142 | /// corresponding to the first encountered collision. | ||
143 | /// \param reorder Put the portion in collision first in order to | ||
144 | /// improve performance of next collision check. | ||
145 | Reports_t validatePath(const Splines_t& splines, | ||
146 | std::vector<std::size_t>& reordering, bool stopAtFirst, | ||
147 | bool reorder) const; | ||
148 | |||
149 | /// \} | ||
150 | |||
151 | /// \name Constraint creation | ||
152 | /// \{ | ||
153 | |||
154 | typedef Eigen::RowBlockIndices RowBlockIndices; | ||
155 | typedef std::vector<bool> Bools_t; | ||
156 | typedef std::vector<size_type> Indices_t; | ||
157 | struct SplineOptimizationData { | ||
158 | ✗ | SplineOptimizationData() {} | |
159 |
2/4✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
|
30 | SplineOptimizationData(size_type rDof) { activeParameters.addRow(0, rDof); } |
160 | |||
161 | /// The set of constraint of the corresponding path. | ||
162 | ConstraintSetPtr_t set; | ||
163 | /// A copy of the explicit solver included in \ref set | ||
164 | shared_ptr<constraints::ExplicitConstraintSet> es; | ||
165 | |||
166 | /// Variable on which we can optimize. | ||
167 | /// Other variables are fully constrained. | ||
168 | RowBlockIndices activeParameters; | ||
169 | }; | ||
170 | typedef std::vector<SplineOptimizationData> SplineOptimizationDatas_t; | ||
171 | |||
172 | void jointBoundConstraint(const Splines_t& splines, | ||
173 | LinearConstraint& lc) const; | ||
174 | |||
175 | /// Unused | ||
176 | std::size_t addBoundConstraints(const Indices_t& bci, | ||
177 | const LinearConstraint& bc, | ||
178 | Bools_t& activeConstraint, | ||
179 | LinearConstraint& constraint) const; | ||
180 | |||
181 | /// Mostly for debugging purpose | ||
182 | Indices_t validateBounds(const Splines_t& splines, | ||
183 | const LinearConstraint& lc) const; | ||
184 | |||
185 | /// Add the linear constraint to connect consecutive splines together. | ||
186 | void addContinuityConstraints(const Splines_t& splines, | ||
187 | const size_type maxOrder, | ||
188 | const SplineOptimizationDatas_t& ess, | ||
189 | LinearConstraint& continuity); | ||
190 | |||
191 | /// \} | ||
192 | |||
193 | PathVectorPtr_t buildPathVector(const Splines_t& splines) const; | ||
194 | |||
195 | DevicePtr_t robot_; | ||
196 | |||
197 | private: | ||
198 | SplinePtr_t steer(ConfigurationIn_t q0, ConfigurationIn_t q1, | ||
199 | value_type length) const; | ||
200 | |||
201 | /// Maybe | ||
202 | // void addCollisionConstraint (const std::size_t idxSpline, | ||
203 | // const SplinePtr_t& spline, const SplinePtr_t& nextSpline, | ||
204 | // const SolverPtr_t& solver, | ||
205 | // const CollisionPathValidationReportPtr_t& report, | ||
206 | // LinearConstraint& collision, CollisionFunctions& functions) const; | ||
207 | }; // SplineGradientBasedAbstract | ||
208 | } // namespace pathOptimization | ||
209 | } // namespace core | ||
210 | } // namespace hpp | ||
211 | |||
212 | #endif // HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_ABSTRACT_HH | ||
213 |