hpp-constraints  4.12.0
Definition of basic geometric constraints for motion planning
hierarchical-iterative.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, 2018
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 // This file is part of hpp-constraints.
5 // hpp-constraints is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-constraints is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-constraints. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
18 #define HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
19 
20 #include <map>
21 #include <functional>
22 
23 #include <hpp/util/serialization-fwd.hh>
24 
25 #include <hpp/util/serialization-fwd.hh>
26 
27 #include <hpp/constraints/fwd.hh>
29 
32 
33 namespace hpp {
34  namespace constraints {
35  namespace solver {
38  namespace lineSearch {
40  struct Constant {
41  template <typename SolverType>
42  bool operator() (const SolverType& solver, vectorOut_t arg,
43  vectorOut_t darg);
44  };
45 
48  struct Backtracking {
49  Backtracking ();
50 
51  template <typename SolverType>
52  bool operator() (const SolverType& solver, vectorOut_t arg,
54 
55  template <typename SolverType>
56  inline value_type computeLocalSlope(const SolverType& solver) const;
57 
58  value_type c, tau, smallAlpha; // 0.8 ^ 7 = 0.209, 0.8 ^ 8 = 0.1677
59  mutable vector_t arg_darg, df, darg;
60  };
61 
65  struct FixedSequence {
66  FixedSequence();
67 
68  template <typename SolverType>
69  bool operator() (const SolverType& solver, vectorOut_t arg,
70  vectorOut_t darg);
71 
74  };
75 
81  struct ErrorNormBased {
83  ErrorNormBased(value_type alphaMin = 0.2);
84 
85  template <typename SolverType>
86  bool operator() (const SolverType& solver, vectorOut_t arg,
87  vectorOut_t darg);
88 
90  };
91  } // namespace lineSearch
92 
93  namespace saturation {
101  struct Base {
113  virtual bool saturate(vectorIn_t q, vectorOut_t qSat,
114  Eigen::VectorXi& saturation);
115  virtual ~Base() {}
116  };
118  struct Function : Base {
119  typedef std::function<bool (vectorIn_t, vectorOut_t, Eigen::VectorXi&)> function_t;
121  Eigen::VectorXi& saturation) {
122  return function(q, qSat, saturation);
123  }
124  Function () {}
125  Function (const function_t& function) : function(function) {}
126  function_t function;
127  };
129  struct Bounds : Base {
130  bool saturate(vectorIn_t q, vectorOut_t qSat,
131  Eigen::VectorXi& saturation);
132  Bounds () {}
133  Bounds (const vector_t& lb, const vector_t& ub) : lb(lb), ub(ub) {}
135  };
137  struct Device : Base {
139  bool saturate(vectorIn_t q, vectorOut_t qSat,
140  Eigen::VectorXi& saturation);
141  Device() {}
144  };
145  }
146 
219  {
220  public:
223 
224  enum Status {
228  SUCCESS
229  };
230  typedef shared_ptr<saturation::Base> Saturation_t;
231 
232  HierarchicalIterative (const LiegroupSpacePtr_t& configSpace);
233 
235 
237 
240 
243  {
244  return configSpace_;
245  }
252  virtual bool contains (const ImplicitPtr_t& numericalConstraint) const;
253 
259  virtual bool add (const ImplicitPtr_t& constraint, const std::size_t& priority);
260 
266  virtual void merge (const HierarchicalIterative& other);
267 
269  void saturation (const Saturation_t& saturate)
270  {
271  saturate_ = saturate;
272  }
273 
275  const Saturation_t& saturation () const
276  {
277  return saturate_;
278  }
279 
281 
284 
299  template <typename LineSearchType>
300  Status solve (vectorOut_t arg, LineSearchType ls = LineSearchType()) const;
301 
316  inline Status solve (vectorOut_t arg) const
317  {
318  return solve (arg, DefaultLineSearch());
319  }
320 
324  bool isSatisfied (vectorIn_t arg) const
325  {
326  computeValue<false>(arg);
327  computeError();
328  return squaredNorm_ < squaredErrorThreshold_;
329  }
330 
335  bool isSatisfied (vectorIn_t arg, value_type errorThreshold) const
336  {
337  computeValue<false>(arg);
338  computeError();
339  return squaredNorm_ < errorThreshold*errorThreshold;
340  }
341 
351  bool isConstraintSatisfied (const ImplicitPtr_t& constraint,
352  vectorIn_t arg, vectorOut_t error,
353  bool& constraintFound) const;
354 
359  const value_type& sigma () const
360  {
361  return sigma_;
362  }
363 
365 
368 
375  void freeVariables (const segments_t intervals)
376  {
377  freeVariables_ = Indices_t();
378  for (std::size_t i = 0; i < intervals.size(); ++i)
379  freeVariables_.addRow (intervals[i].first, intervals[i].second);
380  freeVariables_.updateIndices<true, true, true>();
381  update ();
382  }
383 
388  void freeVariables (const Indices_t& indices)
389  {
390  freeVariables_ = indices;
391  update ();
392  }
393 
395  const Indices_t& freeVariables () const
396  {
397  return freeVariables_;
398  }
399 
401  void maxIterations (size_type iterations)
402  {
403  maxIterations_ = iterations;
404  }
407  {
408  return maxIterations_;
409  }
410 
412  void errorThreshold (const value_type& threshold)
413  {
414  squaredErrorThreshold_ = threshold * threshold;
415  }
418  {
419  return sqrt (squaredErrorThreshold_);
420  }
423  {
424  return squaredErrorThreshold_;
425  }
426 
429  {
430  return inequalityThreshold_;
431  }
434  {
435  inequalityThreshold_ = it;
436  }
437 
438  void lastIsOptional (bool optional)
439  {
440  lastIsOptional_ = optional;
441  }
442 
443  bool lastIsOptional () const
444  {
445  return lastIsOptional_;
446  }
447 
449 
452 
454  const ImplicitConstraintSet& constraints (const std::size_t priority)
455  {
456  assert(priority < stacks_.size());
457  return stacks_[priority];
458  }
459 
462  {
463  return constraints_;
464  }
465 
466  std::size_t numberStacks() const
467  {
468  return stacks_.size();
469  }
470 
471  const size_type& dimension () const
472  {
473  return dimension_;
474  }
475 
478  const size_type& reducedDimension () const
479  {
480  return reducedDimension_;
481  }
482 
484  ArrayXb activeParameters () const;
485 
487  ArrayXb activeDerivativeParameters () const;
488 
490 
493  {
494  return squaredNorm_;
495  }
496 
498  void residualError(vectorOut_t error) const;
499 
508  bool definesSubmanifoldOf (const HierarchicalIterative& solver) const;
509 
512 
519  vector_t rightHandSideFromConfig (ConfigurationIn_t config);
520 
527  virtual bool rightHandSideFromConfig (const ImplicitPtr_t& constraint,
528  ConfigurationIn_t config);
534  virtual bool rightHandSide (const ImplicitPtr_t& constraint,
535  vectorIn_t rhs);
536 
538  virtual bool getRightHandSide (const ImplicitPtr_t& constraint,vectorOut_t rhs) const;
539 
544  virtual void rightHandSide (vectorIn_t rhs);
545 
548  void rightHandSideAt (const value_type& s);
549 
554  vector_t rightHandSide () const;
555 
559  size_type rightHandSideSize () const;
560 
562 
566 
568  template <bool ComputeJac> void computeValue (vectorIn_t arg) const;
569  void computeSaturation (vectorIn_t arg) const;
570  void getValue (vectorOut_t v) const;
571  void getReducedJacobian (matrixOut_t J) const;
574  void computeError () const;
575 
577  const vector_t& lastStep () const
578  {
579  return dq_;
580  }
581 
582  virtual bool integrate(vectorIn_t from, vectorIn_t velocity,
583  vectorOut_t result) const;
585 
586  virtual std::ostream& print (std::ostream& os) const;
587 
588  protected:
589  typedef Eigen::JacobiSVD <matrix_t> SVD_t;
590 
591  struct Data {
593  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
597  matrix_t jacobian, reducedJ;
598 
601 
603 
605  std::vector<std::size_t> inequalityIndices;
608  };
609 
612  void update ();
613 
617  virtual void computeActiveRowsOfJ (std::size_t iStack);
618 
626  void computeDescentDirection () const;
627  void expandDqSmall () const;
628  void saturate (vectorOut_t arg) const;
629 
630 
631  value_type squaredErrorThreshold_, inequalityThreshold_;
633 
634  std::vector<ImplicitConstraintSet> stacks_;
644  std::map <DifferentiableFunctionPtr_t, size_type> iq_;
646  std::map <DifferentiableFunctionPtr_t, size_type> iv_;
648  std::map <DifferentiableFunctionPtr_t, std::size_t> priority_;
649 
652 
653  mutable vector_t dq_, dqSmall_;
655  mutable Eigen::VectorXi saturation_, reducedSaturation_;
657  mutable ArrayXb tmpSat_;
659  mutable std::vector<Data> datas_;
660  mutable SVD_t svd_;
661  mutable vector_t OM_;
662  mutable vector_t OP_;
663 
665 
666  protected:
668  private:
669  HPP_SERIALIZABLE_SPLIT();
670  }; // class HierarchicalIterative
672  inline std::ostream& operator<< (std::ostream& os,
673  const HierarchicalIterative& hs)
674  {
675  return hs.print(os);
676  }
677  } // namespace solver
678  } // namespace constraints
679 } // namespace hpp
680 
681 #endif // HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
hpp::constraints::solver::lineSearch::Backtracking::Backtracking
Backtracking()
hpp::constraints::solver::HierarchicalIterative::lastIsOptional_
bool lastIsOptional_
Definition: hierarchical-iterative.hh:637
hpp::constraints::solver::HierarchicalIterative::squaredErrorThreshold
value_type squaredErrorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:422
hpp::constraints::solver::HierarchicalIterative::maxIterations_
size_type maxIterations_
Definition: hierarchical-iterative.hh:632
hpp::constraints::solver::HierarchicalIterative::errorThreshold
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: hierarchical-iterative.hh:412
hpp::constraints::solver::HierarchicalIterative::sigma
const value_type & sigma() const
Definition: hierarchical-iterative.hh:359
hpp::constraints::solver::saturation::Device
Box constraints use a Device joint limits.
Definition: hierarchical-iterative.hh:137
hpp::constraints::solver::lineSearch::ErrorNormBased::operator()
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:105
hpp::constraints::solver::lineSearch::Backtracking::computeLocalSlope
value_type computeLocalSlope(const SolverType &solver) const
Definition: hierarchical-iterative.hh:82
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition: fwd.hh:47
hpp::constraints::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
hpp::constraints::solver::HierarchicalIterative::inequalityThreshold
void inequalityThreshold(const value_type &it)
set the inequality threshold
Definition: hierarchical-iterative.hh:433
hpp::constraints::solver::lineSearch::ErrorNormBased::b
value_type b
Definition: hierarchical-iterative.hh:89
hpp::constraints::solver::HierarchicalIterative::tmpSat_
ArrayXb tmpSat_
Definition: hierarchical-iterative.hh:657
hpp::constraints::solver::HierarchicalIterative::priority_
std::map< DifferentiableFunctionPtr_t, std::size_t > priority_
Priority level of constraint.
Definition: hierarchical-iterative.hh:648
hpp::constraints::solver::lineSearch::Backtracking::darg
vector_t darg
Definition: hierarchical-iterative.hh:59
hpp::constraints::solver::saturation::Device::Device
Device(const DevicePtr_t &device)
Definition: hierarchical-iterative.hh:142
hpp::constraints::solver::lineSearch::ErrorNormBased
Definition: hierarchical-iterative.hh:81
hpp::constraints::solver::HierarchicalIterative::configSpace_
LiegroupSpacePtr_t configSpace_
Definition: hierarchical-iterative.hh:635
implicit-constraint-set.hh
hpp::constraints::solver::HierarchicalIterative::Data::PK
matrix_t PK
Definition: hierarchical-iterative.hh:600
hpp::constraints::solver::HierarchicalIterative::stacks_
std::vector< ImplicitConstraintSet > stacks_
Definition: hierarchical-iterative.hh:634
hpp::constraints::LiegroupSpacePtr_t
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
Eigen::MatrixBlocks< false, true >
hpp::constraints::solver::lineSearch::ErrorNormBased::a
value_type a
Definition: hierarchical-iterative.hh:89
hpp::constraints::solver::lineSearch::FixedSequence::FixedSequence
FixedSequence()
hpp::constraints::solver::lineSearch::FixedSequence::operator()
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:96
hpp::constraints::solver::saturation::Function
saturation from a std::function.
Definition: hierarchical-iterative.hh:118
hpp::constraints::solver::HierarchicalIterative::DefaultLineSearch
lineSearch::FixedSequence DefaultLineSearch
Definition: hierarchical-iterative.hh:222
hpp::constraints::LiegroupElement
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
hpp::constraints::solver::lineSearch::FixedSequence::alpha
value_type alpha
Definition: hierarchical-iterative.hh:72
hpp::constraints::solver::saturation::Base
Base class for box constraints. To prevent configuration variables to get out of joint limits during ...
Definition: hierarchical-iterative.hh:101
matrix-view.hh
hpp::constraints::solver::HierarchicalIterative::Data::inequalityIndices
std::vector< std::size_t > inequalityIndices
Definition: hierarchical-iterative.hh:605
hpp::constraints::solver::HierarchicalIterative::Saturation_t
shared_ptr< saturation::Base > Saturation_t
Definition: hierarchical-iterative.hh:230
hpp::constraints::solver::HierarchicalIterative::print
virtual std::ostream & print(std::ostream &os) const
hpp::constraints::solver::HierarchicalIterative::configSpace
const LiegroupSpacePtr_t & configSpace() const
Get configuration space on which constraints are defined.
Definition: hierarchical-iterative.hh:242
hpp::constraints::matrix_t
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
hpp::constraints::solver::saturation::Bounds::saturate
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
hpp::constraints::solver::HierarchicalIterative::saturation
const Saturation_t & saturation() const
Get the saturation function.
Definition: hierarchical-iterative.hh:275
hpp::constraints::solver::lineSearch::Backtracking::c
value_type c
Definition: hierarchical-iterative.hh:58
hpp::constraints::solver::lineSearch::FixedSequence::K
value_type K
Definition: hierarchical-iterative.hh:73
hpp::constraints::solver::HierarchicalIterative::Indices_t
Eigen::RowBlockIndices Indices_t
Definition: hierarchical-iterative.hh:221
hpp::constraints::solver::lineSearch::Constant
No line search. Use .
Definition: hierarchical-iterative.hh:40
hpp::constraints::solver::HierarchicalIterative::Data
Definition: hierarchical-iterative.hh:591
hpp::constraints::solver::HierarchicalIterative::OM_
vector_t OM_
Definition: hierarchical-iterative.hh:661
hpp::constraints::solver::saturation::Bounds::Bounds
Bounds(const vector_t &lb, const vector_t &ub)
Definition: hierarchical-iterative.hh:133
Eigen::assert
assert(d.lhs()._blocks()==d.rhs()._blocks())
fwd.hh
hpp::constraints::ImplicitConstraintSet
Definition: implicit-constraint-set.hh:34
hpp::constraints::segments_t
std::vector< segment_t > segments_t
Definition: fwd.hh:72
hpp::constraints::solver::HierarchicalIterative::freeVariables
const Indices_t & freeVariables() const
Get free velocity variables.
Definition: hierarchical-iterative.hh:395
hpp::constraints::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:93
hpp::constraints::solver::HierarchicalIterative::Data::svd
SVD_t svd
Definition: hierarchical-iterative.hh:599
hpp::constraints::solver::HierarchicalIterative::freeVariables
void freeVariables(const Indices_t &indices)
Definition: hierarchical-iterative.hh:388
hpp::constraints::solver::lineSearch::FixedSequence::alphaMax
value_type alphaMax
Definition: hierarchical-iterative.hh:73
hpp::constraints::solver::HierarchicalIterative::reducedDimension_
size_type reducedDimension_
Definition: hierarchical-iterative.hh:636
hpp::constraints::solver::HierarchicalIterative::dqSmall_
vector_t dqSmall_
Definition: hierarchical-iterative.hh:653
hpp::constraints::solver::HierarchicalIterative::Data::equalityIndices
Eigen::RowBlockIndices equalityIndices
Definition: hierarchical-iterative.hh:606
hpp::constraints::solver::saturation::Bounds::Bounds
Bounds()
Definition: hierarchical-iterative.hh:132
hpp::constraints::solver::saturation::Function::function
function_t function
Definition: hierarchical-iterative.hh:126
hpp::constraints::solver::HierarchicalIterative::freeVariables
void freeVariables(const segments_t intervals)
Definition: hierarchical-iterative.hh:375
hpp::constraints::solver::HierarchicalIterative::Status
Status
Definition: hierarchical-iterative.hh:224
hpp::constraints::solver::saturation::Base::saturate
virtual bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
hpp::constraints::solver::lineSearch::Backtracking::arg_darg
vector_t arg_darg
Definition: hierarchical-iterative.hh:59
hpp::constraints::solver::HierarchicalIterative::reducedJ_
matrix_t reducedJ_
Definition: hierarchical-iterative.hh:654
hpp::constraints::solver::saturation::Function::Function
Function(const function_t &function)
Definition: hierarchical-iterative.hh:125
hpp::constraints::solver::HierarchicalIterative::reducedDimension
const size_type & reducedDimension() const
Definition: hierarchical-iterative.hh:478
hpp::constraints::solver::lineSearch::Constant::operator()
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:30
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
hpp::constraints::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
hpp::constraints::ComparisonTypes_t
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:176
hpp::constraints::solver::HierarchicalIterative::saturation_
Eigen::VectorXi saturation_
Definition: hierarchical-iterative.hh:655
hpp::constraints::solver::HierarchicalIterative::Data::activeRowsOfJ
Eigen::MatrixBlocks< false, false > activeRowsOfJ
Definition: hierarchical-iterative.hh:607
hpp::constraints::solver::HierarchicalIterative::solve
Status solve(vectorOut_t arg) const
Definition: hierarchical-iterative.hh:316
hpp::constraints::solver::lineSearch::ErrorNormBased::C
value_type C
Definition: hierarchical-iterative.hh:89
hpp
Definition: active-set-differentiable-function.hh:24
hpp::constraints::solver::HierarchicalIterative::lastIsOptional
bool lastIsOptional() const
Definition: hierarchical-iterative.hh:443
hpp::constraints::solver::HierarchicalIterative::isSatisfied
bool isSatisfied(vectorIn_t arg) const
Compares to internal error threshold.
Definition: hierarchical-iterative.hh:324
hpp::constraints::ArrayXb
pinocchio::ArrayXb ArrayXb
Definition: fwd.hh:68
hpp::constraints::solver::HierarchicalIterative::SVD_t
Eigen::JacobiSVD< matrix_t > SVD_t
Definition: hierarchical-iterative.hh:589
hpp::constraints::solver::HierarchicalIterative::constraints
const NumericalConstraints_t & constraints() const
Get constraints (implicit and explicit)
Definition: hierarchical-iterative.hh:461
hpp::constraints::solver::HierarchicalIterative::constraints
const ImplicitConstraintSet & constraints(const std::size_t priority)
Get set of constraints for a give priority level.
Definition: hierarchical-iterative.hh:454
hpp::constraints::solver::HierarchicalIterative::Data::error
vector_t error
Definition: hierarchical-iterative.hh:596
hpp::constraints::solver::lineSearch::Backtracking::operator()
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:37
hpp::constraints::solver::HierarchicalIterative::qSat_
Configuration_t qSat_
Definition: hierarchical-iterative.hh:656
hpp::constraints::size_type
pinocchio::size_type size_type
Definition: fwd.hh:36
hpp::constraints::solver::HierarchicalIterative::isSatisfied
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: hierarchical-iterative.hh:335
hpp::constraints::solver::HierarchicalIterative
Definition: hierarchical-iterative.hh:218
hpp::constraints::solver::HierarchicalIterative::errorThreshold
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:417
hpp::constraints::solver::HierarchicalIterative::Data::reducedJ
matrix_t reducedJ
Definition: hierarchical-iterative.hh:597
hpp::constraints::solver::saturation::Bounds
simple box constraints
Definition: hierarchical-iterative.hh:129
hpp::constraints::solver::lineSearch::Backtracking
Definition: hierarchical-iterative.hh:48
hpp::constraints::solver::HierarchicalIterative::saturate_
Saturation_t saturate_
Definition: hierarchical-iterative.hh:640
hpp::constraints::solver::lineSearch::Backtracking::tau
value_type tau
Definition: hierarchical-iterative.hh:58
hpp::constraints::solver::HierarchicalIterative::~HierarchicalIterative
virtual ~HierarchicalIterative()
Definition: hierarchical-iterative.hh:236
hpp::constraints::solver::HierarchicalIterative::lastIsOptional
void lastIsOptional(bool optional)
Definition: hierarchical-iterative.hh:438
hpp::constraints::solver::HierarchicalIterative::iq_
std::map< DifferentiableFunctionPtr_t, size_type > iq_
Value rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:644
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
hpp::constraints::value_type
pinocchio::value_type value_type
Definition: fwd.hh:37
hpp::constraints::solver::lineSearch::ErrorNormBased::K
value_type K
Definition: hierarchical-iterative.hh:89
hpp::constraints::solver::saturation::Device::Device
Device()
Definition: hierarchical-iterative.hh:141
hpp::constraints::solver::HierarchicalIterative::squaredNorm_
value_type squaredNorm_
Definition: hierarchical-iterative.hh:658
hpp::constraints::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
hpp::constraints::solver::HierarchicalIterative::sigma_
value_type sigma_
The smallest non-zero singular value.
Definition: hierarchical-iterative.hh:651
hpp::constraints::solver::HierarchicalIterative::datas_
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:659
hpp::constraints::solver::HierarchicalIterative::maxIterations
void maxIterations(size_type iterations)
Set maximal number of iterations.
Definition: hierarchical-iterative.hh:401
hpp::constraints::NumericalConstraints_t
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:165
hpp::constraints::solver::HierarchicalIterative::saturation
void saturation(const Saturation_t &saturate)
Set the saturation function.
Definition: hierarchical-iterative.hh:269
hpp::constraints::solver::HierarchicalIterative::HierarchicalIterative
HierarchicalIterative()
Definition: hierarchical-iterative.hh:667
hpp::constraints::solver::HierarchicalIterative::inequalityThreshold
value_type inequalityThreshold() const
Get the inequality threshold.
Definition: hierarchical-iterative.hh:428
hpp::constraints::solver::saturation::Function::saturate
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
Definition: hierarchical-iterative.hh:120
hpp::constraints::solver::lineSearch::Backtracking::df
vector_t df
Definition: hierarchical-iterative.hh:59
hpp::constraints::solver::operator<<
std::ostream & operator<<(std::ostream &os, const HierarchicalIterative &hs)
Definition: hierarchical-iterative.hh:672
hpp::constraints::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
hpp::constraints::solver::saturation::Device::saturate
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
hpp::constraints::solver::lineSearch::Backtracking::smallAlpha
value_type smallAlpha
Definition: hierarchical-iterative.hh:58
hpp::constraints::solver::HierarchicalIterative::residualError
value_type residualError() const
Returns the squared norm of the error vector.
Definition: hierarchical-iterative.hh:492
hpp::constraints::solver::HierarchicalIterative::freeVariables_
Indices_t freeVariables_
Unknown of the set of implicit constraints.
Definition: hierarchical-iterative.hh:639
hpp::constraints::solver::HierarchicalIterative::numberStacks
std::size_t numberStacks() const
Definition: hierarchical-iterative.hh:466
hpp::constraints::solver::HierarchicalIterative::svd_
SVD_t svd_
Definition: hierarchical-iterative.hh:660
hpp::constraints::solver::HierarchicalIterative::MAX_ITERATION_REACHED
@ MAX_ITERATION_REACHED
Definition: hierarchical-iterative.hh:226
hpp::constraints::solver::saturation::Function::function_t
std::function< bool(vectorIn_t, vectorOut_t, Eigen::VectorXi &)> function_t
Definition: hierarchical-iterative.hh:119
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
hpp::constraints::solver::HierarchicalIterative::OP_
vector_t OP_
Definition: hierarchical-iterative.hh:662
hpp::constraints::solver::HierarchicalIterative::iv_
std::map< DifferentiableFunctionPtr_t, size_type > iv_
Derivative rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:646
hpp::constraints::solver::HierarchicalIterative::dimension
const size_type & dimension() const
Definition: hierarchical-iterative.hh:471
hpp::constraints::solver::HierarchicalIterative::lastStep
const vector_t & lastStep() const
Accessor to the last step done.
Definition: hierarchical-iterative.hh:577
hpp::constraints::solver::HierarchicalIterative::ERROR_INCREASED
@ ERROR_INCREASED
Definition: hierarchical-iterative.hh:225
hpp::constraints::solver::HierarchicalIterative::INFEASIBLE
@ INFEASIBLE
Definition: hierarchical-iterative.hh:227
hpp::constraints::solver::saturation::Bounds::lb
vector_t lb
Definition: hierarchical-iterative.hh:134
hpp::constraints::solver::HierarchicalIterative::Data::maxRank
size_type maxRank
Definition: hierarchical-iterative.hh:602
hpp::constraints::solver::HierarchicalIterative::Data::rightHandSide
LiegroupElement rightHandSide
Definition: hierarchical-iterative.hh:595
hpp::constraints::solver::saturation::Bounds::ub
vector_t ub
Definition: hierarchical-iterative.hh:134
hpp::constraints::solver::HierarchicalIterative::squaredErrorThreshold_
value_type squaredErrorThreshold_
Definition: hierarchical-iterative.hh:631
hpp::constraints::solver::saturation::Base::~Base
virtual ~Base()
Definition: hierarchical-iterative.hh:115
hpp::constraints::solver::HierarchicalIterative::Data::comparison
ComparisonTypes_t comparison
Definition: hierarchical-iterative.hh:604
config.hh
hpp::constraints::solver::HierarchicalIterative::maxIterations
size_type maxIterations() const
Get maximal number of iterations in config projector.
Definition: hierarchical-iterative.hh:406
hpp::constraints::solver::saturation::Device::device
DevicePtr_t device
Definition: hierarchical-iterative.hh:143
hpp::constraints::solver::saturation::Function::Function
Function()
Definition: hierarchical-iterative.hh:124
hpp::constraints::solver::lineSearch::ErrorNormBased::ErrorNormBased
ErrorNormBased(value_type alphaMin, value_type _a, value_type _b)
hpp::constraints::solver::HierarchicalIterative::constraints_
NumericalConstraints_t constraints_
Members moved from core::ConfigProjector.
Definition: hierarchical-iterative.hh:642
hpp::constraints::solver::lineSearch::FixedSequence
Definition: hierarchical-iterative.hh:65