GCC Code Coverage Report

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