hpp-core  4.12.0
Implement basic classes for canonical path planning for kinematic chains.
config-optimization.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015 CNRS
3 // Authors: Joseph Mirabel
4 //
5 // This file is part of hpp-core
6 // hpp-core is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-core is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_CORE_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH
20 # define HPP_CORE_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH
21 
22 # include <functional>
23 
25 # include <hpp/core/path-vector.hh>
26 
27 namespace hpp {
28  namespace core {
29  namespace pathOptimization {
32 
42  static bool addConfigConstraintToPath () { return false; }
43 
44  static std::size_t numberOfPass () { return 5; }
45 
46  static std::size_t numberOfIterations () { return 3; }
47 
48  static value_type alphaInit () { return 0.4; }
49 
50  static Configuration_t getGoal (const PathVector& path)
51  { return path.initial (); }
52 
54  (const PathPtr_t& before, const PathPtr_t& after, bool& isReverse);
55 
56  static bool shouldFilter (JointConstPtr_t joint, const size_type iDof);
57  };
58 
60  {
61  public:
63  template < typename Traits > static
64  ConfigOptimizationPtr_t createWithTraits
65  (const ProblemConstPtr_t& problem);
66 
68  static ConfigOptimizationPtr_t create
69  (const ProblemConstPtr_t& problem);
70 
72  virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
73 
74  struct Parameters {
77 
78  std::size_t numberOfPass;
79 
80  std::size_t numberOfIterations;
81 
83 
84  std::function <Configuration_t (const PathVector&)> getGoal;
85 
86  std::function <
87  ConfigProjectorPtr_t (const PathPtr_t&, const PathPtr_t&, bool&)
89 
90  std::function <bool ( JointConstPtr_t, const size_type)>
92 
93  Parameters ();
94  } parameters;
95 
96  protected:
97  ConfigOptimization (const ProblemConstPtr_t& problem);
98 
99  virtual constraints::ImplicitPtr_t createNumConstraint
100  (const PathVector& path) const;
101 
102  struct Optimizer {
104  virtual bool optimize (ConfigurationOut_t q,
105  const std::size_t numIter) const;
106  };
107 
108  typedef std::vector <Optimizer> Optimizers_t;
109 
110  virtual std::size_t buildOptimizers (const PathVector& pv,
111  Optimizers_t& projectors);
112 
113  private:
114  bool isValid (const PathPtr_t& p) const;
115 
116  void buildConfigVector (const PathVector& path,
117  vectorOut_t configs) const;
118 
119  template <bool forward> bool pass (
120  vectorIn_t configs, vectorOut_t newConfigs,
121  const Optimizers_t& optimizers, const std::size_t& index,
122  PathVectorPtr_t opted, bool& didChange)
123  const;
124  }; // class RandomShortcut
126 
127  template < typename Traits > ConfigOptimizationPtr_t
129  {
130  ConfigOptimization* ptr = new ConfigOptimization (problem);
132  Traits::addConfigConstraintToPath ();
133  ptr->parameters.numberOfPass = Traits::numberOfPass ();
134  ptr->parameters.numberOfIterations = Traits::numberOfIterations ();
135  ptr->parameters.alphaInit = Traits::alphaInit ();
136  ptr->parameters.getGoal = Traits::getGoal;
137  ptr->parameters.getConfigProjector = Traits::getConfigProjector;
138  ptr->parameters.shouldFilter = Traits::shouldFilter;
139  return ConfigOptimizationPtr_t (ptr);
140  }
141  } // namespace pathOptimization
142  } // namespace core
143 } // namespace hpp
144 #endif // HPP_CORE_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH
ConfigProjectorPtr_t proj
Definition: config-optimization.hh:103
static ConfigProjectorPtr_t getConfigProjector(const PathPtr_t &before, const PathPtr_t &after, bool &isReverse)
std::function< Configuration_t(const PathVector &)> getGoal
Definition: config-optimization.hh:84
struct hpp::core::pathOptimization::ConfigOptimization::Parameters parameters
shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:176
Definition: bi-rrt-planner.hh:24
shared_ptr< ConfigOptimization > ConfigOptimizationPtr_t
Definition: fwd.hh:281
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
pinocchio::size_type size_type
Definition: fwd.hh:156
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:180
static bool addConfigConstraintToPath()
Definition: config-optimization.hh:42
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:132
std::size_t numberOfIterations
Definition: config-optimization.hh:80
virtual Configuration_t initial() const
Get the initial configuration.
Definition: path-vector.hh:136
value_type alphaInit
Definition: config-optimization.hh:82
std::size_t numberOfPass
Definition: config-optimization.hh:78
shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition: fwd.hh:104
static std::size_t numberOfPass()
Definition: config-optimization.hh:44
std::function< ConfigProjectorPtr_t(const PathPtr_t &, const PathPtr_t &, bool &) > getConfigProjector
Definition: config-optimization.hh:88
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:203
Definition: config-optimization.hh:41
std::function< bool(JointConstPtr_t, const size_type)> shouldFilter
Definition: config-optimization.hh:91
Definition: config-optimization.hh:59
Definition: path-optimizer.hh:34
pinocchio::value_type value_type
Definition: fwd.hh:157
Concatenation of several paths.
Definition: path-vector.hh:32
bool addConfigConstraintToPath
Defaults to false.
Definition: config-optimization.hh:76
static value_type alphaInit()
Definition: config-optimization.hh:48
static Configuration_t getGoal(const PathVector &path)
Definition: config-optimization.hh:50
static std::size_t numberOfIterations()
Definition: config-optimization.hh:46
static ConfigOptimizationPtr_t createWithTraits(const ProblemConstPtr_t &problem)
Return shared pointer to new object.
Definition: config-optimization.hh:128
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:204
#define HPP_CORE_DLLAPI
Definition: config.hh:64
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
std::vector< Optimizer > Optimizers_t
Definition: config-optimization.hh:108
shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
static bool shouldFilter(JointConstPtr_t joint, const size_type iDof)