hpp-core: include/hpp/core/path-optimization/config-optimization.hh Source File

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

Loading...
Searching...
No Matches
config-optimization.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2015 CNRS
3// Authors: Joseph Mirabel
4//
5
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are
8// met:
9//
10// 1. Redistributions of source code must retain the above copyright
11// notice, this list of conditions and the following disclaimer.
12//
13// 2. Redistributions in binary form must reproduce the above copyright
14// notice, this list of conditions and the following disclaimer in the
15// documentation and/or other materials provided with the distribution.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28// DAMAGE.
29
30#ifndef HPP_CORE_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH
31#define HPP_CORE_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH
32
33#include <functional>
34#include <hpp/core/path-optimizer.hh>
35#include <hpp/core/path-vector.hh>
36
37namespace hpp {
38namespace core {
39namespace pathOptimization {
42
51 struct ConfigOptimizationTraits {
52 static bool addConfigConstraintToPath() { return false; }
53
54 static std::size_t numberOfPass() { return 5; }
55
56 static std::size_t numberOfIterations() { return 3; }
57
58 static value_type alphaInit() { return 0.4; }
59
60 static Configuration_t getGoal(const PathVector& path) {
61 return path.initial();
62 }
63
64 static ConfigProjectorPtr_t getConfigProjector(const PathPtr_t& before,
65 const PathPtr_t& after,
66 bool& isReverse);
67
68 static bool shouldFilter(JointConstPtr_t joint, const size_type iDof);
69};
70
71 class HPP_CORE_DLLAPI ConfigOptimization : public PathOptimizer {
72 public:
74 template <typename Traits>
75 static ConfigOptimizationPtr_t createWithTraits(
76 const ProblemConstPtr_t& problem);
77
79 static ConfigOptimizationPtr_t create(const ProblemConstPtr_t& problem);
80
82 virtual PathVectorPtr_t optimize(const PathVectorPtr_t& path);
83
84 struct Parameters {
86 bool addConfigConstraintToPath;
87
88 std::size_t numberOfPass;
89
90 std::size_t numberOfIterations;
91
92 value_type alphaInit;
93
94 std::function<Configuration_t(const PathVector&)> getGoal;
95
96 std::function<ConfigProjectorPtr_t(const PathPtr_t&, const PathPtr_t&,
97 bool&)>
98 getConfigProjector;
99
100 std::function<bool(JointConstPtr_t, const size_type)> shouldFilter;
101
102 Parameters();
103 } parameters;
104
105 protected:
106 ConfigOptimization(const ProblemConstPtr_t& problem);
107
108 virtual constraints::ImplicitPtr_t createNumConstraint(
109 const PathVector& path) const;
110
111 struct Optimizer {
112 ConfigProjectorPtr_t proj;
113 virtual bool optimize(ConfigurationOut_t q,
114 const std::size_t numIter) const;
115 };
116
117 typedef std::vector<Optimizer> Optimizers_t;
118
119 virtual std::size_t buildOptimizers(const PathVector& pv,
120 Optimizers_t& projectors);
121
122 private:
123 bool isValid(const PathPtr_t& p) const;
124
125 void buildConfigVector(const PathVector& path, vectorOut_t configs) const;
126
127 template <bool forward>
128 bool pass(vectorIn_t configs, vectorOut_t newConfigs,
129 const Optimizers_t& optimizers, const std::size_t& index,
130 PathVectorPtr_t opted, bool& didChange) const;
131}; // class RandomShortcut
133
134template <typename Traits>
135 ConfigOptimizationPtr_t ConfigOptimization::createWithTraits(
136 const ProblemConstPtr_t& problem) {
137 ConfigOptimization* ptr = new ConfigOptimization(problem);
138 ptr->parameters.addConfigConstraintToPath =
139 Traits::addConfigConstraintToPath();
140 ptr->parameters.numberOfPass = Traits::numberOfPass();
141 ptr->parameters.numberOfIterations = Traits::numberOfIterations();
142 ptr->parameters.alphaInit = Traits::alphaInit();
143 ptr->parameters.getGoal = Traits::getGoal;
144 ptr->parameters.getConfigProjector = Traits::getConfigProjector;
145 ptr->parameters.shouldFilter = Traits::shouldFilter;
146 return ConfigOptimizationPtr_t(ptr);
147}
148} // namespace pathOptimization
149} // namespace core
150} // namespace hpp
151#endif // HPP_CORE_PATH_OPTIMIZATION_CONFIG_OPTIMIZATION_HH
hpp::core::PathOptimizer
Definition path-optimizer.hh:44
hpp::core::PathVector
Concatenation of several paths.
Definition path-vector.hh:43
hpp::core::PathVector::initial
virtual Configuration_t initial() const
Get the initial configuration.
Definition path-vector.hh:129
hpp::core::pathOptimization::ConfigOptimization
Definition config-optimization.hh:71
hpp::core::pathOptimization::ConfigOptimization::ConfigOptimization
ConfigOptimization(const ProblemConstPtr_t &problem)
hpp::core::pathOptimization::ConfigOptimization::optimize
virtual PathVectorPtr_t optimize(const PathVectorPtr_t &path)
Optimize path.
hpp::core::pathOptimization::ConfigOptimization::Optimizers_t
std::vector< Optimizer > Optimizers_t
Definition config-optimization.hh:117
hpp::core::pathOptimization::ConfigOptimization::buildOptimizers
virtual std::size_t buildOptimizers(const PathVector &pv, Optimizers_t &projectors)
hpp::core::pathOptimization::ConfigOptimization::parameters
struct hpp::core::pathOptimization::ConfigOptimization::Parameters parameters
hpp::core::pathOptimization::ConfigOptimization::createNumConstraint
virtual constraints::ImplicitPtr_t createNumConstraint(const PathVector &path) const
hpp::core::pathOptimization::ConfigOptimization::create
static ConfigOptimizationPtr_t create(const ProblemConstPtr_t &problem)
Return shared pointer to new object.
hpp::core::pathOptimization::ConfigOptimization::createWithTraits
static ConfigOptimizationPtr_t createWithTraits(const ProblemConstPtr_t &problem)
Return shared pointer to new object.
Definition config-optimization.hh:135
HPP_CORE_DLLAPI
#define HPP_CORE_DLLAPI
Definition config.hh:88
hpp::core::PathOptimizer::problem
ProblemConstPtr_t problem() const
Get problem.
Definition path-optimizer.hh:49
hpp
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
hpp::core::pathOptimization::ConfigOptimizationPtr_t
shared_ptr< ConfigOptimization > ConfigOptimizationPtr_t
Definition fwd.hh:310
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::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:221
hpp::core::JointConstPtr_t
pinocchio::JointConstPtr_t JointConstPtr_t
Definition fwd.hh:150
hpp::core::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:222
hpp::core::ConfigurationOut_t
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition fwd.hh:109
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::ConfigProjectorPtr_t
shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition fwd.hh:114
hpp::core::ProblemConstPtr_t
shared_ptr< const Problem > ProblemConstPtr_t
Definition fwd.hh:197
hpp::core::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::core::PathPtr_t
shared_ptr< Path > PathPtr_t
Definition fwd.hh:187
path-optimizer.hh
path-vector.hh
hpp::core::pathOptimization::ConfigOptimizationTraits
Definition config-optimization.hh:51
hpp::core::pathOptimization::ConfigOptimizationTraits::getGoal
static Configuration_t getGoal(const PathVector &path)
Definition config-optimization.hh:60
hpp::core::pathOptimization::ConfigOptimizationTraits::getConfigProjector
static ConfigProjectorPtr_t getConfigProjector(const PathPtr_t &before, const PathPtr_t &after, bool &isReverse)
hpp::core::pathOptimization::ConfigOptimizationTraits::alphaInit
static value_type alphaInit()
Definition config-optimization.hh:58
hpp::core::pathOptimization::ConfigOptimizationTraits::numberOfIterations
static std::size_t numberOfIterations()
Definition config-optimization.hh:56
hpp::core::pathOptimization::ConfigOptimizationTraits::addConfigConstraintToPath
static bool addConfigConstraintToPath()
Definition config-optimization.hh:52
hpp::core::pathOptimization::ConfigOptimizationTraits::shouldFilter
static bool shouldFilter(JointConstPtr_t joint, const size_type iDof)
hpp::core::pathOptimization::ConfigOptimizationTraits::numberOfPass
static std::size_t numberOfPass()
Definition config-optimization.hh:54
hpp::core::pathOptimization::ConfigOptimization::Optimizer
Definition config-optimization.hh:111
hpp::core::pathOptimization::ConfigOptimization::Optimizer::proj
ConfigProjectorPtr_t proj
Definition config-optimization.hh:112
hpp::core::pathOptimization::ConfigOptimization::Optimizer::optimize
virtual bool optimize(ConfigurationOut_t q, const std::size_t numIter) const
hpp::core::pathOptimization::ConfigOptimization::Parameters
Definition config-optimization.hh:84
hpp::core::pathOptimization::ConfigOptimization::Parameters::shouldFilter
std::function< bool(JointConstPtr_t, const size_type)> shouldFilter
Definition config-optimization.hh:100
hpp::core::pathOptimization::ConfigOptimization::Parameters::addConfigConstraintToPath
bool addConfigConstraintToPath
Defaults to false.
Definition config-optimization.hh:86
hpp::core::pathOptimization::ConfigOptimization::Parameters::Parameters
Parameters()
hpp::core::pathOptimization::ConfigOptimization::Parameters::numberOfIterations
std::size_t numberOfIterations
Definition config-optimization.hh:90
hpp::core::pathOptimization::ConfigOptimization::Parameters::getConfigProjector
std::function< ConfigProjectorPtr_t(const PathPtr_t &, const PathPtr_t &, bool &)> getConfigProjector
Definition config-optimization.hh:98
hpp::core::pathOptimization::ConfigOptimization::Parameters::alphaInit
value_type alphaInit
Definition config-optimization.hh:92
hpp::core::pathOptimization::ConfigOptimization::Parameters::getGoal
std::function< Configuration_t(const PathVector &)> getGoal
Definition config-optimization.hh:94
hpp::core::pathOptimization::ConfigOptimization::Parameters::numberOfPass
std::size_t numberOfPass
Definition config-optimization.hh:88