hpp-core  4.9.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 <boost/function.hpp>
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 
59  class HPP_CORE_DLLAPI ConfigOptimization : public PathOptimizer
60  {
61  public:
63  template < typename Traits > static
64  ConfigOptimizationPtr_t createWithTraits (const Problem& problem);
65 
67  static ConfigOptimizationPtr_t create (const Problem& problem);
68 
70  virtual PathVectorPtr_t optimize (const PathVectorPtr_t& path);
71 
72  struct Parameters {
75 
76  std::size_t numberOfPass;
77 
78  std::size_t numberOfIterations;
79 
81 
82  boost::function <Configuration_t (const PathVector&)> getGoal;
83 
84  boost::function <
85  ConfigProjectorPtr_t (const PathPtr_t&, const PathPtr_t&, bool&)
87 
88  boost::function <bool ( JointConstPtr_t, const size_type)>
90 
91  Parameters ();
92  } parameters;
93 
94  protected:
95  ConfigOptimization (const Problem& problem);
96 
97  virtual constraints::ImplicitPtr_t createNumConstraint
98  (const PathVector& path) const;
99 
100  struct Optimizer {
102  virtual bool optimize (ConfigurationOut_t q,
103  const std::size_t numIter) const;
104  };
105 
106  typedef std::vector <Optimizer> Optimizers_t;
107 
108  virtual std::size_t buildOptimizers (const PathVector& pv,
109  Optimizers_t& projectors);
110 
111  private:
112  bool isValid (const PathPtr_t& p) const;
113 
114  void buildConfigVector (const PathVector& path,
115  vectorOut_t configs) const;
116 
117  template <bool forward> bool pass (
118  vectorIn_t configs, vectorOut_t newConfigs,
119  const Optimizers_t& optimizers, const std::size_t& index,
120  PathVectorPtr_t opted, bool& didChange)
121  const;
122  }; // class RandomShortcut
124 
125  template < typename Traits > ConfigOptimizationPtr_t
127  {
128  ConfigOptimization* ptr = new ConfigOptimization (problem);
130  Traits::addConfigConstraintToPath ();
131  ptr->parameters.numberOfPass = Traits::numberOfPass ();
132  ptr->parameters.numberOfIterations = Traits::numberOfIterations ();
133  ptr->parameters.alphaInit = Traits::alphaInit ();
134  ptr->parameters.getGoal = Traits::getGoal;
135  ptr->parameters.getConfigProjector = Traits::getConfigProjector;
136  ptr->parameters.shouldFilter = Traits::shouldFilter;
137  return ConfigOptimizationPtr_t (ptr);
138  }
139  } // namespace pathOptimization
140  } // namespace core
141 } // namespace hpp
142 #endif // HPP_CORE_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH
ConfigProjectorPtr_t proj
Definition: config-optimization.hh:101
boost::shared_ptr< Path > PathPtr_t
Definition: fwd.hh:170
static ConfigProjectorPtr_t getConfigProjector(const PathPtr_t &before, const PathPtr_t &after, bool &isReverse)
boost::shared_ptr< ConfigOptimization > ConfigOptimizationPtr_t
Definition: fwd.hh:280
struct hpp::core::pathOptimization::ConfigOptimization::Parameters parameters
Definition: problem.hh:48
ObjectFactory * create(ObjectFactory *parent=NULL, const XMLElement *element=NULL)
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
pinocchio::size_type size_type
Definition: fwd.hh:156
static bool addConfigConstraintToPath()
Definition: config-optimization.hh:42
boost::function< ConfigProjectorPtr_t(const PathPtr_t &, const PathPtr_t &, bool &) > getConfigProjector
Definition: config-optimization.hh:86
pinocchio::JointConstPtr_t JointConstPtr_t
Definition: fwd.hh:132
std::size_t numberOfIterations
Definition: config-optimization.hh:78
virtual Configuration_t initial() const
Get the initial configuration.
Definition: path-vector.hh:124
value_type alphaInit
Definition: config-optimization.hh:80
std::size_t numberOfPass
Definition: config-optimization.hh:76
static std::size_t numberOfPass()
Definition: config-optimization.hh:44
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:202
Definition: config-optimization.hh:41
Definition: config-optimization.hh:59
Definition: path-optimizer.hh:32
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:74
boost::function< Configuration_t(const PathVector &)> getGoal
Definition: config-optimization.hh:82
static value_type alphaInit()
Definition: config-optimization.hh:48
boost::shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition: fwd.hh:104
static Configuration_t getGoal(const PathVector &path)
Definition: config-optimization.hh:50
static std::size_t numberOfIterations()
Definition: config-optimization.hh:46
boost::shared_ptr< PathVector > PathVectorPtr_t
Definition: fwd.hh:176
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:203
boost::function< bool(JointConstPtr_t, const size_type)> shouldFilter
Definition: config-optimization.hh:89
boost::shared_ptr< Implicit > ImplicitPtr_t
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:96
static ConfigOptimizationPtr_t createWithTraits(const Problem &problem)
Return shared pointer to new object.
Definition: config-optimization.hh:126
std::vector< Optimizer > Optimizers_t
Definition: config-optimization.hh:106
static bool shouldFilter(JointConstPtr_t joint, const size_type iDof)