hpp-core  4.10.1
Implement basic classes for canonical path planning for kinematic chains.
spline-gradient-based.hh
Go to the documentation of this file.
1 // Copyright (c) 2017 CNRS
2 // Authors: Joseph Mirabel
3 //
4 // This file is part of hpp-core
5 // hpp-core is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-core is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-core If not, see
16 // <http://www.gnu.org/licenses/>.
17 
18 #ifndef HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_HH
19 # define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_HH
20 
21 #include <hpp/constraints/explicit-constraint-set.hh>
22 
23 #include <hpp/constraints/solver/by-substitution.hh>
24 
26 #include <hpp/core/path-vector.hh>
27 #include <hpp/core/path/spline.hh>
28 
30 
31 namespace hpp {
32  namespace core {
35  namespace pathOptimization {
36  template <int _PolynomeBasis, int _SplineOrder>
37  class HPP_CORE_DLLAPI SplineGradientBased : public SplineGradientBasedAbstract<_PolynomeBasis, _SplineOrder>
38  {
39  public:
41  enum {
42  PolynomeBasis = _PolynomeBasis,
43  SplineOrder = _SplineOrder
44  };
45  typedef boost::shared_ptr<SplineGradientBased> Ptr_t;
46 
47  using typename Base::Spline;
48  using typename Base::SplinePtr_t;
49  using typename Base::Splines_t;
50 
53  static Ptr_t create (const Problem& problem);
54 
68  virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
69 
70  protected:
71  using typename Base::RowBlockIndices;
72  using typename Base::SplineOptimizationData;
73  using typename Base::SplineOptimizationDatas_t;
74  using Base::robot_;
75  using Base::problem;
76 
77  SplineGradientBased (const Problem& problem);
78 
81 
91  virtual void addProblemConstraints (const PathVectorPtr_t& init, const Splines_t& splines, LinearConstraint& lc, SplineOptimizationDatas_t& sods) const;
92 
93  void addProblemConstraintOnPath (const PathPtr_t& path, const size_type& idxSpline, const SplinePtr_t& spline, LinearConstraint& lc, SplineOptimizationData& sod) const;
94 
98  Eigen::RowBlockIndices computeActiveParameters
99  (const PathPtr_t& path,
101  const value_type& guessThr = -1,
102  const bool& useExplicitInput = false) const;
103 
105 
107 
108  private:
109  typedef typename Base::Reports_t Reports_t;
110  struct CollisionFunctions;
111 
112  void addCollisionConstraint (const std::size_t idxSpline,
113  const SplinePtr_t& spline, const SplinePtr_t& nextSpline,
114  const SplineOptimizationData& sod,
115  const PathValidationReportPtr_t& report,
116  LinearConstraint& collision, CollisionFunctions& functions) const;
117 
118  bool findNewConstraint (LinearConstraint& constraint,
119  LinearConstraint& collision, LinearConstraint& collisionReduced,
120  CollisionFunctions& functions, const std::size_t iF,
121  const SplinePtr_t& spline, const SplineOptimizationData& sod) const;
122 
123  template <typename Cost_t> bool checkHessian (const Cost_t& cost, const matrix_t& H, const Splines_t& splines) const;
124 
125  // Continuity constraints
126  // matrix_t Jcontinuity_;
127  // vector_t rhsContinuity_;
128  }; // GradientBased
129  } // namespace pathOptimization
130  } // namespace core
131 } // namespace hpp
132 
133 #endif // HPP_CORE_PATH_OPTIMIZATION_GRADIENT_BASED_HH
hpp::core::size_type
pinocchio::size_type size_type
Definition: fwd.hh:156
hpp::core::path::Spline
Definition: fwd.hh:224
hpp::core::pathOptimization::LinearConstraint
A linear constraint .
Definition: linear-constraint.hh:28
path-vector.hh
spline-gradient-based-abstract.hh
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplinePtr_t
Spline::Ptr_t SplinePtr_t
Definition: spline-gradient-based-abstract.hh:47
hpp::core::pathOptimization::SplineGradientBasedAbstract::Splines_t
std::vector< SplinePtr_t > Splines_t
Definition: spline-gradient-based-abstract.hh:48
hpp::core::pathOptimization::SplineGradientBased::checkOptimum_
bool checkOptimum_
Definition: spline-gradient-based.hh:106
hpp::core::pathOptimization::SplineGradientBased::Base
SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder > Base
Definition: spline-gradient-based.hh:40
spline.hh
hpp::core::pathOptimization::SplineGradientBasedAbstract::Reports_t
std::vector< std::pair< PathValidationReportPtr_t, std::size_t > > Reports_t
Definition: spline-gradient-based-abstract.hh:130
hpp
Definition: basic-configuration-shooter.hh:26
hpp::core::pathOptimization::SplineGradientBasedAbstract::RowBlockIndices
Eigen::RowBlockIndices RowBlockIndices
Definition: spline-gradient-based-abstract.hh:149
hpp::core::pathOptimization::SplineGradientBasedAbstract
Common base for optimization-based path optimizer with splines.
Definition: spline-gradient-based-abstract.hh:39
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationDatas_t
std::vector< SplineOptimizationData > SplineOptimizationDatas_t
Definition: spline-gradient-based-abstract.hh:166
hpp::core::matrix_t
pinocchio::matrix_t matrix_t
Definition: fwd.hh:145
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationData
Definition: spline-gradient-based-abstract.hh:152
hpp::core::pathOptimization::SplineGradientBased
Definition: spline-gradient-based.hh:37
hpp::core::PathValidationReportPtr_t
boost::shared_ptr< PathValidationReport > PathValidationReportPtr_t
Definition: fwd.hh:298
spline.hh
hpp::core::pathOptimization::SplineGradientBased::Ptr_t
boost::shared_ptr< SplineGradientBased > Ptr_t
Definition: spline-gradient-based.hh:45
hpp::core::BySubstitution
constraints::solver::BySubstitution BySubstitution
Definition: config-projector.hh:32
hpp::core::value_type
pinocchio::value_type value_type
Definition: fwd.hh:157
hpp::core::PathVectorPtr_t
boost::shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:176
hpp::core::Problem
Definition: problem.hh:48
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition: config.hh:64
hpp::core::PathPtr_t
boost::shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170