17 #ifndef HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH 18 #define HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH 21 #include <boost/function.hpp> 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>
56 inline value_type computeLocalSlope(
const SolverType& solver)
const;
68 template <
typename SolverType>
85 template <
typename SolverType>
93 namespace saturation {
114 Eigen::VectorXi& saturation);
119 typedef boost::function<bool (vectorIn_t, vectorOut_t, Eigen::VectorXi&)>
function_t;
121 Eigen::VectorXi& saturation) {
122 return function(q, qSat, saturation);
125 Function (
const function_t&
function) : function(function) {}
131 Eigen::VectorXi& saturation);
140 Eigen::VectorXi& saturation);
252 virtual bool contains (
const ImplicitPtr_t& numericalConstraint)
const;
259 virtual bool add (
const ImplicitPtr_t& constraint,
const std::size_t& priority);
271 saturate_ = saturate;
299 template <
typename LineSearchType>
318 return solve (arg, DefaultLineSearch());
323 computeValue<false>(arg);
325 return squaredNorm_ < squaredErrorThreshold_;
339 bool& constraintFound)
const;
363 freeVariables_ = Indices_t();
364 for (std::size_t i = 0; i < intervals.size(); ++i)
365 freeVariables_.addRow (intervals[i].first, intervals[i].second);
366 freeVariables_.updateIndices<
true,
true,
true>();
376 freeVariables_ = indices;
383 return freeVariables_;
389 maxIterations_ = iterations;
394 return maxIterations_;
400 squaredErrorThreshold_ = threshold * threshold;
405 return sqrt (squaredErrorThreshold_);
410 return squaredErrorThreshold_;
416 return inequalityThreshold_;
421 inequalityThreshold_ = it;
426 lastIsOptional_ = optional;
431 return lastIsOptional_;
442 assert(priority < stacks_.size());
443 return stacks_[priority];
454 return stacks_.size();
466 return reducedDimension_;
470 ArrayXb activeParameters ()
const;
473 ArrayXb activeDerivativeParameters ()
const;
513 virtual bool rightHandSideFromConfig (
const ImplicitPtr_t& constraint,
554 template <
bool ComputeJac>
void computeValue (
vectorIn_t arg)
const;
555 void computeSaturation (
vectorIn_t arg)
const;
560 void computeError ()
const;
572 virtual std::ostream& print (std::ostream& os)
const;
575 typedef Eigen::JacobiSVD <matrix_t>
SVD_t;
579 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
603 virtual void computeActiveRowsOfJ (std::size_t iStack);
612 void computeDescentDirection ()
const;
613 void expandDqSmall ()
const;
630 std::map <DifferentiableFunctionPtr_t, size_type>
iq_;
632 std::map <DifferentiableFunctionPtr_t, size_type>
iv_;
634 std::map <DifferentiableFunctionPtr_t, std::size_t>
priority_;
655 HPP_SERIALIZABLE_SPLIT();
662 #endif // HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH pinocchio::vector_t vector_t
Definition: fwd.hh:47
Definition: hierarchical-iterative.hh:225
Device(const DevicePtr_t &device)
Definition: hierarchical-iterative.hh:142
vector_t ub
Definition: hierarchical-iterative.hh:134
pinocchio::vectorIn_t vectorIn_t
Definition: fwd.hh:48
Definition: implicit-constraint-set.hh:34
boost::function< bool(vectorIn_t, vectorOut_t, Eigen::VectorXi &)> function_t
Definition: hierarchical-iterative.hh:119
NumericalConstraints_t constraints_
Members moved from core::ConfigProjector.
Definition: hierarchical-iterative.hh:628
Definition: hierarchical-iterative.hh:48
void inequalityThreshold(const value_type &it)
set the inequality threshold
Definition: hierarchical-iterative.hh:419
bool lastIsOptional_
Definition: hierarchical-iterative.hh:623
std::map< DifferentiableFunctionPtr_t, std::size_t > priority_
Priority level of constraint.
Definition: hierarchical-iterative.hh:634
size_type maxIterations_
Definition: hierarchical-iterative.hh:618
void errorThreshold(const value_type &threshold)
Set error threshold.
Definition: hierarchical-iterative.hh:398
Definition: hierarchical-iterative.hh:81
std::vector< std::size_t > inequalityIndices
Definition: hierarchical-iterative.hh:591
Function()
Definition: hierarchical-iterative.hh:124
Eigen::RowBlockIndices equalityIndices
Definition: hierarchical-iterative.hh:592
Definition: active-set-differentiable-function.hh:24
pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t
Definition: fwd.hh:57
value_type K
Definition: hierarchical-iterative.hh:89
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:97
ArrayXb tmpSat_
Definition: hierarchical-iterative.hh:643
matrix_t reducedJ
Definition: hierarchical-iterative.hh:583
pinocchio::LiegroupElement LiegroupElement
Definition: fwd.hh:53
lineSearch::FixedSequence DefaultLineSearch
Definition: hierarchical-iterative.hh:222
LiegroupSpacePtr_t configSpace_
Definition: hierarchical-iterative.hh:621
SVD_t svd
Definition: hierarchical-iterative.hh:585
std::vector< ImplicitConstraintSet > stacks_
Definition: hierarchical-iterative.hh:620
std::vector< segment_t > segments_t
Definition: fwd.hh:72
simple box constraints
Definition: hierarchical-iterative.hh:129
pinocchio::matrix_t matrix_t
Definition: fwd.hh:44
Eigen::RowBlockIndices Indices_t
Definition: hierarchical-iterative.hh:221
pinocchio::ArrayXb ArrayXb
Definition: fwd.hh:68
value_type K
Definition: hierarchical-iterative.hh:73
size_type maxRank
Definition: hierarchical-iterative.hh:588
const LiegroupSpacePtr_t & configSpace() const
Get configuration space on which constraints are defined.
Definition: hierarchical-iterative.hh:242
Base class for box constraints. To prevent configuration variables to get out of joint limits during ...
Definition: hierarchical-iterative.hh:101
const Saturation_t & saturation() const
Get the saturation function.
Definition: hierarchical-iterative.hh:275
vector_t dqSmall_
Definition: hierarchical-iterative.hh:639
No line search. Use .
Definition: hierarchical-iterative.hh:40
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:93
Status
Definition: hierarchical-iterative.hh:224
Definition: hierarchical-iterative.hh:65
vector_t OM_
Definition: hierarchical-iterative.hh:647
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:94
std::vector< ComparisonType > ComparisonTypes_t
Definition: fwd.hh:176
const size_type & reducedDimension() const
Definition: hierarchical-iterative.hh:464
value_type alpha
Definition: hierarchical-iterative.hh:72
assert(d.lhs()._blocks()==d.rhs()._blocks())
const Indices_t & freeVariables() const
Get free velocity variables.
Definition: hierarchical-iterative.hh:381
Bounds(const vector_t &lb, const vector_t &ub)
Definition: hierarchical-iterative.hh:133
Status solve(vectorOut_t arg) const
Definition: hierarchical-iterative.hh:316
void freeVariables(const Indices_t &indices)
Definition: hierarchical-iterative.hh:374
size_type reducedDimension_
Definition: hierarchical-iterative.hh:622
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:30
void freeVariables(const segments_t intervals)
Definition: hierarchical-iterative.hh:361
bool isSatisfied(vectorIn_t arg) const
Definition: hierarchical-iterative.hh:321
const NumericalConstraints_t & constraints() const
Get constraints (implicit and explicit)
Definition: hierarchical-iterative.hh:447
const ImplicitConstraintSet & constraints(const std::size_t priority)
Get set of constraints for a give priority level.
Definition: hierarchical-iterative.hh:440
Configuration_t qSat_
Definition: hierarchical-iterative.hh:642
matrix_t reducedJ_
Definition: hierarchical-iterative.hh:640
value_type errorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:403
Device()
Definition: hierarchical-iterative.hh:141
Eigen::VectorXi saturation_
Definition: hierarchical-iterative.hh:641
Bounds()
Definition: hierarchical-iterative.hh:132
Saturation_t saturate_
Definition: hierarchical-iterative.hh:626
bool lastIsOptional() const
Definition: hierarchical-iterative.hh:429
std::map< DifferentiableFunctionPtr_t, size_type > iq_
Value rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:630
bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi &saturation)
Definition: hierarchical-iterative.hh:120
Eigen::JacobiSVD< matrix_t > SVD_t
Definition: hierarchical-iterative.hh:575
Function(const function_t &function)
Definition: hierarchical-iterative.hh:125
saturation from a boost::function.
Definition: hierarchical-iterative.hh:118
value_type squaredNorm_
Definition: hierarchical-iterative.hh:644
value_type tau
Definition: hierarchical-iterative.hh:58
virtual ~Base()
Definition: hierarchical-iterative.hh:115
value_type inequalityThreshold() const
Get the inequality threshold.
Definition: hierarchical-iterative.hh:414
ComparisonTypes_t comparison
Definition: hierarchical-iterative.hh:590
vector_t df
Definition: hierarchical-iterative.hh:59
void lastIsOptional(bool optional)
Definition: hierarchical-iterative.hh:424
virtual ~HierarchicalIterative()
Definition: hierarchical-iterative.hh:236
Definition: hierarchical-iterative.hh:577
boost::shared_ptr< saturation::Base > Saturation_t
Definition: hierarchical-iterative.hh:230
#define HPP_CONSTRAINTS_DLLAPI
Definition: config.hh:64
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
Indices_t freeVariables_
Unknown of the set of implicit constraints.
Definition: hierarchical-iterative.hh:625
value_type sigma_
The smallest non-zero singular value.
Definition: hierarchical-iterative.hh:637
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:645
void maxIterations(size_type iterations)
Set maximal number of iterations.
Definition: hierarchical-iterative.hh:387
void saturation(const Saturation_t &saturate)
Set the saturation function.
Definition: hierarchical-iterative.hh:269
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::Ref< matrix_t > matrixOut_t
Definition: fwd.hh:46
vector_t error
Definition: hierarchical-iterative.hh:582
HierarchicalIterative()
Definition: hierarchical-iterative.hh:653
Definition: hierarchical-iterative.hh:227
LiegroupElement rightHandSide
Definition: hierarchical-iterative.hh:581
SVD_t svd_
Definition: hierarchical-iterative.hh:646
Box constraints use a Device joint limits.
Definition: hierarchical-iterative.hh:137
vector_t OP_
Definition: hierarchical-iterative.hh:648
pinocchio::value_type value_type
Definition: fwd.hh:37
value_type residualError() const
Returns the squared norm of the error vector.
Definition: hierarchical-iterative.hh:478
const size_type & dimension() const
Definition: hierarchical-iterative.hh:457
Definition: hierarchical-iterative.hh:226
Eigen::MatrixBlocks< false, false > activeRowsOfJ
Definition: hierarchical-iterative.hh:593
matrix_t PK
Definition: hierarchical-iterative.hh:586
value_type squaredErrorThreshold_
Definition: hierarchical-iterative.hh:617
boost::shared_ptr< Implicit > ImplicitPtr_t
Definition: fwd.hh:163
std::size_t numberStacks() const
Definition: hierarchical-iterative.hh:452
DevicePtr_t device
Definition: hierarchical-iterative.hh:143
Definition: hierarchical-iterative.hh:218
size_type maxIterations() const
Get maximal number of iterations in config projector.
Definition: hierarchical-iterative.hh:392
std::map< DifferentiableFunctionPtr_t, size_type > iv_
Derivative rank of constraint in its priority level.
Definition: hierarchical-iterative.hh:632
const vector_t & lastStep() const
Accessor to the last step done.
Definition: hierarchical-iterative.hh:563
std::vector< constraints::ImplicitPtr_t > NumericalConstraints_t
Definition: fwd.hh:165
value_type squaredErrorThreshold() const
Get error threshold.
Definition: hierarchical-iterative.hh:408
const value_type & sigma() const
Definition: hierarchical-iterative.hh:345