hpp-core: include/hpp/core/path-optimization/spline-gradient-based.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
spline-gradient-based.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_HH
30#define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_HH
31
32#include <hpp/constraints/explicit-constraint-set.hh>
33#include <hpp/constraints/solver/by-substitution.hh>
34#include <hpp/core/path-optimization/spline-gradient-based-abstract.hh>
35#include <hpp/core/path-vector.hh>
36#include <hpp/core/path/spline.hh>
37#include <hpp/core/steering-method/spline.hh>
38
39namespace hpp {
40namespace core {
43namespace pathOptimization {
44template <int _PolynomeBasis, int _SplineOrder>
45 class HPP_CORE_DLLAPI SplineGradientBased
46 : public SplineGradientBasedAbstract<_PolynomeBasis, _SplineOrder> {
47 public:
48 typedef SplineGradientBasedAbstract<_PolynomeBasis, _SplineOrder> Base;
49 enum { PolynomeBasis = _PolynomeBasis, SplineOrder = _SplineOrder };
50 typedef shared_ptr<SplineGradientBased> Ptr_t;
51
52 using typename Base::Spline;
53 using typename Base::SplinePtr_t;
54 using typename Base::Splines_t;
55
58 static Ptr_t create(const ProblemConstPtr_t& problem);
59
73 virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
74
77 value_type alphaInit;
79 bool alwaysStopAtFirst;
82 size_type costOrder;
84 bool usePathLengthAsWeights;
86 bool reorderIntervals;
88 bool linearizeAtEachStep;
90 bool checkJointBound;
93 bool returnOptimum;
96 value_type costThreshold;
100 value_type guessThreshold;
102 value_type QPAccuracy;
103
104 protected:
105 using Base::problem;
106 using Base::robot_;
107 using typename Base::RowBlockIndices;
108 using typename Base::SplineOptimizationData;
109 using typename Base::SplineOptimizationDatas_t;
110
111 SplineGradientBased(const ProblemConstPtr_t& problem);
112
115
125 virtual void addProblemConstraints(const PathVectorPtr_t& init,
126 const Splines_t& splines,
127 LinearConstraint& lc,
128 SplineOptimizationDatas_t& sods) const;
129
130 void addProblemConstraintOnPath(const PathPtr_t& path,
131 const size_type& idxSpline,
132 const SplinePtr_t& spline,
133 LinearConstraint& lc,
134 SplineOptimizationData& sod) const;
135
139 Eigen::RowBlockIndices computeActiveParameters(
140 const PathPtr_t& path, const constraints::solver::BySubstitution& hs,
141 const value_type& guessThr = -1,
142 const bool& useExplicitInput = false) const;
143
145
146 bool checkOptimum_;
147
148 private:
149 typedef typename Base::Reports_t Reports_t;
150 struct CollisionFunctions;
151
152 void addCollisionConstraint(const std::size_t idxSpline,
153 const SplinePtr_t& spline,
154 const SplinePtr_t& nextSpline,
155 const SplineOptimizationData& sod,
156 const PathValidationReportPtr_t& report,
157 LinearConstraint& collision,
158 CollisionFunctions& functions) const;
159
160 bool findNewConstraint(LinearConstraint& constraint,
161 LinearConstraint& collision,
162 LinearConstraint& collisionReduced,
163 CollisionFunctions& functions, const std::size_t iF,
164 const SplinePtr_t& spline,
165 const SplineOptimizationData& sod) const;
166
167 template <typename Cost_t>
168 bool checkHessian(const Cost_t& cost, const matrix_t& H,
169 const Splines_t& splines) const;
170
171 // Continuity constraints
172 // matrix_t Jcontinuity_;
173 // vector_t rhsContinuity_;
174}; // GradientBased
175} // namespace pathOptimization
176} // namespace core
177} // namespace hpp
178
179#endif // HPP_CORE_PATH_OPTIMIZATION_GRADIENT_BASED_HH
by-substitution.hh
Eigen::MatrixBlocks< false, true >
hpp::constraints::solver::BySubstitution
hpp::core::pathOptimization::SplineGradientBasedAbstract
Common base for optimization-based path optimizer with splines.
Definition spline-gradient-based-abstract.hh:47
hpp::core::pathOptimization::SplineGradientBased
Definition spline-gradient-based.hh:46
hpp::core::pathOptimization::SplineGradientBased::Splines_t
std::vector< SplinePtr_t > Splines_t
Definition spline-gradient-based-abstract.hh:52
hpp::core::pathOptimization::SplineGradientBased::SplineOptimizationDatas_t
std::vector< SplineOptimizationData > SplineOptimizationDatas_t
Definition spline-gradient-based-abstract.hh:170
hpp::core::pathOptimization::SplineGradientBased::SplinePtr_t
Spline::Ptr_t SplinePtr_t
Definition spline-gradient-based-abstract.hh:51
hpp::core::path::Spline
Definition spline.hh:91
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
explicit-constraint-set.hh
hpp::core::pathOptimization::SplineGradientBased::optimize
virtual PathVectorPtr_t optimize(const PathVectorPtr_t &path)
hpp::core::pathOptimization::SplineGradientBased::costThreshold
value_type costThreshold
Definition spline-gradient-based.hh:96
hpp::core::pathOptimization::SplineGradientBased::addProblemConstraints
virtual void addProblemConstraints(const PathVectorPtr_t &init, const Splines_t &splines, LinearConstraint &lc, SplineOptimizationDatas_t &sods) const
hpp::core::pathOptimization::SplineGradientBased::alwaysStopAtFirst
bool alwaysStopAtFirst
If true, consider only one (not all) collision constraint per iteration.
Definition spline-gradient-based.hh:79
hpp::core::pathOptimization::SplineGradientBased::usePathLengthAsWeights
bool usePathLengthAsWeights
If true, the initial path length is used to weight the splines.
Definition spline-gradient-based.hh:84
hpp::core::pathOptimization::SplineGradientBased::create
static Ptr_t create(const ProblemConstPtr_t &problem)
hpp::core::pathOptimization::SplineGradientBased::alphaInit
value_type alphaInit
Definition spline-gradient-based.hh:77
hpp::core::pathOptimization::SplineGradientBased::SplineGradientBased
SplineGradientBased(const ProblemConstPtr_t &problem)
hpp::core::pathOptimization::SplineGradientBasedAbstract::Reports_t
std::vector< std::pair< PathValidationReportPtr_t, std::size_t > > Reports_t
Definition spline-gradient-based-abstract.hh:136
hpp::core::pathOptimization::SplineGradientBased::costOrder
size_type costOrder
Definition spline-gradient-based.hh:82
hpp::core::pathOptimization::SplineGradientBased::linearizeAtEachStep
bool linearizeAtEachStep
If true, collision constraint will be re-linearized at each iteration.
Definition spline-gradient-based.hh:88
hpp::core::pathOptimization::SplineGradientBasedAbstract::Splines_t
std::vector< SplinePtr_t > Splines_t
Definition spline-gradient-based-abstract.hh:52
hpp::core::pathOptimization::SplineGradientBased::returnOptimum
bool returnOptimum
Definition spline-gradient-based.hh:93
hpp::core::pathOptimization::SplineGradientBased::reorderIntervals
bool reorderIntervals
If true, intervals in collision are checked first at the next iteration.
Definition spline-gradient-based.hh:86
hpp::core::pathOptimization::SplineGradientBased::guessThreshold
value_type guessThreshold
Definition spline-gradient-based.hh:100
hpp::core::pathOptimization::SplineGradientBased::checkOptimum_
bool checkOptimum_
Definition spline-gradient-based.hh:146
hpp::core::pathOptimization::SplineGradientBased::checkJointBound
bool checkJointBound
If true, joint bounds are enforced.
Definition spline-gradient-based.hh:90
hpp::core::pathOptimization::SplineGradientBased::QPAccuracy
value_type QPAccuracy
Accuracy of QP solver (only used by proxqp).
Definition spline-gradient-based.hh:102
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationDatas_t
std::vector< SplineOptimizationData > SplineOptimizationDatas_t
Definition spline-gradient-based-abstract.hh:170
hpp::core::pathOptimization::SplineGradientBased::computeActiveParameters
Eigen::RowBlockIndices computeActiveParameters(const PathPtr_t &path, const constraints::solver::BySubstitution &hs, const value_type &guessThr=-1, const bool &useExplicitInput=false) const
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplinePtr_t
Spline::Ptr_t SplinePtr_t
Definition spline-gradient-based-abstract.hh:51
hpp::core::pathOptimization::SplineGradientBased::Base
SplineGradientBasedAbstract< _PolynomeBasis, _SplineOrder > Base
Definition spline-gradient-based.hh:48
hpp::core::pathOptimization::SplineGradientBased::addProblemConstraintOnPath
void addProblemConstraintOnPath(const PathPtr_t &path, const size_type &idxSpline, const SplinePtr_t &spline, LinearConstraint &lc, SplineOptimizationData &sod) const
hpp::core::pathOptimization::SplineGradientBased::Ptr_t
shared_ptr< SplineGradientBased > Ptr_t
Definition spline-gradient-based.hh:50
hpp
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::PathVectorPtr_t
shared_ptr< PathVector > PathVectorPtr_t
Definition fwd.hh:193
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::ProblemConstPtr_t
shared_ptr< const Problem > ProblemConstPtr_t
Definition fwd.hh:197
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
hpp::core::PathValidationReportPtr_t
shared_ptr< PathValidationReport > PathValidationReportPtr_t
Definition fwd.hh:329
hpp::core::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:162
path-vector.hh
spline.hh
spline-gradient-based-abstract.hh
spline.hh
hpp::core::pathOptimization::LinearConstraint
A linear constraint .
Definition linear-constraint.hh:39
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationData
Definition spline-gradient-based-abstract.hh:157