hpp-core  4.15.1
Implement basic classes for canonical path planning for kinematic chains.
spline-gradient-based-abstract.hh
Go to the documentation of this file.
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>
36 #include <hpp/core/path-vector.hh>
37 #include <hpp/core/path/spline.hh>
39 
40 namespace hpp {
41 namespace core {
44 namespace pathOptimization {
46 template <int _PolynomeBasis, int _SplineOrder>
48  public:
49  enum { PolynomeBasis = _PolynomeBasis, SplineOrder = _SplineOrder };
51  typedef typename Spline::Ptr_t SplinePtr_t;
52  typedef std::vector<SplinePtr_t> Splines_t;
53 
56 
58  static void copy(const Splines_t& in, Splines_t& out);
59 
63  void updateSplines(Splines_t& spline, const vector_t& param) const;
64 
68  void updateParameters(vector_t& param, const Splines_t& spline) const;
69 
71  static void interpolate(const Splines_t& a, const Splines_t& b,
72  const value_type& alpha, Splines_t& res);
73 
75 
76  protected:
78 
81 
83  static PathVectorPtr_t cleanInput(const PathVectorPtr_t& input);
84 
88 
98  void appendEquivalentSpline(const StraightPathPtr_t& path,
99  Splines_t& splines) const;
100 
112  void appendEquivalentSpline(const InterpolatedPathPtr_t& path,
113  Splines_t& splines) const;
114 
118  void appendEquivalentSpline(const PathVectorPtr_t& path,
119  Splines_t& splines) const;
120 
122 
125 
128  std::vector<PathValidationPtr_t> validations_;
129 
133  virtual void initializePathValidation(const Splines_t& splines);
134 
135  typedef std::vector<std::pair<PathValidationReportPtr_t, std::size_t> >
145  Reports_t validatePath(const Splines_t& splines,
146  std::vector<std::size_t>& reordering, bool stopAtFirst,
147  bool reorder) const;
148 
150 
153 
154  typedef Eigen::RowBlockIndices RowBlockIndices;
155  typedef std::vector<bool> Bools_t;
156  typedef std::vector<size_type> Indices_t;
159  SplineOptimizationData(size_type rDof) { activeParameters.addRow(0, rDof); }
160 
164  shared_ptr<constraints::ExplicitConstraintSet> es;
165 
169  };
170  typedef std::vector<SplineOptimizationData> SplineOptimizationDatas_t;
171 
172  void jointBoundConstraint(const Splines_t& splines,
173  LinearConstraint& lc) const;
174 
176  std::size_t addBoundConstraints(const Indices_t& bci,
177  const LinearConstraint& bc,
178  Bools_t& activeConstraint,
179  LinearConstraint& constraint) const;
180 
182  Indices_t validateBounds(const Splines_t& splines,
183  const LinearConstraint& lc) const;
184 
186  void addContinuityConstraints(const Splines_t& splines,
187  const size_type maxOrder,
188  const SplineOptimizationDatas_t& ess,
189  LinearConstraint& continuity);
190 
192 
193  PathVectorPtr_t buildPathVector(const Splines_t& splines) const;
194 
196 
197  private:
199  // void addCollisionConstraint (const std::size_t idxSpline,
200  // const SplinePtr_t& spline, const SplinePtr_t& nextSpline,
201  // const SolverPtr_t& solver,
202  // const CollisionPathValidationReportPtr_t& report,
203  // LinearConstraint& collision, CollisionFunctions& functions) const;
204 }; // SplineGradientBasedAbstract
205 } // namespace pathOptimization
206 } // namespace core
207 } // namespace hpp
208 
209 #endif // HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_ABSTRACT_HH
std::vector< std::pair< PathValidationReportPtr_t, std::size_t > > Reports_t
Definition: spline-gradient-based-abstract.hh:136
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
DevicePtr_t robot_
Definition: spline-gradient-based-abstract.hh:195
shared_ptr< InterpolatedPath > InterpolatedPathPtr_t
Definition: fwd.hh:211
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:193
Definition: bi-rrt-planner.hh:35
RowBlockIndices activeParameters
Definition: spline-gradient-based-abstract.hh:168
Eigen::RowBlockIndices RowBlockIndices
Definition: spline-gradient-based-abstract.hh:154
pinocchio::size_type size_type
Definition: fwd.hh:173
steeringMethod::Spline< PolynomeBasis, SplineOrder > SSM_t
Spline steering method.
Definition: spline-gradient-based-abstract.hh:86
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:197
std::vector< bool > Bools_t
Definition: spline-gradient-based-abstract.hh:155
SplineOptimizationData()
Definition: spline-gradient-based-abstract.hh:158
shared_ptr< StraightPath > StraightPathPtr_t
Definition: fwd.hh:200
std::vector< SplinePtr_t > Splines_t
Definition: spline-gradient-based-abstract.hh:52
Common base for optimization-based path optimizer with splines.
Definition: spline-gradient-based-abstract.hh:47
shared_ptr< constraints::ExplicitConstraintSet > es
A copy of the explicit solver included in set.
Definition: spline-gradient-based-abstract.hh:164
Spline::Ptr_t SplinePtr_t
Definition: spline-gradient-based-abstract.hh:51
std::vector< PathValidationPtr_t > validations_
Definition: spline-gradient-based-abstract.hh:128
path::Spline< PolynomeBasis, SplineOrder > Spline
Definition: spline-gradient-based-abstract.hh:50
std::vector< size_type > Indices_t
Definition: spline-gradient-based-abstract.hh:156
Definition: path-optimizer.hh:44
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
pinocchio::vector_t vector_t
Definition: fwd.hh:220
pinocchio::value_type value_type
Definition: fwd.hh:174
SSM_t::Ptr_t steeringMethod_
Definition: spline-gradient-based-abstract.hh:87
Definition: fwd.hh:244
SplineOptimizationData(size_type rDof)
Definition: spline-gradient-based-abstract.hh:159
std::vector< SplineOptimizationData > SplineOptimizationDatas_t
Definition: spline-gradient-based-abstract.hh:170
shared_ptr< Spline > Ptr_t
Definition: spline.hh:108
#define HPP_CORE_DLLAPI
Definition: config.hh:64
A linear constraint .
Definition: linear-constraint.hh:39
shared_ptr< Spline > Ptr_t
Definition: spline.hh:54