hpp-core  4.15.1
Implement basic classes for canonical path planning for kinematic chains.
config-projector.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Florent Lamiraux
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_CONFIG_PROJECTOR_HH
31 #define HPP_CORE_CONFIG_PROJECTOR_HH
32 
33 #include <Eigen/SVD>
34 #include <hpp/core/config.hh>
35 #include <hpp/core/constraint.hh>
36 #include <hpp/core/deprecated.hh>
37 #include <hpp/statistics/success-bin.hh>
38 
39 namespace hpp {
40 namespace core {
42 
45 
67  public:
68  enum LineSearchType { Backtracking, ErrorNormBased, FixedSequence, Constant };
69 
76  static ConfigProjectorPtr_t create(const DevicePtr_t& robot,
77  const std::string& name,
78  value_type errorThreshold,
79  size_type maxIterations);
80 
83  static ConfigProjectorPtr_t createCopy(const ConfigProjectorPtr_t cp);
84 
86  virtual ConstraintPtr_t copy() const;
87 
89  virtual ~ConfigProjector();
90 
95  bool contains(const constraints::ImplicitPtr_t& numericalConstraint) const;
96 
106  bool add(const constraints::ImplicitPtr_t& numericalConstraint,
107  const std::size_t priority = 0);
108 
109  void lastIsOptional(bool optional);
110 
111  bool lastIsOptional() const;
112 
117  bool optimize(ConfigurationOut_t config, std::size_t maxIter = 0);
118 
120  const DevicePtr_t& robot() const { return robot_; }
121 
131  void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity,
132  vectorOut_t result);
133 
143  virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to,
144  ConfigurationOut_t result);
145 
156  void computeValueAndJacobian(ConfigurationIn_t configuration,
157  vectorOut_t value, matrixOut_t reducedJacobian);
158 
166 
168  size_type numberFreeVariables() const;
169 
171  size_type dimension() const;
172 
177  void compressVector(vectorIn_t normal, vectorOut_t small) const;
178 
184  void uncompressVector(vectorIn_t small, vectorOut_t normal) const;
185 
191  void compressMatrix(matrixIn_t normal, matrixOut_t small,
192  bool rows = true) const;
193 
199  void uncompressMatrix(matrixIn_t small, matrixOut_t normal,
200  bool rows = true) const;
201 
203 
205  void maxIterations(size_type iterations);
207  size_type maxIterations() const;
208 
210  void errorThreshold(const value_type& threshold);
212  value_type errorThreshold() const;
213 
214  value_type residualError() const;
215 
216  const value_type& sigma() const;
217 
220 
236  vector_t rightHandSideFromConfig(ConfigurationIn_t config);
237 
240  void rightHandSideFromConfig(const constraints::ImplicitPtr_t& nm,
241  ConfigurationIn_t config);
242 
245  void rightHandSide(const vector_t& param);
246 
249  void rightHandSide(const constraints::ImplicitPtr_t& nm, vectorIn_t rhs);
250 
253  vector_t rightHandSide() const;
254 
256  void rightHandSideAt(const value_type& s);
257 
259 
262  virtual bool isSatisfied(ConfigurationIn_t config);
263 
268  virtual bool isSatisfied(ConfigurationIn_t config, value_type errorThreshold);
269 
276  virtual bool isSatisfied(ConfigurationIn_t config, vector_t& error);
277 
279  ::hpp::statistics::SuccessStatistics& statistics() { return statistics_; }
280 
283  const NumericalConstraints_t& numericalConstraints() const;
284 
285  const BySubstitution& solver() const { return *solver_; }
286 
287  BySubstitution& solver() { return *solver_; }
288 
290  void lineSearchType(LineSearchType ls) { lineSearchType_ = ls; }
291 
293  LineSearchType lineSearchType() const { return lineSearchType_; }
294 
295  static void defaultLineSearch(LineSearchType ls);
296 
297  protected:
304  ConfigProjector(const DevicePtr_t& robot, const std::string& name,
305  value_type errorThreshold, size_type maxIterations);
307  ConfigProjector(const ConfigProjector& cp);
308 
310  void init(const ConfigProjectorPtr_t& self) {
311  Constraint::init(self);
312  weak_ = self;
313  }
315  virtual bool impl_compute(ConfigurationOut_t configuration);
316 
317  private:
318  virtual std::ostream& print(std::ostream& os) const;
319 
320  DevicePtr_t robot_;
321  static LineSearchType defaultLineSearch_;
322  LineSearchType lineSearchType_;
323  BySubstitution* solver_;
324 
325  bool solverOneStep(ConfigurationOut_t config) const;
326  int solverSolve(ConfigurationOut_t config) const;
327 
328  ConfigProjectorWkPtr_t weak_;
329  ::hpp::statistics::SuccessStatistics statistics_;
330 
331  ConfigProjector() {}
332  HPP_SERIALIZABLE();
333 }; // class ConfigProjector
335 } // namespace core
336 } // namespace hpp
337 #endif // HPP_CORE_CONFIG_PROJECTOR_HH
void lineSearchType(LineSearchType ls)
Set the line search type.
Definition: config-projector.hh:290
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:107
Definition: bi-rrt-planner.hh:35
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:108
pinocchio::size_type size_type
Definition: fwd.hh:173
BySubstitution & solver()
Definition: config-projector.hh:287
Definition: config-projector.hh:66
constraints::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:232
LineSearchType lineSearchType() const
Get the line search type.
Definition: config-projector.hh:293
shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition: fwd.hh:114
Definition: constraint.hh:49
void init(const ConstraintPtr_t &self)
Store shared pointer to itself.
Definition: constraint.hh:82
::hpp::statistics::SuccessStatistics & statistics()
Get the statistics.
Definition: config-projector.hh:279
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:221
constraints::matrixIn_t matrixIn_t
Definition: fwd.hh:166
pinocchio::vector_t vector_t
Definition: fwd.hh:220
pinocchio::value_type value_type
Definition: fwd.hh:174
LineSearchType
Definition: config-projector.hh:68
void init(const ConfigProjectorPtr_t &self)
Store weak pointer to itself.
Definition: config-projector.hh:310
constraints::matrixOut_t matrixOut_t
Definition: fwd.hh:167
const BySubstitution & solver() const
Definition: config-projector.hh:285
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:222
constraints::solver::BySubstitution BySubstitution
Definition: config-projector.hh:41
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:129
const DevicePtr_t & robot() const
Get robot.
Definition: config-projector.hh:120