hpp-core: include/hpp/core/path-optimization/spline-gradient-based-abstract.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-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>
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
40namespace hpp {
41namespace core {
44namespace pathOptimization {
46template <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
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:
77 SplineGradientBasedAbstract(const ProblemConstPtr_t& problem);
78
81
83 static PathVectorPtr_t cleanInput(const PathVectorPtr_t& input);
84
86 typedef steeringMethod::Spline<PolynomeBasis, SplineOrder> SSM_t;
87 typename SSM_t::Ptr_t steeringMethod_;
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> >
136 Reports_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;
157 struct SplineOptimizationData {
158 SplineOptimizationData() {}
159 SplineOptimizationData(size_type rDof) { activeParameters.addRow(0, rDof); }
160
162 ConstraintSetPtr_t set;
164 shared_ptr<constraints::ExplicitConstraintSet> es;
165
168 RowBlockIndices activeParameters;
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
195 DevicePtr_t robot_;
196
197 private:
198 SplinePtr_t steer(ConfigurationIn_t q0, ConfigurationIn_t q1,
199 value_type length) const;
200
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
by-substitution.hh
Eigen::MatrixBlocks< false, true >
hpp::core::PathOptimizer
Definition path-optimizer.hh:44
hpp::core::pathOptimization::SplineGradientBasedAbstract
Common base for optimization-based path optimizer with splines.
Definition spline-gradient-based-abstract.hh:47
hpp::core::path::Spline
Definition spline.hh:91
hpp::core::path::Spline::Ptr_t
shared_ptr< Spline > Ptr_t
Definition spline.hh:111
hpp::core::steeringMethod::Spline
Definition spline.hh:48
hpp::core::steeringMethod::Spline::Ptr_t
shared_ptr< Spline > Ptr_t
Definition spline.hh:54
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
explicit-constraint-set.hh
hpp::core::pathOptimization::SplineGradientBasedAbstract::jointBoundConstraint
void jointBoundConstraint(const Splines_t &splines, LinearConstraint &lc) const
hpp::core::pathOptimization::SplineGradientBasedAbstract::appendEquivalentSpline
void appendEquivalentSpline(const InterpolatedPathPtr_t &path, Splines_t &splines) const
hpp::core::pathOptimization::SplineGradientBasedAbstract::cleanInput
static PathVectorPtr_t cleanInput(const PathVectorPtr_t &input)
Flatten path and remove path of zero length.
hpp::core::pathOptimization::SplineGradientBasedAbstract::Bools_t
std::vector< bool > Bools_t
Definition spline-gradient-based-abstract.hh:155
hpp::core::pathOptimization::SplineGradientBasedAbstract::interpolate
static void interpolate(const Splines_t &a, const Splines_t &b, const value_type &alpha, Splines_t &res)
Returns res = (1 - alpha) * a + alpha * b.
hpp::core::pathOptimization::SplineGradientBasedAbstract::addContinuityConstraints
void addContinuityConstraints(const Splines_t &splines, const size_type maxOrder, const SplineOptimizationDatas_t &ess, LinearConstraint &continuity)
Add the linear constraint to connect consecutive splines together.
hpp::core::pathOptimization::SplineGradientBasedAbstract::appendEquivalentSpline
void appendEquivalentSpline(const StraightPathPtr_t &path, Splines_t &splines) const
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationData::set
ConstraintSetPtr_t set
The set of constraint of the corresponding path.
Definition spline-gradient-based-abstract.hh:162
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::SplineGradientBasedAbstract::Splines_t
std::vector< SplinePtr_t > Splines_t
Definition spline-gradient-based-abstract.hh:52
hpp::core::pathOptimization::SplineGradientBasedAbstract::Spline
path::Spline< PolynomeBasis, SplineOrder > Spline
Definition spline-gradient-based-abstract.hh:50
hpp::core::pathOptimization::SplineGradientBasedAbstract::updateParameters
void updateParameters(vector_t &param, const Splines_t &spline) const
hpp::core::pathOptimization::SplineGradientBasedAbstract::validations_
std::vector< PathValidationPtr_t > validations_
Definition spline-gradient-based-abstract.hh:128
hpp::core::pathOptimization::SplineGradientBasedAbstract::updateSplines
void updateSplines(Splines_t &spline, const vector_t &param) const
hpp::core::pathOptimization::SplineGradientBasedAbstract::addBoundConstraints
std::size_t addBoundConstraints(const Indices_t &bci, const LinearConstraint &bc, Bools_t &activeConstraint, LinearConstraint &constraint) const
Unused.
hpp::core::pathOptimization::SplineGradientBasedAbstract::steeringMethod_
SSM_t::Ptr_t steeringMethod_
Definition spline-gradient-based-abstract.hh:87
hpp::core::pathOptimization::SplineGradientBasedAbstract::appendEquivalentSpline
void appendEquivalentSpline(const PathVectorPtr_t &path, Splines_t &splines) const
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineGradientBasedAbstract
SplineGradientBasedAbstract(const ProblemConstPtr_t &problem)
hpp::core::pathOptimization::SplineGradientBasedAbstract::validateBounds
Indices_t validateBounds(const Splines_t &splines, const LinearConstraint &lc) const
Mostly for debugging purpose.
hpp::core::pathOptimization::SplineGradientBasedAbstract::validatePath
Reports_t validatePath(const Splines_t &splines, std::vector< std::size_t > &reordering, bool stopAtFirst, bool reorder) const
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationData::SplineOptimizationData
SplineOptimizationData(size_type rDof)
Definition spline-gradient-based-abstract.hh:159
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationDatas_t
std::vector< SplineOptimizationData > SplineOptimizationDatas_t
Definition spline-gradient-based-abstract.hh:170
hpp::core::pathOptimization::SplineGradientBasedAbstract::RowBlockIndices
Eigen::RowBlockIndices RowBlockIndices
Definition spline-gradient-based-abstract.hh:154
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationData::SplineOptimizationData
SplineOptimizationData()
Definition spline-gradient-based-abstract.hh:158
hpp::core::pathOptimization::SplineGradientBasedAbstract::copy
static void copy(const Splines_t &in, Splines_t &out)
Copy a vector of Spline.
hpp::core::pathOptimization::SplineGradientBasedAbstract::SSM_t
steeringMethod::Spline< PolynomeBasis, SplineOrder > SSM_t
Spline steering method.
Definition spline-gradient-based-abstract.hh:86
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationData::es
shared_ptr< constraints::ExplicitConstraintSet > es
A copy of the explicit solver included in set.
Definition spline-gradient-based-abstract.hh:164
hpp::core::pathOptimization::SplineGradientBasedAbstract::initializePathValidation
virtual void initializePathValidation(const Splines_t &splines)
hpp::core::pathOptimization::SplineGradientBasedAbstract::Indices_t
std::vector< size_type > Indices_t
Definition spline-gradient-based-abstract.hh:156
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplinePtr_t
Spline::Ptr_t SplinePtr_t
Definition spline-gradient-based-abstract.hh:51
hpp::core::pathOptimization::SplineGradientBasedAbstract::robot_
DevicePtr_t robot_
Definition spline-gradient-based-abstract.hh:195
hpp::core::pathOptimization::SplineGradientBasedAbstract::buildPathVector
PathVectorPtr_t buildPathVector(const Splines_t &splines) const
hpp::core::pathOptimization::SplineGradientBasedAbstract::SplineOptimizationData::activeParameters
RowBlockIndices activeParameters
Definition spline-gradient-based-abstract.hh:168
linear-constraint.hh
hpp
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::StraightPathPtr_t
shared_ptr< StraightPath > StraightPathPtr_t
Definition fwd.hh:200
hpp::core::PathVectorPtr_t
shared_ptr< PathVector > PathVectorPtr_t
Definition fwd.hh:193
hpp::core::InterpolatedPathPtr_t
shared_ptr< InterpolatedPath > InterpolatedPathPtr_t
Definition fwd.hh:211
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:220
hpp::core::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
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::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:134
hpp::core::ConstraintSetPtr_t
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition fwd.hh:130
path-optimizer.hh
path-vector.hh
spline.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