hpp-constraints  4.14.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 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28 
29 #ifndef HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
30 #define HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
31 
32 #include <functional>
34 #include <hpp/constraints/fwd.hh>
37 #include <hpp/util/serialization-fwd.hh>
38 #include <map>
39 
40 namespace hpp {
41 namespace constraints {
42 namespace solver {
45 namespace lineSearch {
47 struct Constant {
48  template <typename SolverType>
49  bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
50 };
51 
54 struct Backtracking {
55  Backtracking();
56 
57  template <typename SolverType>
58  bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
59 
60  template <typename SolverType>
61  inline value_type computeLocalSlope(const SolverType& solver) const;
62 
63  value_type c, tau, smallAlpha; // 0.8 ^ 7 = 0.209, 0.8 ^ 8 = 0.1677
64  mutable vector_t arg_darg, df, darg;
65 };
66 
70 struct FixedSequence {
71  FixedSequence();
72 
73  template <typename SolverType>
74  bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
75 
78 };
79 
87  ErrorNormBased(value_type alphaMin = 0.2);
88 
89  template <typename SolverType>
90  bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
91 
93 };
94 } // namespace lineSearch
95 
96 namespace saturation {
104 struct Base {
116  virtual bool saturate(vectorIn_t q, vectorOut_t qSat,
117  Eigen::VectorXi& saturation);
118  virtual ~Base() {}
119 };
121 struct Function : Base {
122  typedef std::function<bool(vectorIn_t, vectorOut_t, Eigen::VectorXi&)>
124  bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation) {
125  return function(q, qSat, saturation);
126  }
127  Function() {}
128  Function(const function_t& function) : function(function) {}
129  function_t function;
130 };
132 struct Bounds : Base {
133  bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation);
134  Bounds() {}
135  Bounds(const vector_t& lb, const vector_t& ub) : lb(lb), ub(ub) {}
137 };
139 struct Device : Base {
141  bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation);
142  Device() {}
145 };
146 } // namespace saturation
147 
233  public:
236 
237  enum Status { ERROR_INCREASED, MAX_ITERATION_REACHED, INFEASIBLE, SUCCESS };
238  typedef shared_ptr<saturation::Base> Saturation_t;
239 
240  HierarchicalIterative(const LiegroupSpacePtr_t& configSpace);
241 
243 
245 
248 
250  const LiegroupSpacePtr_t& configSpace() const { return configSpace_; }
254  virtual bool contains(const ImplicitPtr_t& numericalConstraint) const;
255 
261  virtual bool add(const ImplicitPtr_t& constraint,
262  const std::size_t& priority);
263 
269  virtual void merge(const HierarchicalIterative& other);
270 
272  void saturation(const Saturation_t& saturate) { saturate_ = saturate; }
273 
275  const Saturation_t& saturation() const { return saturate_; }
276 
278 
281 
296  template <typename LineSearchType>
297  Status solve(vectorOut_t arg, LineSearchType ls = LineSearchType()) const;
298 
313  inline Status solve(vectorOut_t arg) const {
314  return solve(arg, DefaultLineSearch());
315  }
316 
320  bool isSatisfied(vectorIn_t arg) const {
321  computeValue<false>(arg);
322  computeError();
323  return squaredNorm_ < squaredErrorThreshold_;
324  }
325 
330  bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const {
331  computeValue<false>(arg);
332  computeError();
333  return squaredNorm_ < errorThreshold * errorThreshold;
334  }
335 
345  bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
346  vectorOut_t error, bool& constraintFound) const;
347 
352  const value_type& sigma() const { return sigma_; }
353 
355 
358 
365  void freeVariables(const segments_t intervals) {
366  freeVariables_ = Indices_t();
367  for (std::size_t i = 0; i < intervals.size(); ++i)
368  freeVariables_.addRow(intervals[i].first, intervals[i].second);
369  freeVariables_.updateIndices<true, true, true>();
370  update();
371  }
372 
377  void freeVariables(const Indices_t& indices) {
378  freeVariables_ = indices;
379  update();
380  }
381 
383  const Indices_t& freeVariables() const { return freeVariables_; }
384 
386  void maxIterations(size_type iterations) { maxIterations_ = iterations; }
388  size_type maxIterations() const { return maxIterations_; }
389 
391  void errorThreshold(const value_type& threshold) {
392  squaredErrorThreshold_ = threshold * threshold;
393  }
395  value_type errorThreshold() const { return sqrt(squaredErrorThreshold_); }
397  value_type squaredErrorThreshold() const { return squaredErrorThreshold_; }
398 
400  value_type inequalityThreshold() const { return inequalityThreshold_; }
402  void inequalityThreshold(const value_type& it) { inequalityThreshold_ = it; }
403 
404  void lastIsOptional(bool optional) { lastIsOptional_ = optional; }
405 
406  bool lastIsOptional() const { return lastIsOptional_; }
407 
408  // Set whether the constraints are solved level by level
409  //
410  // If true, the solver will consider only the higher level of
411  // priority that is not satisfied when computing the direction
412  // of descent.
413  void solveLevelByLevel(bool solveLevelByLevel) {
414  solveLevelByLevel_ = solveLevelByLevel;
415  }
416 
417  // Get whether the constraints are solved level by level
418  bool solveLevelByLevel() const { return solveLevelByLevel_; }
420 
423 
425  const ImplicitConstraintSet& constraints(const std::size_t priority) {
426  assert(priority < stacks_.size());
427  return stacks_[priority];
428  }
429 
431  const NumericalConstraints_t& constraints() const { return constraints_; }
432 
433  std::size_t numberStacks() const { return stacks_.size(); }
434 
435  const size_type& dimension() const { return dimension_; }
436 
439  const size_type& reducedDimension() const { return reducedDimension_; }
440 
442  ArrayXb activeParameters() const;
443 
445  ArrayXb activeDerivativeParameters() const;
446 
448 
450  value_type residualError() const { return squaredNorm_; }
451 
453  void residualError(vectorOut_t error) const;
454 
463  bool definesSubmanifoldOf(const HierarchicalIterative& solver) const;
464 
467 
474  vector_t rightHandSideFromConfig(ConfigurationIn_t config);
475 
482  virtual bool rightHandSideFromConfig(const ImplicitPtr_t& constraint,
483  ConfigurationIn_t config);
489  virtual bool rightHandSide(const ImplicitPtr_t& constraint, vectorIn_t rhs);
490 
492  virtual bool getRightHandSide(const ImplicitPtr_t& constraint,
493  vectorOut_t rhs) const;
494 
499  virtual void rightHandSide(vectorIn_t rhs);
500 
503  void rightHandSideAt(const value_type& s);
504 
509  vector_t rightHandSide() const;
510 
514  size_type rightHandSideSize() const;
515 
517 
521 
523  template <bool ComputeJac>
524  void computeValue(vectorIn_t arg) const;
525  void computeSaturation(vectorIn_t arg) const;
526  void getValue(vectorOut_t v) const;
527  void getReducedJacobian(matrixOut_t J) const;
530  void computeError() const;
531 
533  const vector_t& lastStep() const { return dq_; }
534 
535  virtual bool integrate(vectorIn_t from, vectorIn_t velocity,
536  vectorOut_t result) const;
538 
539  virtual std::ostream& print(std::ostream& os) const;
540 
541  protected:
542  typedef Eigen::JacobiSVD<matrix_t> SVD_t;
543 
544  struct Data {
546  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
550  matrix_t jacobian, reducedJ;
551 
554 
556 
558  std::vector<std::size_t> inequalityIndices;
561  };
562 
565  void update();
566 
570  virtual void computeActiveRowsOfJ(std::size_t iStack);
571 
579  void computeDescentDirection() const;
580  void expandDqSmall() const;
581  void saturate(vectorOut_t arg) const;
582 
583  value_type squaredErrorThreshold_, inequalityThreshold_;
585 
586  std::vector<ImplicitConstraintSet> stacks_;
597  std::map<DifferentiableFunctionPtr_t, size_type> iq_;
599  std::map<DifferentiableFunctionPtr_t, size_type> iv_;
601  std::map<DifferentiableFunctionPtr_t, std::size_t> priority_;
602 
605 
606  mutable vector_t dq_, dqSmall_;
608  mutable Eigen::VectorXi saturation_, reducedSaturation_;
610  mutable ArrayXb tmpSat_;
612  mutable std::vector<Data> datas_;
613  mutable SVD_t svd_;
614  mutable vector_t OM_;
615  mutable vector_t OP_;
616 
618 
619  protected:
621 
622  private:
623  HPP_SERIALIZABLE_SPLIT();
624 }; // class HierarchicalIterative
626 inline std::ostream& operator<<(std::ostream& os,
627  const HierarchicalIterative& hs) {
628  return hs.print(os);
629 }
630 } // namespace solver
631 } // namespace constraints
632 } // namespace hpp
633 
634 #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:589
hpp::constraints::solver::HierarchicalIterative::solveLevelByLevel
bool solveLevelByLevel() const
Definition: hierarchical-iterative.hh:418
hpp::constraints::solver::HierarchicalIterative::squaredErrorThreshold
value_type squaredErrorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:397
hpp::constraints::solver::HierarchicalIterative::maxIterations_
size_type maxIterations_
Definition: hierarchical-iterative.hh:584
hpp::constraints::solver::HierarchicalIterative::errorThreshold
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: hierarchical-iterative.hh:391
hpp::constraints::solver::HierarchicalIterative::sigma
const value_type & sigma() const
Definition: hierarchical-iterative.hh:352
hpp::constraints::solver::saturation::Device
Box constraints use a Device joint limits.
Definition: hierarchical-iterative.hh:139
hpp::constraints::solver::lineSearch::ErrorNormBased::operator()
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:118
hpp::constraints::solver::lineSearch::Backtracking::computeLocalSlope
value_type computeLocalSlope(const SolverType &solver) const
Definition: hierarchical-iterative.hh:94
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition: fwd.hh:59
hpp::constraints::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:109
hpp::constraints::solver::HierarchicalIterative::inequalityThreshold
void inequalityThreshold(const value_type &it)
set the inequality threshold
Definition: hierarchical-iterative.hh:402
hpp::constraints::solver::lineSearch::ErrorNormBased::b
value_type b
Definition: hierarchical-iterative.hh:92
hpp::constraints::solver::HierarchicalIterative::tmpSat_
ArrayXb tmpSat_
Definition: hierarchical-iterative.hh:610
hpp::constraints::solver::HierarchicalIterative::priority_
std::map< DifferentiableFunctionPtr_t, std::size_t > priority_
Priority level of constraint.
Definition: hierarchical-iterative.hh:601
hpp::constraints::solver::lineSearch::Backtracking::darg
vector_t darg
Definition: hierarchical-iterative.hh:64
hpp::constraints::solver::saturation::Device::Device
Device(const DevicePtr_t &device)
Definition: hierarchical-iterative.hh:143
hpp::constraints::solver::lineSearch::ErrorNormBased
Definition: hierarchical-iterative.hh:85
hpp::constraints::solver::HierarchicalIterative::configSpace_
LiegroupSpacePtr_t configSpace_
Definition: hierarchical-iterative.hh:587
implicit-constraint-set.hh
hpp::constraints::solver::HierarchicalIterative::Data::PK
matrix_t PK
Definition: hierarchical-iterative.hh:553
hpp::constraints::solver::HierarchicalIterative::stacks_
std::vector< ImplicitConstraintSet > stacks_
Definition: hierarchical-iterative.hh:586
hpp::constraints::LiegroupSpacePtr_t
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:69
Eigen::MatrixBlocks< false, true >
hpp::constraints::solver::lineSearch::ErrorNormBased::a
value_type a
Definition: hierarchical-iterative.hh:92
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:109
hpp::constraints::solver::saturation::Function
saturation from a std::function.
Definition: hierarchical-iterative.hh:121
hpp::constraints::solver::HierarchicalIterative::DefaultLineSearch
lineSearch::FixedSequence DefaultLineSearch
Definition: hierarchical-iterative.hh:235
hpp::constraints::LiegroupElement
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:65
hpp::constraints::solver::lineSearch::FixedSequence::alpha
value_type alpha
Definition: hierarchical-iterative.hh:76
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:104
matrix-view.hh
hpp::constraints::solver::HierarchicalIterative::Data::inequalityIndices
std::vector< std::size_t > inequalityIndices
Definition: hierarchical-iterative.hh:558
hpp::constraints::solver::HierarchicalIterative::Saturation_t
shared_ptr< saturation::Base > Saturation_t
Definition: hierarchical-iterative.hh:238
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:250
hpp::constraints::matrix_t
pinocchio::matrix_t matrix_t
Definition: fwd.hh:56
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:63
hpp::constraints::solver::lineSearch::FixedSequence::K
value_type K
Definition: hierarchical-iterative.hh:77
hpp::constraints::solver::HierarchicalIterative::Indices_t
Eigen::RowBlockIndices Indices_t
Definition: hierarchical-iterative.hh:234
hpp::constraints::solver::lineSearch::Constant
No line search. Use .
Definition: hierarchical-iterative.hh:47
hpp::constraints::solver::HierarchicalIterative::Data
Definition: hierarchical-iterative.hh:544
hpp::constraints::solver::HierarchicalIterative::OM_
vector_t OM_
Definition: hierarchical-iterative.hh:614
hpp::constraints::solver::saturation::Bounds::Bounds
Bounds(const vector_t &lb, const vector_t &ub)
Definition: hierarchical-iterative.hh:135
Eigen::assert
assert(d.lhs()._blocks()==d.rhs()._blocks())
fwd.hh
hpp::constraints::ImplicitConstraintSet
Definition: implicit-constraint-set.hh:45
hpp::constraints::solver::HierarchicalIterative::freeVariables
const Indices_t & freeVariables() const
Get free velocity variables.
Definition: hierarchical-iterative.hh:383
hpp::constraints::solver::saturation::Function::function_t
std::function< bool(vectorIn_t, vectorOut_t, Eigen::VectorXi &)> function_t
Definition: hierarchical-iterative.hh:123
hpp::constraints::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:105
hpp::constraints::solver::HierarchicalIterative::Data::svd
SVD_t svd
Definition: hierarchical-iterative.hh:552
hpp::constraints::solver::HierarchicalIterative::freeVariables
void freeVariables(const Indices_t &indices)
Definition: hierarchical-iterative.hh:377
hpp::constraints::NumericalConstraints_t
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:177
hpp::constraints::solver::lineSearch::FixedSequence::alphaMax
value_type alphaMax
Definition: hierarchical-iterative.hh:77
hpp::constraints::solver::HierarchicalIterative::reducedDimension_
size_type reducedDimension_
Definition: hierarchical-iterative.hh:588
hpp::constraints::solver::HierarchicalIterative::dqSmall_
vector_t dqSmall_
Definition: hierarchical-iterative.hh:606
hpp::constraints::solver::HierarchicalIterative::Data::equalityIndices
Eigen::RowBlockIndices equalityIndices
Definition: hierarchical-iterative.hh:559
hpp::constraints::solver::saturation::Bounds::Bounds
Bounds()
Definition: hierarchical-iterative.hh:134
hpp::constraints::solver::saturation::Function::function
function_t function
Definition: hierarchical-iterative.hh:129
hpp::constraints::solver::HierarchicalIterative::freeVariables
void freeVariables(const segments_t intervals)
Definition: hierarchical-iterative.hh:365
hpp::constraints::solver::HierarchicalIterative::Status
Status
Definition: hierarchical-iterative.hh:237
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:64
hpp::constraints::solver::HierarchicalIterative::reducedJ_
matrix_t reducedJ_
Definition: hierarchical-iterative.hh:607
hpp::constraints::solver::saturation::Function::Function
Function(const function_t &function)
Definition: hierarchical-iterative.hh:128
hpp::constraints::solver::HierarchicalIterative::reducedDimension
const size_type & reducedDimension() const
Definition: hierarchical-iterative.hh:439
hpp::constraints::solver::lineSearch::Constant::operator()
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:41
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
hpp::constraints::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:58
hpp::constraints::ComparisonTypes_t
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:182
hpp::constraints::solver::HierarchicalIterative::saturation_
Eigen::VectorXi saturation_
Definition: hierarchical-iterative.hh:608
hpp::constraints::solver::HierarchicalIterative::solveLevelByLevel
void solveLevelByLevel(bool solveLevelByLevel)
Definition: hierarchical-iterative.hh:413
hpp::constraints::solver::HierarchicalIterative::solve
Status solve(vectorOut_t arg) const
Definition: hierarchical-iterative.hh:313
hpp::constraints::solver::lineSearch::ErrorNormBased::C
value_type C
Definition: hierarchical-iterative.hh:92
hpp
Definition: active-set-differentiable-function.hh:36
hpp::constraints::solver::HierarchicalIterative::lastIsOptional
bool lastIsOptional() const
Definition: hierarchical-iterative.hh:406
hpp::constraints::solver::HierarchicalIterative::isSatisfied
bool isSatisfied(vectorIn_t arg) const
Definition: hierarchical-iterative.hh:320
hpp::constraints::ArrayXb
pinocchio::ArrayXb ArrayXb
Definition: fwd.hh:80
hpp::constraints::solver::HierarchicalIterative::SVD_t
Eigen::JacobiSVD< matrix_t > SVD_t
Definition: hierarchical-iterative.hh:542
hpp::constraints::solver::HierarchicalIterative::constraints
const NumericalConstraints_t & constraints() const
Get constraints (implicit and explicit)
Definition: hierarchical-iterative.hh:431
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:425
hpp::constraints::solver::HierarchicalIterative::Data::error
vector_t error
Definition: hierarchical-iterative.hh:549
hpp::constraints::solver::lineSearch::Backtracking::operator()
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:48
hpp::constraints::solver::HierarchicalIterative::qSat_
Configuration_t qSat_
Definition: hierarchical-iterative.hh:609
hpp::constraints::size_type
pinocchio::size_type size_type
Definition: fwd.hh:47
hpp::constraints::solver::HierarchicalIterative::isSatisfied
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: hierarchical-iterative.hh:330
hpp::constraints::solver::HierarchicalIterative
Definition: hierarchical-iterative.hh:232
hpp::constraints::solver::HierarchicalIterative::errorThreshold
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:395
hpp::constraints::solver::HierarchicalIterative::Data::reducedJ
matrix_t reducedJ
Definition: hierarchical-iterative.hh:550
hpp::constraints::solver::saturation::Bounds
simple box constraints
Definition: hierarchical-iterative.hh:132
hpp::constraints::solver::lineSearch::Backtracking
Definition: hierarchical-iterative.hh:54
hpp::constraints::solver::HierarchicalIterative::saturate_
Saturation_t saturate_
Definition: hierarchical-iterative.hh:593
hpp::constraints::solver::lineSearch::Backtracking::tau
value_type tau
Definition: hierarchical-iterative.hh:63
hpp::constraints::solver::HierarchicalIterative::~HierarchicalIterative
virtual ~HierarchicalIterative()
Definition: hierarchical-iterative.hh:244
hpp::constraints::solver::HierarchicalIterative::lastIsOptional
void lastIsOptional(bool optional)
Definition: hierarchical-iterative.hh:404
hpp::constraints::solver::HierarchicalIterative::iq_
std::map< DifferentiableFunctionPtr_t, size_type > iq_
Value rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:597
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:175
hpp::constraints::value_type
pinocchio::value_type value_type
Definition: fwd.hh:48
hpp::constraints::solver::lineSearch::ErrorNormBased::K
value_type K
Definition: hierarchical-iterative.hh:92
hpp::constraints::solver::saturation::Device::Device
Device()
Definition: hierarchical-iterative.hh:142
hpp::constraints::solver::HierarchicalIterative::squaredNorm_
value_type squaredNorm_
Definition: hierarchical-iterative.hh:611
hpp::constraints::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
hpp::constraints::solver::HierarchicalIterative::sigma_
value_type sigma_
The smallest non-zero singular value.
Definition: hierarchical-iterative.hh:604
hpp::constraints::solver::HierarchicalIterative::datas_
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:612
hpp::constraints::solver::HierarchicalIterative::maxIterations
void maxIterations(size_type iterations)
Set maximal number of iterations.
Definition: hierarchical-iterative.hh:386
hpp::constraints::solver::HierarchicalIterative::saturation
void saturation(const Saturation_t &saturate)
Set the saturation function.
Definition: hierarchical-iterative.hh:272
hpp::constraints::solver::HierarchicalIterative::HierarchicalIterative
HierarchicalIterative()
Definition: hierarchical-iterative.hh:620
hpp::constraints::solver::HierarchicalIterative::inequalityThreshold
value_type inequalityThreshold() const
Get the inequality threshold.
Definition: hierarchical-iterative.hh:400
hpp::constraints::solver::saturation::Function::saturate
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
Definition: hierarchical-iterative.hh:124
hpp::constraints::solver::lineSearch::Backtracking::df
vector_t df
Definition: hierarchical-iterative.hh:64
hpp::constraints::solver::operator<<
std::ostream & operator<<(std::ostream &os, const HierarchicalIterative &hs)
Definition: hierarchical-iterative.hh:626
hpp::constraints::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
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:63
hpp::constraints::solver::HierarchicalIterative::residualError
value_type residualError() const
Returns the squared norm of the error vector.
Definition: hierarchical-iterative.hh:450
hpp::constraints::solver::HierarchicalIterative::freeVariables_
Indices_t freeVariables_
Unknown of the set of implicit constraints.
Definition: hierarchical-iterative.hh:592
hpp::constraints::solver::HierarchicalIterative::numberStacks
std::size_t numberStacks() const
Definition: hierarchical-iterative.hh:433
hpp::constraints::solver::HierarchicalIterative::Data::activeRowsOfJ
Eigen::MatrixBlocks< false, false > activeRowsOfJ
Definition: hierarchical-iterative.hh:560
hpp::constraints::solver::HierarchicalIterative::svd_
SVD_t svd_
Definition: hierarchical-iterative.hh:613
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
hpp::constraints::solver::HierarchicalIterative::OP_
vector_t OP_
Definition: hierarchical-iterative.hh:615
hpp::constraints::solver::HierarchicalIterative::iv_
std::map< DifferentiableFunctionPtr_t, size_type > iv_
Derivative rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:599
hpp::constraints::solver::HierarchicalIterative::dimension
const size_type & dimension() const
Definition: hierarchical-iterative.hh:435
hpp::constraints::solver::HierarchicalIterative::lastStep
const vector_t & lastStep() const
Accessor to the last step done.
Definition: hierarchical-iterative.hh:533
hpp::constraints::solver::saturation::Bounds::lb
vector_t lb
Definition: hierarchical-iterative.hh:136
hpp::constraints::segments_t
std::vector< segment_t > segments_t
Definition: fwd.hh:84
hpp::constraints::solver::HierarchicalIterative::Data::maxRank
size_type maxRank
Definition: hierarchical-iterative.hh:555
hpp::constraints::solver::HierarchicalIterative::Data::rightHandSide
LiegroupElement rightHandSide
Definition: hierarchical-iterative.hh:548
hpp::constraints::solver::saturation::Bounds::ub
vector_t ub
Definition: hierarchical-iterative.hh:136
hpp::constraints::solver::HierarchicalIterative::squaredErrorThreshold_
value_type squaredErrorThreshold_
Definition: hierarchical-iterative.hh:583
hpp::constraints::solver::saturation::Base::~Base
virtual ~Base()
Definition: hierarchical-iterative.hh:118
hpp::constraints::solver::HierarchicalIterative::Data::comparison
ComparisonTypes_t comparison
Definition: hierarchical-iterative.hh:557
config.hh
hpp::constraints::solver::HierarchicalIterative::maxIterations
size_type maxIterations() const
Get maximal number of iterations in config projector.
Definition: hierarchical-iterative.hh:388
hpp::constraints::solver::saturation::Device::device
DevicePtr_t device
Definition: hierarchical-iterative.hh:144
hpp::constraints::solver::saturation::Function::Function
Function()
Definition: hierarchical-iterative.hh:127
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:595
hpp::constraints::solver::lineSearch::FixedSequence
Definition: hierarchical-iterative.hh:70
hpp::constraints::solver::HierarchicalIterative::solveLevelByLevel_
bool solveLevelByLevel_
Definition: hierarchical-iterative.hh:590