17 #ifndef HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
18 #define HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
23 #include <hpp/util/serialization-fwd.hh>
25 #include <hpp/util/serialization-fwd.hh>
34 namespace constraints {
38 namespace lineSearch {
41 template <
typename SolverType>
51 template <
typename SolverType>
55 template <
typename SolverType>
68 template <
typename SolverType>
85 template <
typename SolverType>
93 namespace saturation {
114 Eigen::VectorXi& saturation);
121 Eigen::VectorXi& saturation) {
122 return function(q, qSat, saturation);
131 Eigen::VectorXi& saturation);
140 Eigen::VectorXi& saturation);
271 saturate_ = saturate;
299 template <
typename LineSearchType>
326 computeValue<false>(arg);
328 return squaredNorm_ < squaredErrorThreshold_;
337 computeValue<false>(arg);
339 return squaredNorm_ < errorThreshold*errorThreshold;
353 bool& constraintFound)
const;
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>();
390 freeVariables_ = indices;
397 return freeVariables_;
403 maxIterations_ = iterations;
408 return maxIterations_;
414 squaredErrorThreshold_ = threshold * threshold;
419 return sqrt (squaredErrorThreshold_);
424 return squaredErrorThreshold_;
430 return inequalityThreshold_;
435 inequalityThreshold_ = it;
440 lastIsOptional_ = optional;
445 return lastIsOptional_;
456 assert(priority < stacks_.size());
457 return stacks_[priority];
468 return stacks_.size();
480 return reducedDimension_;
586 virtual std::ostream&
print (std::ostream& os)
const;
589 typedef Eigen::JacobiSVD <matrix_t>
SVD_t;
593 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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_;
669 HPP_SERIALIZABLE_SPLIT();
Definition: implicit-constraint-set.hh:36
Definition: hierarchical-iterative.hh:219
void expandDqSmall() const
const size_type & dimension() const
Definition: hierarchical-iterative.hh:471
Configuration_t qSat_
Definition: hierarchical-iterative.hh:656
virtual ~HierarchicalIterative()
Definition: hierarchical-iterative.hh:236
virtual bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result) const
virtual bool rightHandSideFromConfig(const ImplicitPtr_t &constraint, ConfigurationIn_t config)
std::vector< ImplicitConstraintSet > stacks_
Definition: hierarchical-iterative.hh:634
lineSearch::FixedSequence DefaultLineSearch
Definition: hierarchical-iterative.hh:222
Status solve(vectorOut_t arg) const
Definition: hierarchical-iterative.hh:316
Saturation_t saturate_
Definition: hierarchical-iterative.hh:640
virtual bool getRightHandSide(const ImplicitPtr_t &constraint, vectorOut_t rhs) const
Get right hand side of a constraints.
void saturate(vectorOut_t arg) const
virtual void computeActiveRowsOfJ(std::size_t iStack)
size_type maxIterations_
Definition: hierarchical-iterative.hh:632
void rightHandSideAt(const value_type &s)
void saturation(const Saturation_t &saturate)
Set the saturation function.
Definition: hierarchical-iterative.hh:269
vector_t rightHandSideFromConfig(ConfigurationIn_t config)
Eigen::VectorXi reducedSaturation_
Definition: hierarchical-iterative.hh:655
const size_type & reducedDimension() const
Definition: hierarchical-iterative.hh:478
const ImplicitConstraintSet & constraints(const std::size_t priority)
Get set of constraints for a give priority level.
Definition: hierarchical-iterative.hh:454
void freeVariables(const Indices_t &indices)
Definition: hierarchical-iterative.hh:388
bool isConstraintSatisfied(const ImplicitPtr_t &constraint, vectorIn_t arg, vectorOut_t error, bool &constraintFound) const
vector_t OM_
Definition: hierarchical-iterative.hh:661
HierarchicalIterative(const LiegroupSpacePtr_t &configSpace)
value_type residualError() const
Returns the squared norm of the error vector.
Definition: hierarchical-iterative.hh:492
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: hierarchical-iterative.hh:412
HierarchicalIterative(const HierarchicalIterative &other)
value_type squaredNorm_
Definition: hierarchical-iterative.hh:658
virtual std::ostream & print(std::ostream &os) const
void computeValue(vectorIn_t arg) const
Compute the value of each level, and the jacobian if ComputeJac is true.
void getValue(vectorOut_t v) const
std::size_t numberStacks() const
Definition: hierarchical-iterative.hh:466
value_type inequalityThreshold_
Definition: hierarchical-iterative.hh:631
virtual bool add(const ImplicitPtr_t &constraint, const std::size_t &priority)
size_type rightHandSideSize() const
std::map< DifferentiableFunctionPtr_t, std::size_t > priority_
Priority level of constraint.
Definition: hierarchical-iterative.hh:648
LiegroupSpacePtr_t configSpace_
Definition: hierarchical-iterative.hh:635
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
const Saturation_t & saturation() const
Get the saturation function.
Definition: hierarchical-iterative.hh:275
void computeSaturation(vectorIn_t arg) const
vector_t dq_
Definition: hierarchical-iterative.hh:653
virtual void rightHandSide(vectorIn_t rhs)
Eigen::JacobiSVD< matrix_t > SVD_t
Definition: hierarchical-iterative.hh:589
const vector_t & lastStep() const
Accessor to the last step done.
Definition: hierarchical-iterative.hh:577
HierarchicalIterative()
Definition: hierarchical-iterative.hh:667
Status
Definition: hierarchical-iterative.hh:224
@ ERROR_INCREASED
Definition: hierarchical-iterative.hh:225
@ MAX_ITERATION_REACHED
Definition: hierarchical-iterative.hh:226
@ INFEASIBLE
Definition: hierarchical-iterative.hh:227
ArrayXb activeDerivativeParameters() const
Velocity parameters involved in the constraint resolution.
void freeVariables(const segments_t intervals)
Definition: hierarchical-iterative.hh:375
vector_t OP_
Definition: hierarchical-iterative.hh:662
std::map< DifferentiableFunctionPtr_t, size_type > iq_
Value rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:644
void lastIsOptional(bool optional)
Definition: hierarchical-iterative.hh:438
value_type sigma_
The smallest non-zero singular value.
Definition: hierarchical-iterative.hh:651
vector_t rightHandSide() const
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:659
void inequalityThreshold(const value_type &it)
set the inequality threshold
Definition: hierarchical-iterative.hh:433
ArrayXb tmpSat_
Definition: hierarchical-iterative.hh:657
NumericalConstraints_t constraints_
Members moved from core::ConfigProjector.
Definition: hierarchical-iterative.hh:642
void residualError(vectorOut_t error) const
Returns the error vector.
bool lastIsOptional() const
Definition: hierarchical-iterative.hh:443
bool isSatisfied(vectorIn_t arg) const
Compares to internal error threshold.
Definition: hierarchical-iterative.hh:324
virtual void merge(const HierarchicalIterative &other)
virtual bool contains(const ImplicitPtr_t &numericalConstraint) const
void computeError() const
ArrayXb activeParameters() const
Configuration parameters involved in the constraint resolution.
bool definesSubmanifoldOf(const HierarchicalIterative &solver) const
const LiegroupSpacePtr_t & configSpace() const
Get configuration space on which constraints are defined.
Definition: hierarchical-iterative.hh:242
Eigen::RowBlockIndices Indices_t
Definition: hierarchical-iterative.hh:221
size_type dimension_
Definition: hierarchical-iterative.hh:636
shared_ptr< saturation::Base > Saturation_t
Definition: hierarchical-iterative.hh:230
std::map< DifferentiableFunctionPtr_t, size_type > iv_
Derivative rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:646
size_type maxIterations() const
Get maximal number of iterations in config projector.
Definition: hierarchical-iterative.hh:406
void maxIterations(size_type iterations)
Set maximal number of iterations.
Definition: hierarchical-iterative.hh:401
bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const
Definition: hierarchical-iterative.hh:335
const value_type & sigma() const
Definition: hierarchical-iterative.hh:359
SVD_t svd_
Definition: hierarchical-iterative.hh:660
const Indices_t & freeVariables() const
Get free velocity variables.
Definition: hierarchical-iterative.hh:395
void computeDescentDirection() const
value_type inequalityThreshold() const
Get the inequality threshold.
Definition: hierarchical-iterative.hh:428
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:417
Indices_t freeVariables_
Unknown of the set of implicit constraints.
Definition: hierarchical-iterative.hh:639
matrix_t reducedJ_
Definition: hierarchical-iterative.hh:654
bool lastIsOptional_
Definition: hierarchical-iterative.hh:637
value_type squaredErrorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:422
virtual bool rightHandSide(const ImplicitPtr_t &constraint, vectorIn_t rhs)
void getReducedJacobian(matrixOut_t J) const
const NumericalConstraints_t & constraints() const
Get constraints (implicit and explicit)
Definition: hierarchical-iterative.hh:461
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
assert(d.lhs()._blocks()==d.rhs()._blocks())
std::ostream & operator<<(std::ostream &os, const HierarchicalIterative &hs)
Definition: hierarchical-iterative.hh:672
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:93
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:176
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:165
pinocchio::size_type size_type
Definition: fwd.hh:36
std::vector< segment_t > segments_t
Definition: fwd.hh:72
pinocchio::ArrayXb ArrayXb
Definition: fwd.hh:68
pinocchio::value_type value_type
Definition: fwd.hh:37
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
pinocchio::vector_t vector_t
Definition: fwd.hh:47
Definition: active-set-differentiable-function.hh:24
Definition: hierarchical-iterative.hh:591
std::vector< std::size_t > inequalityIndices
Definition: hierarchical-iterative.hh:605
LiegroupElement output
Definition: hierarchical-iterative.hh:595
size_type maxRank
Definition: hierarchical-iterative.hh:602
Eigen::MatrixBlocks< false, false > activeRowsOfJ
Definition: hierarchical-iterative.hh:607
ComparisonTypes_t comparison
Definition: hierarchical-iterative.hh:604
Eigen::RowBlockIndices equalityIndices
Definition: hierarchical-iterative.hh:606
matrix_t jacobian
Definition: hierarchical-iterative.hh:597
SVD_t svd
Definition: hierarchical-iterative.hh:599
vector_t error
Definition: hierarchical-iterative.hh:596
matrix_t PK
Definition: hierarchical-iterative.hh:600
Definition: hierarchical-iterative.hh:48
vector_t df
Definition: hierarchical-iterative.hh:59
vector_t arg_darg
Definition: hierarchical-iterative.hh:59
value_type smallAlpha
Definition: hierarchical-iterative.hh:58
value_type tau
Definition: hierarchical-iterative.hh:58
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:37
value_type computeLocalSlope(const SolverType &solver) const
Definition: hierarchical-iterative.hh:82
value_type c
Definition: hierarchical-iterative.hh:58
vector_t darg
Definition: hierarchical-iterative.hh:59
No line search. Use .
Definition: hierarchical-iterative.hh:40
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:30
Definition: hierarchical-iterative.hh:81
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:105
ErrorNormBased(value_type alphaMin=0.2)
value_type b
Definition: hierarchical-iterative.hh:89
value_type K
Definition: hierarchical-iterative.hh:89
ErrorNormBased(value_type alphaMin, value_type _a, value_type _b)
value_type a
Definition: hierarchical-iterative.hh:89
value_type C
Definition: hierarchical-iterative.hh:89
Definition: hierarchical-iterative.hh:65
value_type alphaMax
Definition: hierarchical-iterative.hh:73
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:96
value_type K
Definition: hierarchical-iterative.hh:73
value_type alpha
Definition: hierarchical-iterative.hh:72
Base class for box constraints. To prevent configuration variables to get out of joint limits during ...
Definition: hierarchical-iterative.hh:101
virtual ~Base()
Definition: hierarchical-iterative.hh:115
virtual bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
simple box constraints
Definition: hierarchical-iterative.hh:129
Bounds(const vector_t &lb, const vector_t &ub)
Definition: hierarchical-iterative.hh:133
vector_t ub
Definition: hierarchical-iterative.hh:134
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
vector_t lb
Definition: hierarchical-iterative.hh:134
Bounds()
Definition: hierarchical-iterative.hh:132
Box constraints use a Device joint limits.
Definition: hierarchical-iterative.hh:137
DevicePtr_t device
Definition: hierarchical-iterative.hh:143
Device()
Definition: hierarchical-iterative.hh:141
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
Device(const DevicePtr_t &device)
Definition: hierarchical-iterative.hh:142
saturation from a std::function.
Definition: hierarchical-iterative.hh:118
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
Definition: hierarchical-iterative.hh:120
std::function< bool(vectorIn_t, vectorOut_t, Eigen::VectorXi &)> function_t
Definition: hierarchical-iterative.hh:119
function_t function
Definition: hierarchical-iterative.hh:126
Function()
Definition: hierarchical-iterative.hh:124
Function(const function_t &function)
Definition: hierarchical-iterative.hh:125