hpp-core  4.12.0
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 // 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_CONFIG_PROJECTOR_HH
20 # define HPP_CORE_CONFIG_PROJECTOR_HH
21 
22 # include <Eigen/SVD>
23 
24 # include <hpp/statistics/success-bin.hh>
25 
26 # include <hpp/core/config.hh>
27 # include <hpp/core/constraint.hh>
28 # include <hpp/core/deprecated.hh>
29 
30 namespace hpp {
31  namespace core {
33 
36 
58  {
59  public:
64  Constant
65  };
66 
73  static ConfigProjectorPtr_t create (const DevicePtr_t& robot,
74  const std::string& name,
75  value_type errorThreshold,
76  size_type maxIterations);
77 
80  static ConfigProjectorPtr_t createCopy (const ConfigProjectorPtr_t cp);
81 
83  virtual ConstraintPtr_t copy () const;
84 
86  virtual ~ConfigProjector ();
87 
92  bool contains (const constraints::ImplicitPtr_t& numericalConstraint)
93  const;
94 
104  bool add (const constraints::ImplicitPtr_t& numericalConstraint,
105  const std::size_t priority = 0);
106 
107  void lastIsOptional (bool optional);
108 
109  bool lastIsOptional () const;
110 
115  bool optimize (ConfigurationOut_t config,
116  std::size_t maxIter = 0);
117 
119  const DevicePtr_t& robot () const
120  {
121  return robot_;
122  }
123 
133  void projectVectorOnKernel (ConfigurationIn_t from,
134  vectorIn_t velocity, vectorOut_t result);
135 
145  virtual void projectOnKernel (ConfigurationIn_t from,
147 
158  void computeValueAndJacobian (ConfigurationIn_t configuration,
159  vectorOut_t value,
160  matrixOut_t reducedJacobian);
161 
169 
171  size_type numberFreeVariables () const;
172 
174  size_type dimension () const;
175 
180  void compressVector (vectorIn_t normal, vectorOut_t small) const;
181 
187  void uncompressVector (vectorIn_t small, vectorOut_t normal) const;
188 
194  void compressMatrix (matrixIn_t normal, matrixOut_t small,
195  bool rows = true) const;
196 
202  void uncompressMatrix (matrixIn_t small, matrixOut_t normal,
203  bool rows = true) const;
204 
206 
208  void maxIterations (size_type iterations);
210  size_type maxIterations () const;
211 
213  void errorThreshold (const value_type& threshold);
215  value_type errorThreshold () const;
216 
217  value_type residualError() const;
218 
219  const value_type& sigma() const;
220 
223 
239  vector_t rightHandSideFromConfig (ConfigurationIn_t config);
240 
243  void rightHandSideFromConfig (const constraints::ImplicitPtr_t& nm,
244  ConfigurationIn_t config);
245 
248  void rightHandSide (const vector_t& param);
249 
252  void rightHandSide (const constraints::ImplicitPtr_t& nm, vectorIn_t rhs);
253 
256  vector_t rightHandSide () const;
257 
259  void rightHandSideAt (const value_type& s);
260 
262 
265  virtual bool isSatisfied (ConfigurationIn_t config);
266 
271  virtual bool isSatisfied (ConfigurationIn_t config, value_type
272  errorThreshold);
273 
279  virtual bool isSatisfied (ConfigurationIn_t config, vector_t& error);
280 
282  ::hpp::statistics::SuccessStatistics& statistics()
283  {
284  return statistics_;
285  }
286 
289  const NumericalConstraints_t& numericalConstraints () const;
290 
291  const BySubstitution& solver () const
292  {
293  return *solver_;
294  }
295 
298  {
299  lineSearchType_ = ls;
300  }
301 
304  {
305  return lineSearchType_;
306  }
307 
308  static void defaultLineSearch (LineSearchType ls);
309 
310  protected:
317  ConfigProjector (const DevicePtr_t& robot, const std::string& name,
318  value_type errorThreshold, size_type maxIterations);
320  ConfigProjector (const ConfigProjector& cp);
321 
323  void init (const ConfigProjectorPtr_t& self)
324  {
325  Constraint::init (self);
326  weak_ = self;
327  }
329  virtual bool impl_compute (ConfigurationOut_t configuration);
330 
331  private:
332  virtual std::ostream& print (std::ostream& os) const;
333 
334  DevicePtr_t robot_;
335  static LineSearchType defaultLineSearch_;
336  LineSearchType lineSearchType_;
337  BySubstitution* solver_;
338 
339  bool solverOneStep (ConfigurationOut_t config) const;
340  int solverSolve (ConfigurationOut_t config) const;
341 
342  ConfigProjectorWkPtr_t weak_;
343  ::hpp::statistics::SuccessStatistics statistics_;
344 
345  ConfigProjector() {}
346  HPP_SERIALIZABLE();
347  }; // class ConfigProjector
349  } // namespace core
350 } // namespace hpp
351 #endif // HPP_CORE_CONFIG_PROJECTOR_HH
void lineSearchType(LineSearchType ls)
Set the line search type.
Definition: config-projector.hh:297
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:97
Definition: bi-rrt-planner.hh:24
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:98
pinocchio::size_type size_type
Definition: fwd.hh:156
Definition: config-projector.hh:57
constraints::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:214
LineSearchType lineSearchType() const
Get the line search type.
Definition: config-projector.hh:303
shared_ptr< ConfigProjector > ConfigProjectorPtr_t
Definition: fwd.hh:104
Definition: constraint.hh:38
void init(const ConstraintPtr_t &self)
Store shared pointer to itself.
Definition: constraint.hh:80
::hpp::statistics::SuccessStatistics & statistics()
Get the statistics.
Definition: config-projector.hh:282
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:203
Definition: config-projector.hh:61
constraints::matrixIn_t matrixIn_t
Definition: fwd.hh:149
pinocchio::vector_t vector_t
Definition: fwd.hh:202
pinocchio::value_type value_type
Definition: fwd.hh:157
LineSearchType
Definition: config-projector.hh:60
void init(const ConfigProjectorPtr_t &self)
Store weak pointer to itself.
Definition: config-projector.hh:323
constraints::matrixOut_t matrixOut_t
Definition: fwd.hh:150
const BySubstitution & solver() const
Definition: config-projector.hh:291
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:204
Definition: config-projector.hh:62
Definition: config-projector.hh:63
constraints::solver::BySubstitution BySubstitution
Definition: config-projector.hh:32
#define HPP_CORE_DLLAPI
Definition: config.hh:64
shared_ptr< Constraint > ConstraintPtr_t
Definition: fwd.hh:109
const DevicePtr_t & robot() const
Get robot.
Definition: config-projector.hh:119