29 #ifndef HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH 30 #define HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH 37 #include <hpp/util/serialization-fwd.hh> 41 namespace constraints {
45 namespace lineSearch {
48 template <
typename SolverType>
57 template <
typename SolverType>
60 template <
typename SolverType>
73 template <
typename SolverType>
89 template <
typename SolverType>
96 namespace saturation {
117 Eigen::VectorXi& saturation);
122 typedef std::function<bool(vectorIn_t, vectorOut_t, Eigen::VectorXi&)>
125 return function(q, qSat, saturation);
245 enum Status { ERROR_INCREASED, MAX_ITERATION_REACHED, INFEASIBLE, SUCCESS };
262 virtual bool contains(
const ImplicitPtr_t& numericalConstraint)
const;
270 const std::size_t& priority);
304 template <
typename LineSearchType>
305 Status solve(
vectorOut_t arg, LineSearchType ls = LineSearchType())
const;
329 computeValue<false>(arg);
331 return squaredNorm_ < squaredErrorThreshold_;
339 computeValue<false>(arg);
341 return squaredNorm_ < errorThreshold * errorThreshold;
375 for (std::size_t i = 0; i < intervals.size(); ++i)
376 freeVariables_.addRow(intervals[i].first, intervals[i].second);
377 freeVariables_.updateIndices<
true,
true,
true>();
386 freeVariables_ = indices;
400 squaredErrorThreshold_ = threshold * threshold;
422 solveLevelByLevel_ = solveLevelByLevel;
434 assert(priority < stacks_.size());
435 return stacks_[priority];
450 ArrayXb activeParameters()
const;
453 ArrayXb activeDerivativeParameters()
const;
490 virtual bool rightHandSideFromConfig(
const ImplicitPtr_t& constraint,
500 virtual bool getRightHandSide(
const ImplicitPtr_t& constraint,
531 template <
bool ComputeJac>
538 void computeError()
const;
547 virtual std::ostream& print(std::ostream& os)
const;
550 typedef Eigen::JacobiSVD<matrix_t>
SVD_t;
554 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
578 virtual void computeActiveRowsOfJ(std::size_t iStack);
587 void computeDescentDirection()
const;
588 void expandDqSmall()
const;
605 std::map<DifferentiableFunctionPtr_t, size_type>
iq_;
607 std::map<DifferentiableFunctionPtr_t, size_type>
iv_;
609 std::map<DifferentiableFunctionPtr_t, std::size_t>
priority_;
631 HPP_SERIALIZABLE_SPLIT();
642 #endif // HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH std::vector< segment_t > segments_t
Definition: fwd.hh:84
std::ostream & operator<<(std::ostream &os, const HierarchicalIterative &hs)
Definition: hierarchical-iterative.hh:634
pinocchio::vector_t vector_t
Definition: fwd.hh:59
Device(const DevicePtr_t &device)
Definition: hierarchical-iterative.hh:151
vector_t ub
Definition: hierarchical-iterative.hh:143
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:60
Definition: implicit-constraint-set.hh:45
NumericalConstraints_t constraints_
Members moved from core::ConfigProjector.
Definition: hierarchical-iterative.hh:603
value_type computeLocalSlope(const SolverType &solver) const
Definition: hierarchical-iterative.hh:94
Definition: hierarchical-iterative.hh:54
void inequalityThreshold(const value_type &it)
set the inequality threshold
Definition: hierarchical-iterative.hh:410
vector_t darg
Definition: hierarchical-iterative.hh:64
bool lastIsOptional_
Definition: hierarchical-iterative.hh:597
std::map< DifferentiableFunctionPtr_t, std::size_t > priority_
Priority level of constraint.
Definition: hierarchical-iterative.hh:609
Eigen::MatrixBlocks< false, false > activeRowsOfJ
Definition: hierarchical-iterative.hh:568
size_type maxIterations_
Definition: hierarchical-iterative.hh:592
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: hierarchical-iterative.hh:399
Definition: hierarchical-iterative.hh:85
std::vector< std::size_t > inequalityIndices
Definition: hierarchical-iterative.hh:566
Function()
Definition: hierarchical-iterative.hh:127
Eigen::RowBlockIndices equalityIndices
Definition: hierarchical-iterative.hh:567
value_type C
Definition: hierarchical-iterative.hh:92
Definition: active-set-differentiable-function.hh:36
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:69
value_type K
Definition: hierarchical-iterative.hh:92
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:109
ArrayXb tmpSat_
Definition: hierarchical-iterative.hh:618
matrix_t reducedJ
Definition: hierarchical-iterative.hh:558
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:175
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:65
value_type a
Definition: hierarchical-iterative.hh:92
lineSearch::FixedSequence DefaultLineSearch
Definition: hierarchical-iterative.hh:243
LiegroupSpacePtr_t configSpace_
Definition: hierarchical-iterative.hh:595
SVD_t svd
Definition: hierarchical-iterative.hh:560
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
std::vector< ImplicitConstraintSet > stacks_
Definition: hierarchical-iterative.hh:594
shared_ptr< saturation::Base > Saturation_t
Definition: hierarchical-iterative.hh:246
Bounds(const vector_t &lb, const vector_t &ub, const Eigen::VectorXi &iq2iv)
Definition: hierarchical-iterative.hh:141
simple box constraints
Definition: hierarchical-iterative.hh:132
pinocchio::matrix_t matrix_t
Definition: fwd.hh:56
Eigen::RowBlockIndices Indices_t
Definition: hierarchical-iterative.hh:242
value_type c
Definition: hierarchical-iterative.hh:63
pinocchio::ArrayXb ArrayXb
Definition: fwd.hh:80
value_type b
Definition: hierarchical-iterative.hh:92
value_type K
Definition: hierarchical-iterative.hh:77
size_type maxRank
Definition: hierarchical-iterative.hh:563
virtual std::ostream & print(std::ostream &os) const
const LiegroupSpacePtr_t & configSpace() const
Get configuration space on which constraints are defined.
Definition: hierarchical-iterative.hh:258
Base class for box constraints. To prevent configuration variables to get out of joint limits during ...
Definition: hierarchical-iterative.hh:104
std::function< bool(vectorIn_t, vectorOut_t, Eigen::VectorXi &)> function_t
Definition: hierarchical-iterative.hh:123
const Saturation_t & saturation() const
Get the saturation function.
Definition: hierarchical-iterative.hh:283
vector_t dqSmall_
Definition: hierarchical-iterative.hh:614
No line search. Use .
Definition: hierarchical-iterative.hh:47
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:105
Status
Definition: hierarchical-iterative.hh:245
Definition: hierarchical-iterative.hh:70
vector_t OM_
Definition: hierarchical-iterative.hh:622
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:106
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:180
const size_type & reducedDimension() const
Definition: hierarchical-iterative.hh:447
value_type alpha
Definition: hierarchical-iterative.hh:76
assert(d.lhs()._blocks()==d.rhs()._blocks())
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:109
const Indices_t & freeVariables() const
Get free velocity variables.
Definition: hierarchical-iterative.hh:391
Bounds(const vector_t &lb, const vector_t &ub)
Definition: hierarchical-iterative.hh:135
vector_t arg_darg
Definition: hierarchical-iterative.hh:64
Status solve(vectorOut_t arg) const
Definition: hierarchical-iterative.hh:321
void solveLevelByLevel(bool solveLevelByLevel)
Definition: hierarchical-iterative.hh:421
void freeVariables(const Indices_t &indices)
Definition: hierarchical-iterative.hh:385
size_type reducedDimension_
Definition: hierarchical-iterative.hh:596
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:41
function_t function
Definition: hierarchical-iterative.hh:129
void freeVariables(const segments_t intervals)
Definition: hierarchical-iterative.hh:373
bool isSatisfied(vectorIn_t arg) const
Definition: hierarchical-iterative.hh:328
const NumericalConstraints_t & constraints() const
Get constraints (implicit and explicit)
Definition: hierarchical-iterative.hh:439
const ImplicitConstraintSet & constraints(const std::size_t priority)
Get set of constraints for a give priority level.
Definition: hierarchical-iterative.hh:433
Configuration_t qSat_
Definition: hierarchical-iterative.hh:617
matrix_t reducedJ_
Definition: hierarchical-iterative.hh:615
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: hierarchical-iterative.hh:338
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:403
Device()
Definition: hierarchical-iterative.hh:150
Eigen::VectorXi saturation_
Definition: hierarchical-iterative.hh:616
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:118
Bounds()
Definition: hierarchical-iterative.hh:134
Saturation_t saturate_
Definition: hierarchical-iterative.hh:601
bool lastIsOptional() const
Definition: hierarchical-iterative.hh:414
std::map< DifferentiableFunctionPtr_t, size_type > iq_
Value rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:605
Eigen::VectorXi iq2iv_
Definition: hierarchical-iterative.hh:144
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
Definition: hierarchical-iterative.hh:124
Eigen::JacobiSVD< matrix_t > SVD_t
Definition: hierarchical-iterative.hh:550
Function(const function_t &function)
Definition: hierarchical-iterative.hh:128
saturation from a std::function.
Definition: hierarchical-iterative.hh:121
value_type squaredNorm_
Definition: hierarchical-iterative.hh:619
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:48
value_type tau
Definition: hierarchical-iterative.hh:63
virtual ~Base()
Definition: hierarchical-iterative.hh:118
value_type inequalityThreshold() const
Get the inequality threshold.
Definition: hierarchical-iterative.hh:408
ComparisonTypes_t comparison
Definition: hierarchical-iterative.hh:565
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
vector_t df
Definition: hierarchical-iterative.hh:64
void lastIsOptional(bool optional)
Definition: hierarchical-iterative.hh:412
virtual ~HierarchicalIterative()
Definition: hierarchical-iterative.hh:252
Definition: hierarchical-iterative.hh:552
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:88
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
value_type smallAlpha
Definition: hierarchical-iterative.hh:63
Indices_t freeVariables_
Unknown of the set of implicit constraints.
Definition: hierarchical-iterative.hh:600
value_type sigma_
The smallest non-zero singular value.
Definition: hierarchical-iterative.hh:612
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:620
void maxIterations(size_type iterations)
Set maximal number of iterations.
Definition: hierarchical-iterative.hh:394
void saturation(const Saturation_t &saturate)
Set the saturation function.
Definition: hierarchical-iterative.hh:280
pinocchio::size_type size_type
Definition: fwd.hh:47
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:58
vector_t error
Definition: hierarchical-iterative.hh:557
HierarchicalIterative()
Definition: hierarchical-iterative.hh:628
LiegroupElement rightHandSide
Definition: hierarchical-iterative.hh:556
SVD_t svd_
Definition: hierarchical-iterative.hh:621
Box constraints use a Device joint limits.
Definition: hierarchical-iterative.hh:147
vector_t lb
Definition: hierarchical-iterative.hh:143
vector_t OP_
Definition: hierarchical-iterative.hh:623
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:173
pinocchio::value_type value_type
Definition: fwd.hh:48
value_type residualError() const
Returns the squared norm of the error vector.
Definition: hierarchical-iterative.hh:458
const size_type & dimension() const
Definition: hierarchical-iterative.hh:443
virtual bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
matrix_t PK
Definition: hierarchical-iterative.hh:561
value_type squaredErrorThreshold_
Definition: hierarchical-iterative.hh:591
std::size_t numberStacks() const
Definition: hierarchical-iterative.hh:441
DevicePtr_t device
Definition: hierarchical-iterative.hh:152
value_type alphaMax
Definition: hierarchical-iterative.hh:77
Definition: hierarchical-iterative.hh:240
ErrorNormBased(value_type alphaMin, value_type _a, value_type _b)
size_type maxIterations() const
Get maximal number of iterations in config projector.
Definition: hierarchical-iterative.hh:396
std::map< DifferentiableFunctionPtr_t, size_type > iv_
Derivative rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:607
bool solveLevelByLevel_
Definition: hierarchical-iterative.hh:598
const vector_t & lastStep() const
Accessor to the last step done.
Definition: hierarchical-iterative.hh:541
bool solveLevelByLevel() const
Definition: hierarchical-iterative.hh:426
value_type squaredErrorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:405
const value_type & sigma() const
Definition: hierarchical-iterative.hh:360