hpp-constraints
4.9.0
Definition of basic geometric constraints for motion planning
|
Go to the documentation of this file.
17 #ifndef HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
18 #define HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
20 #include <boost/function.hpp>
22 #include <hpp/statistics/success-bin.hh>
32 namespace constraints {
36 namespace lineSearch {
39 template <
typename SolverType>
49 template <
typename SolverType>
53 template <
typename SolverType>
66 template <
typename SolverType>
83 template <
typename SolverType>
189 Eigen::VectorXi& saturation)>
213 virtual bool contains (
const ImplicitPtr_t& numericalConstraint)
const;
240 void add (
const ImplicitPtr_t& constraint,
const std::size_t& priority);
252 saturate_ = saturate;
280 template <
typename LineSearchType>
281 Status solve (
vectorOut_t arg, LineSearchType ls = LineSearchType())
const;
304 computeValue<false>(arg);
306 return squaredNorm_ < squaredErrorThreshold_;
320 bool& constraintFound)
const;
345 for (std::size_t i = 0; i < intervals.size(); ++i)
346 freeVariables_.addRow (intervals[i].first, intervals[i].second);
347 freeVariables_.updateIndices<
true,
true,
true>();
357 freeVariables_ = indices;
364 return freeVariables_;
370 maxIterations_ = iterations;
375 return maxIterations_;
381 squaredErrorThreshold_ = threshold * threshold;
386 return sqrt (squaredErrorThreshold_);
391 return squaredErrorThreshold_;
397 return inequalityThreshold_;
402 inequalityThreshold_ = it;
407 lastIsOptional_ = optional;
412 return lastIsOptional_;
423 assert(priority < stacks_.size());
424 return stacks_[priority];
435 return stacks_.size();
447 return reducedDimension_;
451 ArrayXb activeParameters ()
const;
454 ArrayXb activeDerivativeParameters ()
const;
494 virtual bool rightHandSideFromConfig (
const ImplicitPtr_t& constraint,
535 template <
bool ComputeJac>
void computeValue (
vectorIn_t arg)
const;
536 void computeSaturation (
vectorIn_t arg)
const;
541 void computeError ()
const;
553 virtual std::ostream& print (std::ostream& os)
const;
556 typedef Eigen::JacobiSVD <matrix_t>
SVD_t;
560 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
584 virtual void computeActiveRowsOfJ (std::size_t iStack);
593 void computeDescentDirection ()
const;
594 void expandDqSmall ()
const;
611 std::map <DifferentiableFunctionPtr_t, size_type>
iq_;
613 std::map <DifferentiableFunctionPtr_t, size_type>
iv_;
615 std::map <DifferentiableFunctionPtr_t, std::size_t>
priority_;
638 namespace lineSearch {
639 typedef ::hpp::constraints::solver::lineSearch::Constant Constant
641 typedef ::hpp::constraints::solver::lineSearch::Backtracking Backtracking
643 typedef ::hpp::constraints::solver::lineSearch::FixedSequence
645 typedef ::hpp::constraints::solver::lineSearch::ErrorNormBased
651 #endif // HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
bool lastIsOptional_
Definition: hierarchical-iterative.hh:604
value_type squaredErrorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:389
size_type maxIterations_
Definition: hierarchical-iterative.hh:599
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: hierarchical-iterative.hh:379
const value_type & sigma() const
Definition: hierarchical-iterative.hh:326
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:102
value_type computeLocalSlope(const SolverType &solver) const
Definition: hierarchical-iterative.hh:79
pinocchio::vector_t vector_t
Definition: fwd.hh:45
void inequalityThreshold(const value_type &it)
set the inequality threshold
Definition: hierarchical-iterative.hh:400
value_type b
Definition: hierarchical-iterative.hh:87
::hpp::constraints::solver::lineSearch::Constant Constant HPP_CONSTRAINTS_DEPRECATED
Definition: hierarchical-iterative.hh:640
ArrayXb tmpSat_
Definition: hierarchical-iterative.hh:624
std::map< DifferentiableFunctionPtr_t, std::size_t > priority_
Priority level of constraint.
Definition: hierarchical-iterative.hh:615
vector_t darg
Definition: hierarchical-iterative.hh:57
Definition: hierarchical-iterative.hh:79
LiegroupSpacePtr_t configSpace_
Definition: hierarchical-iterative.hh:602
matrix_t PK
Definition: hierarchical-iterative.hh:567
std::vector< ImplicitConstraintSet > stacks_
Definition: hierarchical-iterative.hh:601
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:54
value_type a
Definition: hierarchical-iterative.hh:87
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:93
lineSearch::FixedSequence DefaultLineSearch
Definition: hierarchical-iterative.hh:169
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:51
value_type alpha
Definition: hierarchical-iterative.hh:70
std::vector< std::size_t > inequalityIndices
Definition: hierarchical-iterative.hh:572
const LiegroupSpacePtr_t & configSpace() const
Get configuration space on which constraints are defined.
Definition: hierarchical-iterative.hh:202
pinocchio::matrix_t matrix_t
Definition: fwd.hh:42
const Saturation_t & saturation() const
Get the saturation function.
Definition: hierarchical-iterative.hh:256
value_type c
Definition: hierarchical-iterative.hh:56
value_type K
Definition: hierarchical-iterative.hh:71
Eigen::RowBlockIndices Indices_t
Definition: hierarchical-iterative.hh:168
No line search. Use .
Definition: hierarchical-iterative.hh:38
Definition: hierarchical-iterative.hh:558
vector_t OM_
Definition: hierarchical-iterative.hh:628
assert(d.lhs()._blocks()==d.rhs()._blocks())
Definition: implicit-constraint-set.hh:34
std::vector< segment_t > segments_t
Definition: fwd.hh:69
const Indices_t & freeVariables() const
Get free velocity variables.
Definition: hierarchical-iterative.hh:362
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:87
SVD_t svd
Definition: hierarchical-iterative.hh:566
void freeVariables(const Indices_t &indices)
Definition: hierarchical-iterative.hh:355
value_type alphaMax
Definition: hierarchical-iterative.hh:71
size_type reducedDimension_
Definition: hierarchical-iterative.hh:603
vector_t dqSmall_
Definition: hierarchical-iterative.hh:620
Eigen::RowBlockIndices equalityIndices
Definition: hierarchical-iterative.hh:573
void freeVariables(const segments_t intervals)
Definition: hierarchical-iterative.hh:342
Status
Definition: hierarchical-iterative.hh:171
vector_t arg_darg
Definition: hierarchical-iterative.hh:57
matrix_t reducedJ_
Definition: hierarchical-iterative.hh:621
const size_type & reducedDimension() const
Definition: hierarchical-iterative.hh:445
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:27
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:88
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:44
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:170
Eigen::VectorXi saturation_
Definition: hierarchical-iterative.hh:622
Eigen::MatrixBlocks< false, false > activeRowsOfJ
Definition: hierarchical-iterative.hh:574
Status solve(vectorOut_t arg) const
Definition: hierarchical-iterative.hh:297
value_type C
Definition: hierarchical-iterative.hh:87
Definition: active-set-differentiable-function.hh:24
bool lastIsOptional() const
Definition: hierarchical-iterative.hh:410
bool isSatisfied(vectorIn_t arg) const
Definition: hierarchical-iterative.hh:302
pinocchio::ArrayXb ArrayXb
Definition: fwd.hh:65
Eigen::JacobiSVD< matrix_t > SVD_t
Definition: hierarchical-iterative.hh:556
const NumericalConstraints_t & constraints() const
Get constraints (implicit and explicit)
Definition: hierarchical-iterative.hh:428
const ImplicitConstraintSet & constraints(const std::size_t priority)
Get set of constraints for a give priority level.
Definition: hierarchical-iterative.hh:421
vector_t error
Definition: hierarchical-iterative.hh:563
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:34
Configuration_t qSat_
Definition: hierarchical-iterative.hh:623
pinocchio::size_type size_type
Definition: fwd.hh:35
Definition: hierarchical-iterative.hh:165
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:384
matrix_t reducedJ
Definition: hierarchical-iterative.hh:564
Definition: hierarchical-iterative.hh:46
Saturation_t saturate_
Definition: hierarchical-iterative.hh:607
value_type tau
Definition: hierarchical-iterative.hh:56
virtual ~HierarchicalIterative()
Definition: hierarchical-iterative.hh:196
void lastIsOptional(bool optional)
Definition: hierarchical-iterative.hh:405
std::map< DifferentiableFunctionPtr_t, size_type > iq_
Value rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:611
pinocchio::value_type value_type
Definition: fwd.hh:36
value_type K
Definition: hierarchical-iterative.hh:87
value_type squaredNorm_
Definition: hierarchical-iterative.hh:625
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:47
value_type sigma_
The smallest non-zero singular value.
Definition: hierarchical-iterative.hh:618
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:626
void maxIterations(size_type iterations)
Set maximal number of iterations.
Definition: hierarchical-iterative.hh:368
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:159
void saturation(const Saturation_t &saturate)
Set the saturation function.
Definition: hierarchical-iterative.hh:250
value_type inequalityThreshold() const
Get the inequality threshold.
Definition: hierarchical-iterative.hh:395
vector_t df
Definition: hierarchical-iterative.hh:57
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:46
value_type smallAlpha
Definition: hierarchical-iterative.hh:56
boost::shared_ptr< DifferentiableFunction > DifferentiableFunctionPtr_t
Definition: fwd.hh:95
value_type residualError() const
Returns the squared norm of the error vector.
Definition: hierarchical-iterative.hh:459
Indices_t freeVariables_
Unknown of the set of implicit constraints.
Definition: hierarchical-iterative.hh:606
mutable ::hpp::statistics::SuccessStatistics statistics_
Definition: hierarchical-iterative.hh:632
std::size_t numberStacks() const
Definition: hierarchical-iterative.hh:433
SVD_t svd_
Definition: hierarchical-iterative.hh:627
boost::shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:157
@ MAX_ITERATION_REACHED
Definition: hierarchical-iterative.hh:173
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
vector_t OP_
Definition: hierarchical-iterative.hh:629
DifferentiableFunctionSet DifferentiableFunctionStack HPP_CONSTRAINTS_DEPRECATED
Definition: fwd.hh:99
boost::function< bool(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)> Saturation_t
Definition: hierarchical-iterative.hh:190
std::map< DifferentiableFunctionPtr_t, size_type > iv_
Derivative rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:613
const size_type & dimension() const
Definition: hierarchical-iterative.hh:438
const vector_t & lastStep() const
Accessor to the last step done.
Definition: hierarchical-iterative.hh:544
@ ERROR_INCREASED
Definition: hierarchical-iterative.hh:172
@ INFEASIBLE
Definition: hierarchical-iterative.hh:174
size_type maxRank
Definition: hierarchical-iterative.hh:569
LiegroupElement rightHandSide
Definition: hierarchical-iterative.hh:562
value_type squaredErrorThreshold_
Definition: hierarchical-iterative.hh:598
ComparisonTypes_t comparison
Definition: hierarchical-iterative.hh:571
size_type maxIterations() const
Get maximal number of iterations in config projector.
Definition: hierarchical-iterative.hh:373
ErrorNormBased(value_type alphaMin, value_type _a, value_type _b)
NumericalConstraints_t constraints_
Members moved from core::ConfigProjector.
Definition: hierarchical-iterative.hh:609
Definition: hierarchical-iterative.hh:63