hpp-constraints: include/hpp/constraints/solver/hierarchical-iterative.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
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>
33#include <hpp/constraints/config.hh>
34#include <hpp/constraints/fwd.hh>
35#include <hpp/constraints/implicit-constraint-set.hh>
36#include <hpp/constraints/matrix-view.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
76 value_type alpha;
77 value_type alphaMax, K;
78};
79
85 struct ErrorNormBased {
86 ErrorNormBased(value_type alphaMin, value_type _a, value_type _b);
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
92 value_type C, K, a, b;
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&)>
123 function_t;
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), iq2iv_() {
136 iq2iv_.resize(ub.size());
137 for (size_type i = 0; i < ub.size(); ++i) {
138 iq2iv_[i] = (Eigen::VectorXi::Scalar)i;
139 }
140 }
141 Bounds(const vector_t& lb, const vector_t& ub, const Eigen::VectorXi& iq2iv)
142 : lb(lb), ub(ub), iq2iv_(iq2iv) {}
143 vector_t lb, ub;
144 Eigen::VectorXi iq2iv_;
145};
147 struct Device : Base {
149 bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation);
150 Device() {}
151 Device(const DevicePtr_t& device) : device(device) {}
152 DevicePtr_t device;
153};
154} // namespace saturation
155
240 class HPP_CONSTRAINTS_DLLAPI HierarchicalIterative {
241 public:
242 typedef Eigen::RowBlockIndices Indices_t;
243 typedef lineSearch::FixedSequence DefaultLineSearch;
244
245 enum Status { ERROR_INCREASED, MAX_ITERATION_REACHED, INFEASIBLE, SUCCESS };
246 typedef shared_ptr<saturation::Base> Saturation_t;
247
248 HierarchicalIterative(const LiegroupSpacePtr_t& configSpace);
249
250 HierarchicalIterative(const HierarchicalIterative& other);
251
252 virtual ~HierarchicalIterative() {}
253
256
258 const LiegroupSpacePtr_t& configSpace() const { return configSpace_; }
262 virtual bool contains(const ImplicitPtr_t& numericalConstraint) const;
263
269 virtual bool add(const ImplicitPtr_t& constraint,
270 const std::size_t& priority);
271
277 virtual void merge(const HierarchicalIterative& other);
278
280 void saturation(const Saturation_t& saturate) { saturate_ = saturate; }
281
283 const Saturation_t& saturation() const { return saturate_; }
284
293 HierarchicalIterative extract(interval_t interval) const;
294
296
299
314 template <typename LineSearchType>
315 Status solve(vectorOut_t arg, LineSearchType ls = LineSearchType()) const;
316
331 inline Status solve(vectorOut_t arg) const {
332 return solve(arg, DefaultLineSearch());
333 }
334
338 bool isSatisfied(vectorIn_t arg) const {
339 computeValue<false>(arg);
340 computeError();
341 return squaredNorm_ < squaredErrorThreshold_;
342 }
343
348 bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const {
349 computeValue<false>(arg);
350 computeError();
351 return squaredNorm_ < errorThreshold * errorThreshold;
352 }
353
363 bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
364 vectorOut_t error, bool& constraintFound) const;
365
370 const value_type& sigma() const { return sigma_; }
371
373
376
383 void freeVariables(const segments_t intervals) {
384 freeVariables_ = Indices_t();
385 for (std::size_t i = 0; i < intervals.size(); ++i)
386 freeVariables_.addRow(intervals[i].first, intervals[i].second);
387 freeVariables_.updateIndices<true, true, true>();
388 update();
389 }
390
395 void freeVariables(const Indices_t& indices) {
396 freeVariables_ = indices;
397 update();
398 }
399
401 const Indices_t& freeVariables() const { return freeVariables_; }
402
404 void maxIterations(size_type iterations) { maxIterations_ = iterations; }
406 size_type maxIterations() const { return maxIterations_; }
407
409 void errorThreshold(const value_type& threshold) {
410 squaredErrorThreshold_ = threshold * threshold;
411 }
413 value_type errorThreshold() const { return sqrt(squaredErrorThreshold_); }
415 value_type squaredErrorThreshold() const { return squaredErrorThreshold_; }
416
418 value_type inequalityThreshold() const { return inequalityThreshold_; }
420 void inequalityThreshold(const value_type& it) { inequalityThreshold_ = it; }
421
422 void lastIsOptional(bool optional) { lastIsOptional_ = optional; }
423
424 bool lastIsOptional() const { return lastIsOptional_; }
425
426 // Set whether the constraints are solved level by level
427 //
428 // If true, the solver will consider only the higher level of
429 // priority that is not satisfied when computing the direction
430 // of descent.
431 void solveLevelByLevel(bool solveLevelByLevel) {
432 solveLevelByLevel_ = solveLevelByLevel;
433 }
434
435 // Get whether the constraints are solved level by level
436 bool solveLevelByLevel() const { return solveLevelByLevel_; }
438
441
443 const ImplicitConstraintSet& constraints(const std::size_t priority) {
444 assert(priority < stacks_.size());
445 return stacks_[priority];
446 }
447
449 const NumericalConstraints_t& constraints() const { return constraints_; }
450
451 std::size_t numberStacks() const { return stacks_.size(); }
452
453 const size_type& dimension() const { return dimension_; }
454
457 const size_type& reducedDimension() const { return reducedDimension_; }
458
460 ArrayXb activeParameters() const;
461
463 ArrayXb activeDerivativeParameters() const;
464
466
468 value_type residualError() const { return squaredNorm_; }
469
471 void residualError(vectorOut_t error) const;
472
481 bool definesSubmanifoldOf(const HierarchicalIterative& solver) const;
482
485
492 vector_t rightHandSideFromConfig(ConfigurationIn_t config);
493
500 virtual bool rightHandSideFromConfig(const ImplicitPtr_t& constraint,
501 ConfigurationIn_t config);
507 virtual bool rightHandSide(const ImplicitPtr_t& constraint, vectorIn_t rhs);
508
510 virtual bool getRightHandSide(const ImplicitPtr_t& constraint,
511 vectorOut_t rhs) const;
512
517 virtual void rightHandSide(vectorIn_t rhs);
518
521 void rightHandSideAt(const value_type& s);
522
527 vector_t rightHandSide() const;
528
532 size_type rightHandSideSize() const;
533
535
539
541 template <bool ComputeJac>
542 void computeValue(vectorIn_t arg) const;
543 void computeSaturation(vectorIn_t arg) const;
544 void getValue(vectorOut_t v) const;
545 void getReducedJacobian(matrixOut_t J) const;
548 void computeError() const;
549
551 const vector_t& lastStep() const { return dq_; }
552
553 virtual bool integrate(vectorIn_t from, vectorIn_t velocity,
554 vectorOut_t result) const;
556
557 virtual std::ostream& print(std::ostream& os) const;
558
559 protected:
560 typedef Eigen::JacobiSVD<matrix_t> SVD_t;
561
562 struct Data {
563 Data()
564 : output(),
565 rightHandSide(),
566 error(),
567 jacobian(),
568 reducedJ(),
569 svd(),
570 PK(),
571 comparison(),
572 inequalityIndices(),
573 equalityIndices(),
574 activeRowsOfJ() {}
576 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
578 LiegroupElement output, rightHandSide;
579 vector_t error;
580 matrix_t jacobian, reducedJ;
581
582 SVD_t svd;
583 matrix_t PK;
584
585 mutable size_type maxRank;
586
587 ComparisonTypes_t comparison;
588 std::vector<std::size_t> inequalityIndices;
589 Eigen::RowBlockIndices equalityIndices;
590 Eigen::MatrixBlocks<false, false> activeRowsOfJ;
591 };
592
595 void update();
596
600 virtual void computeActiveRowsOfJ(std::size_t iStack);
601
609 void computeDescentDirection() const;
610 void expandDqSmall() const;
611 void saturate(vectorOut_t arg) const;
612
613 value_type squaredErrorThreshold_, inequalityThreshold_;
614 size_type maxIterations_;
615
616 std::vector<ImplicitConstraintSet> stacks_;
617 LiegroupSpacePtr_t configSpace_;
618 size_type dimension_, reducedDimension_;
619 bool lastIsOptional_;
620 bool solveLevelByLevel_;
622 Indices_t freeVariables_;
623 Saturation_t saturate_;
625 NumericalConstraints_t constraints_;
627 std::map<DifferentiableFunctionPtr_t, size_type> iq_;
629 std::map<DifferentiableFunctionPtr_t, size_type> iv_;
631 std::map<DifferentiableFunctionPtr_t, std::size_t> priority_;
632
634 mutable value_type sigma_;
635
636 mutable vector_t dq_, dqSmall_;
637 mutable matrix_t reducedJ_;
638 mutable Eigen::VectorXi saturation_, reducedSaturation_;
639 mutable Configuration_t qSat_;
640 mutable ArrayXb tmpSat_;
641 mutable value_type squaredNorm_;
642 mutable std::vector<Data> datas_;
643 mutable SVD_t svd_;
644 mutable vector_t OM_;
645 mutable vector_t OP_;
646
647 friend struct lineSearch::Backtracking;
648
649 protected:
650 HierarchicalIterative() {}
651
652 private:
653 HPP_SERIALIZABLE_SPLIT();
654}; // class HierarchicalIterative
656 inline std::ostream& operator<<(std::ostream& os,
657 const HierarchicalIterative& hs) {
658 return hs.print(os);
659}
660} // namespace solver
661} // namespace constraints
662} // namespace hpp
663
664#endif // HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
Eigen::MatrixBlocks< false, true >
hpp::constraints::ImplicitConstraintSet
Definition implicit-constraint-set.hh:45
hpp::constraints::solver::HierarchicalIterative
Definition hierarchical-iterative.hh:240
hpp::constraints::solver::HierarchicalIterative::expandDqSmall
void expandDqSmall() const
hpp::constraints::solver::HierarchicalIterative::lastStep
const vector_t & lastStep() const
Accessor to the last step done.
Definition hierarchical-iterative.hh:551
hpp::constraints::solver::HierarchicalIterative::qSat_
Configuration_t qSat_
Definition hierarchical-iterative.hh:639
hpp::constraints::solver::HierarchicalIterative::~HierarchicalIterative
virtual ~HierarchicalIterative()
Definition hierarchical-iterative.hh:252
hpp::constraints::solver::HierarchicalIterative::integrate
virtual bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
hpp::constraints::solver::HierarchicalIterative::rightHandSideFromConfig
virtual bool rightHandSideFromConfig(const ImplicitPtr_t &constraint, ConfigurationIn_t config)
hpp::constraints::solver::HierarchicalIterative::stacks_
std::vector< ImplicitConstraintSet > stacks_
Definition hierarchical-iterative.hh:616
hpp::constraints::solver::HierarchicalIterative::DefaultLineSearch
lineSearch::FixedSequence DefaultLineSearch
Definition hierarchical-iterative.hh:243
hpp::constraints::solver::HierarchicalIterative::solve
Status solve(vectorOut_t arg) const
Definition hierarchical-iterative.hh:331
hpp::constraints::solver::HierarchicalIterative::saturate_
Saturation_t saturate_
Definition hierarchical-iterative.hh:623
hpp::constraints::solver::HierarchicalIterative::getRightHandSide
virtual bool getRightHandSide(const ImplicitPtr_t &constraint, vectorOut_t rhs) const
Get right hand side of a constraints.
hpp::constraints::solver::HierarchicalIterative::saturate
void saturate(vectorOut_t arg) const
hpp::constraints::solver::HierarchicalIterative::solveLevelByLevel
bool solveLevelByLevel() const
Definition hierarchical-iterative.hh:436
hpp::constraints::solver::HierarchicalIterative::computeActiveRowsOfJ
virtual void computeActiveRowsOfJ(std::size_t iStack)
hpp::constraints::solver::HierarchicalIterative::maxIterations_
size_type maxIterations_
Definition hierarchical-iterative.hh:614
hpp::constraints::solver::HierarchicalIterative::rightHandSideAt
void rightHandSideAt(const value_type &s)
hpp::constraints::solver::HierarchicalIterative::saturation
void saturation(const Saturation_t &saturate)
Set the saturation function.
Definition hierarchical-iterative.hh:280
hpp::constraints::solver::HierarchicalIterative::rightHandSideFromConfig
vector_t rightHandSideFromConfig(ConfigurationIn_t config)
hpp::constraints::solver::HierarchicalIterative::solveLevelByLevel_
bool solveLevelByLevel_
Definition hierarchical-iterative.hh:620
hpp::constraints::solver::HierarchicalIterative::reducedSaturation_
Eigen::VectorXi reducedSaturation_
Definition hierarchical-iterative.hh:638
hpp::constraints::solver::HierarchicalIterative::print
virtual std::ostream & print(std::ostream &os) const
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:443
hpp::constraints::solver::HierarchicalIterative::freeVariables
void freeVariables(const Indices_t &indices)
Definition hierarchical-iterative.hh:395
hpp::constraints::solver::HierarchicalIterative::isConstraintSatisfied
bool isConstraintSatisfied(const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
hpp::constraints::solver::HierarchicalIterative::OM_
vector_t OM_
Definition hierarchical-iterative.hh:644
hpp::constraints::solver::HierarchicalIterative::HierarchicalIterative
HierarchicalIterative(const LiegroupSpacePtr_t &configSpace)
hpp::constraints::solver::HierarchicalIterative::residualError
value_type residualError() const
Returns the squared norm of the error vector.
Definition hierarchical-iterative.hh:468
hpp::constraints::solver::HierarchicalIterative::errorThreshold
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition hierarchical-iterative.hh:409
hpp::constraints::solver::HierarchicalIterative::HierarchicalIterative
HierarchicalIterative(const HierarchicalIterative &other)
hpp::constraints::solver::HierarchicalIterative::squaredNorm_
value_type squaredNorm_
Definition hierarchical-iterative.hh:641
hpp::constraints::solver::HierarchicalIterative::computeValue
void computeValue(vectorIn_t arg) const
Compute the value of each level, and the jacobian if ComputeJac is true.
hpp::constraints::solver::HierarchicalIterative::getValue
void getValue(vectorOut_t v) const
hpp::constraints::solver::HierarchicalIterative::numberStacks
std::size_t numberStacks() const
Definition hierarchical-iterative.hh:451
hpp::constraints::solver::HierarchicalIterative::inequalityThreshold_
value_type inequalityThreshold_
Definition hierarchical-iterative.hh:613
hpp::constraints::solver::HierarchicalIterative::add
virtual bool add(const ImplicitPtr_t &constraint, const std::size_t &priority)
hpp::constraints::solver::HierarchicalIterative::rightHandSideSize
size_type rightHandSideSize() const
hpp::constraints::solver::HierarchicalIterative::priority_
std::map< DifferentiableFunctionPtr_t, std::size_t > priority_
Priority level of constraint.
Definition hierarchical-iterative.hh:631
hpp::constraints::solver::HierarchicalIterative::configSpace_
LiegroupSpacePtr_t configSpace_
Definition hierarchical-iterative.hh:617
hpp::constraints::solver::HierarchicalIterative::update
void update()
hpp::constraints::solver::HierarchicalIterative::sigma
const value_type & sigma() const
Definition hierarchical-iterative.hh:370
hpp::constraints::solver::HierarchicalIterative::solve
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
hpp::constraints::solver::HierarchicalIterative::computeSaturation
void computeSaturation(vectorIn_t arg) const
hpp::constraints::solver::HierarchicalIterative::dq_
vector_t dq_
Definition hierarchical-iterative.hh:636
hpp::constraints::solver::HierarchicalIterative::rightHandSide
virtual void rightHandSide(vectorIn_t rhs)
hpp::constraints::solver::HierarchicalIterative::SVD_t
Eigen::JacobiSVD< matrix_t > SVD_t
Definition hierarchical-iterative.hh:560
hpp::constraints::solver::HierarchicalIterative::HierarchicalIterative
HierarchicalIterative()
Definition hierarchical-iterative.hh:650
hpp::constraints::solver::HierarchicalIterative::saturation
const Saturation_t & saturation() const
Get the saturation function.
Definition hierarchical-iterative.hh:283
hpp::constraints::solver::HierarchicalIterative::Status
Status
Definition hierarchical-iterative.hh:245
hpp::constraints::solver::HierarchicalIterative::ERROR_INCREASED
@ ERROR_INCREASED
Definition hierarchical-iterative.hh:245
hpp::constraints::solver::HierarchicalIterative::activeDerivativeParameters
ArrayXb activeDerivativeParameters() const
Velocity parameters involved in the constraint resolution.
hpp::constraints::solver::HierarchicalIterative::freeVariables
void freeVariables(const segments_t intervals)
Definition hierarchical-iterative.hh:383
hpp::constraints::solver::HierarchicalIterative::reducedDimension
const size_type & reducedDimension() const
Definition hierarchical-iterative.hh:457
hpp::constraints::solver::HierarchicalIterative::OP_
vector_t OP_
Definition hierarchical-iterative.hh:645
hpp::constraints::solver::HierarchicalIterative::iq_
std::map< DifferentiableFunctionPtr_t, size_type > iq_
Value rank of constraint in its priority level.
Definition hierarchical-iterative.hh:627
hpp::constraints::solver::HierarchicalIterative::lastIsOptional
void lastIsOptional(bool optional)
Definition hierarchical-iterative.hh:422
hpp::constraints::solver::HierarchicalIterative::sigma_
value_type sigma_
The smallest non-zero singular value.
Definition hierarchical-iterative.hh:634
hpp::constraints::solver::HierarchicalIterative::rightHandSide
vector_t rightHandSide() const
hpp::constraints::solver::HierarchicalIterative::datas_
std::vector< Data > datas_
Definition hierarchical-iterative.hh:642
hpp::constraints::solver::HierarchicalIterative::inequalityThreshold
void inequalityThreshold(const value_type &it)
set the inequality threshold
Definition hierarchical-iterative.hh:420
hpp::constraints::solver::HierarchicalIterative::tmpSat_
ArrayXb tmpSat_
Definition hierarchical-iterative.hh:640
hpp::constraints::solver::HierarchicalIterative::constraints_
NumericalConstraints_t constraints_
Members moved from core::ConfigProjector.
Definition hierarchical-iterative.hh:625
hpp::constraints::solver::HierarchicalIterative::residualError
void residualError(vectorOut_t error) const
Returns the error vector.
hpp::constraints::solver::HierarchicalIterative::freeVariables
const Indices_t & freeVariables() const
Get free velocity variables.
Definition hierarchical-iterative.hh:401
hpp::constraints::solver::HierarchicalIterative::lastIsOptional
bool lastIsOptional() const
Definition hierarchical-iterative.hh:424
hpp::constraints::solver::HierarchicalIterative::isSatisfied
bool isSatisfied(vectorIn_t arg) const
Definition hierarchical-iterative.hh:338
hpp::constraints::solver::HierarchicalIterative::merge
virtual void merge(const HierarchicalIterative &other)
hpp::constraints::solver::HierarchicalIterative::constraints
const NumericalConstraints_t & constraints() const
Get constraints (implicit and explicit)
Definition hierarchical-iterative.hh:449
hpp::constraints::solver::HierarchicalIterative::contains
virtual bool contains(const ImplicitPtr_t &numericalConstraint) const
hpp::constraints::solver::HierarchicalIterative::computeError
void computeError() const
hpp::constraints::solver::HierarchicalIterative::activeParameters
ArrayXb activeParameters() const
Configuration parameters involved in the constraint resolution.
hpp::constraints::solver::HierarchicalIterative::extract
HierarchicalIterative extract(interval_t interval) const
hpp::constraints::solver::HierarchicalIterative::definesSubmanifoldOf
bool definesSubmanifoldOf(const HierarchicalIterative &solver) const
hpp::constraints::solver::HierarchicalIterative::solveLevelByLevel
void solveLevelByLevel(bool solveLevelByLevel)
Definition hierarchical-iterative.hh:431
hpp::constraints::solver::HierarchicalIterative::Indices_t
Eigen::RowBlockIndices Indices_t
Definition hierarchical-iterative.hh:242
hpp::constraints::solver::HierarchicalIterative::dimension_
size_type dimension_
Definition hierarchical-iterative.hh:618
hpp::constraints::solver::HierarchicalIterative::Saturation_t
shared_ptr< saturation::Base > Saturation_t
Definition hierarchical-iterative.hh:246
hpp::constraints::solver::HierarchicalIterative::iv_
std::map< DifferentiableFunctionPtr_t, size_type > iv_
Derivative rank of constraint in its priority level.
Definition hierarchical-iterative.hh:629
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::HierarchicalIterative::maxIterations
void maxIterations(size_type iterations)
Set maximal number of iterations.
Definition hierarchical-iterative.hh:404
hpp::constraints::solver::HierarchicalIterative::isSatisfied
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition hierarchical-iterative.hh:348
hpp::constraints::solver::HierarchicalIterative::svd_
SVD_t svd_
Definition hierarchical-iterative.hh:643
hpp::constraints::solver::HierarchicalIterative::computeDescentDirection
void computeDescentDirection() const
hpp::constraints::solver::HierarchicalIterative::inequalityThreshold
value_type inequalityThreshold() const
Get the inequality threshold.
Definition hierarchical-iterative.hh:418
hpp::constraints::solver::HierarchicalIterative::errorThreshold
value_type errorThreshold() const
Get error threshold.
Definition hierarchical-iterative.hh:413
hpp::constraints::solver::HierarchicalIterative::freeVariables_
Indices_t freeVariables_
Unknown of the set of implicit constraints.
Definition hierarchical-iterative.hh:622
hpp::constraints::solver::HierarchicalIterative::configSpace
const LiegroupSpacePtr_t & configSpace() const
Get configuration space on which constraints are defined.
Definition hierarchical-iterative.hh:258
hpp::constraints::solver::HierarchicalIterative::reducedJ_
matrix_t reducedJ_
Definition hierarchical-iterative.hh:637
hpp::constraints::solver::HierarchicalIterative::lastIsOptional_
bool lastIsOptional_
Definition hierarchical-iterative.hh:619
hpp::constraints::solver::HierarchicalIterative::squaredErrorThreshold
value_type squaredErrorThreshold() const
Get error threshold.
Definition hierarchical-iterative.hh:415
hpp::constraints::solver::HierarchicalIterative::rightHandSide
virtual bool rightHandSide(const ImplicitPtr_t &constraint, vectorIn_t rhs)
hpp::constraints::solver::HierarchicalIterative::getReducedJacobian
void getReducedJacobian(matrixOut_t J) const
hpp::constraints::solver::HierarchicalIterative::dimension
const size_type & dimension() const
Definition hierarchical-iterative.hh:453
hpp::pinocchio::LiegroupElementBase
config.hh
HPP_CONSTRAINTS_DLLAPI
#define HPP_CONSTRAINTS_DLLAPI
Definition config.hh:88
fwd.hh
implicit-constraint-set.hh
matrix-view.hh
hpp::constraints::solver::operator<<
std::ostream & operator<<(std::ostream &os, const HierarchicalIterative &hs)
Definition hierarchical-iterative.hh:656
hpp::constraints::Configuration_t
pinocchio::Configuration_t Configuration_t
Definition fwd.hh:107
hpp::constraints::ComparisonTypes_t
std::vector< ComparisonType > ComparisonTypes_t
Definition fwd.hh:182
hpp::constraints::DevicePtr_t
pinocchio::DevicePtr_t DevicePtr_t
Definition fwd.hh:111
hpp::constraints::LiegroupSpacePtr_t
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition fwd.hh:70
hpp::constraints::ImplicitPtr_t
shared_ptr< Implicit > ImplicitPtr_t
Definition fwd.hh:175
hpp::constraints::interval_t
std::pair< value_type, value_type > interval_t
Definition fwd.hh:85
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::ArrayXb
pinocchio::ArrayXb ArrayXb
Definition fwd.hh:81
hpp::constraints::value_type
pinocchio::value_type value_type
Definition fwd.hh:49
hpp::constraints::vectorIn_t
pinocchio::vectorIn_t vectorIn_t
Definition fwd.hh:61
hpp::constraints::ConfigurationIn_t
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition fwd.hh:108
hpp::constraints::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:57
hpp::constraints::matrixOut_t
Eigen::Ref< matrix_t > matrixOut_t
Definition fwd.hh:59
hpp::constraints::segments_t
std::vector< segment_t > segments_t
Definition fwd.hh:86
hpp::constraints::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:62
hpp::constraints::NumericalConstraints_t
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition fwd.hh:177
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:60
hpp
serialization-fwd.hh
HPP_SERIALIZABLE_SPLIT
#define HPP_SERIALIZABLE_SPLIT()
hpp::constraints::solver::HierarchicalIterative::Data
Definition hierarchical-iterative.hh:562
hpp::constraints::solver::HierarchicalIterative::Data::inequalityIndices
std::vector< std::size_t > inequalityIndices
Definition hierarchical-iterative.hh:588
hpp::constraints::solver::HierarchicalIterative::Data::output
LiegroupElement output
Definition hierarchical-iterative.hh:578
hpp::constraints::solver::HierarchicalIterative::Data::maxRank
size_type maxRank
Definition hierarchical-iterative.hh:585
hpp::constraints::solver::HierarchicalIterative::Data::comparison
ComparisonTypes_t comparison
Definition hierarchical-iterative.hh:587
hpp::constraints::solver::HierarchicalIterative::Data::equalityIndices
Eigen::RowBlockIndices equalityIndices
Definition hierarchical-iterative.hh:589
hpp::constraints::solver::HierarchicalIterative::Data::jacobian
matrix_t jacobian
Definition hierarchical-iterative.hh:580
hpp::constraints::solver::HierarchicalIterative::Data::svd
SVD_t svd
Definition hierarchical-iterative.hh:582
hpp::constraints::solver::HierarchicalIterative::Data::error
vector_t error
Definition hierarchical-iterative.hh:579
hpp::constraints::solver::HierarchicalIterative::Data::PK
matrix_t PK
Definition hierarchical-iterative.hh:583
hpp::constraints::solver::HierarchicalIterative::Data::activeRowsOfJ
Eigen::MatrixBlocks< false, false > activeRowsOfJ
Definition hierarchical-iterative.hh:590
hpp::constraints::solver::HierarchicalIterative::Data::Data
Data()
Definition hierarchical-iterative.hh:563
hpp::constraints::solver::lineSearch::Backtracking
Definition hierarchical-iterative.hh:54
hpp::constraints::solver::lineSearch::Backtracking::df
vector_t df
Definition hierarchical-iterative.hh:64
hpp::constraints::solver::lineSearch::Backtracking::arg_darg
vector_t arg_darg
Definition hierarchical-iterative.hh:64
hpp::constraints::solver::lineSearch::Backtracking::smallAlpha
value_type smallAlpha
Definition hierarchical-iterative.hh:63
hpp::constraints::solver::lineSearch::Backtracking::tau
value_type tau
Definition hierarchical-iterative.hh:63
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::lineSearch::Backtracking::Backtracking
Backtracking()
hpp::constraints::solver::lineSearch::Backtracking::computeLocalSlope
value_type computeLocalSlope(const SolverType &solver) const
Definition hierarchical-iterative.hh:94
hpp::constraints::solver::lineSearch::Backtracking::c
value_type c
Definition hierarchical-iterative.hh:63
hpp::constraints::solver::lineSearch::Backtracking::darg
vector_t darg
Definition hierarchical-iterative.hh:64
hpp::constraints::solver::lineSearch::Constant
No line search. Use .
Definition hierarchical-iterative.hh:47
hpp::constraints::solver::lineSearch::Constant::operator()
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition hierarchical-iterative.hh:41
hpp::constraints::solver::lineSearch::ErrorNormBased
Definition hierarchical-iterative.hh:85
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::ErrorNormBased::ErrorNormBased
ErrorNormBased(value_type alphaMin=0.2)
hpp::constraints::solver::lineSearch::ErrorNormBased::b
value_type b
Definition hierarchical-iterative.hh:92
hpp::constraints::solver::lineSearch::ErrorNormBased::K
value_type K
Definition hierarchical-iterative.hh:92
hpp::constraints::solver::lineSearch::ErrorNormBased::ErrorNormBased
ErrorNormBased(value_type alphaMin, value_type _a, value_type _b)
hpp::constraints::solver::lineSearch::ErrorNormBased::a
value_type a
Definition hierarchical-iterative.hh:92
hpp::constraints::solver::lineSearch::ErrorNormBased::C
value_type C
Definition hierarchical-iterative.hh:92
hpp::constraints::solver::lineSearch::FixedSequence
Definition hierarchical-iterative.hh:70
hpp::constraints::solver::lineSearch::FixedSequence::FixedSequence
FixedSequence()
hpp::constraints::solver::lineSearch::FixedSequence::alphaMax
value_type alphaMax
Definition hierarchical-iterative.hh:77
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::lineSearch::FixedSequence::K
value_type K
Definition hierarchical-iterative.hh:77
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
hpp::constraints::solver::saturation::Base::~Base
virtual ~Base()
Definition hierarchical-iterative.hh:118
hpp::constraints::solver::saturation::Base::saturate
virtual bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
hpp::constraints::solver::saturation::Bounds
simple box constraints
Definition hierarchical-iterative.hh:132
hpp::constraints::solver::saturation::Bounds::Bounds
Bounds(const vector_t &lb, const vector_t &ub)
Definition hierarchical-iterative.hh:135
hpp::constraints::solver::saturation::Bounds::ub
vector_t ub
Definition hierarchical-iterative.hh:143
hpp::constraints::solver::saturation::Bounds::Bounds
Bounds(const vector_t &lb, const vector_t &ub, const Eigen::VectorXi &iq2iv)
Definition hierarchical-iterative.hh:141
hpp::constraints::solver::saturation::Bounds::iq2iv_
Eigen::VectorXi iq2iv_
Definition hierarchical-iterative.hh:144
hpp::constraints::solver::saturation::Bounds::saturate
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
hpp::constraints::solver::saturation::Bounds::lb
vector_t lb
Definition hierarchical-iterative.hh:143
hpp::constraints::solver::saturation::Bounds::Bounds
Bounds()
Definition hierarchical-iterative.hh:134
hpp::constraints::solver::saturation::Device
Box constraints use a Device joint limits.
Definition hierarchical-iterative.hh:147
hpp::constraints::solver::saturation::Device::device
DevicePtr_t device
Definition hierarchical-iterative.hh:152
hpp::constraints::solver::saturation::Device::Device
Device()
Definition hierarchical-iterative.hh:150
hpp::constraints::solver::saturation::Device::saturate
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
hpp::constraints::solver::saturation::Device::Device
Device(const DevicePtr_t &device)
Definition hierarchical-iterative.hh:151
hpp::constraints::solver::saturation::Function
saturation from a std::function.
Definition hierarchical-iterative.hh:121
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::saturation::Function::function
function_t function
Definition hierarchical-iterative.hh:129
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::solver::saturation::Function::Function
Function()
Definition hierarchical-iterative.hh:127
hpp::constraints::solver::saturation::Function::Function
Function(const function_t &function)
Definition hierarchical-iterative.hh:128