17 #ifndef HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH 18 #define HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH 20 #include <hpp/util/debug.hh> 26 namespace constraints {
28 namespace lineSearch {
29 template <
typename SolverType>
32 solver.integrate (arg, darg, arg);
36 template <
typename SolverType>
43 const value_type f_arg_norm2 = solver.residualError();
46 hppDout (error,
"The descent direction is not valid: " << t/
c);
57 solver.template computeValue<false> (
arg_darg);
58 solver.computeError ();
62 const value_type f_arg_darg_norm2 = solver.residualError();
63 if (f_arg_norm2 - f_arg_darg_norm2 >= - alpha * t) {
71 hppDout (error,
"Could find alpha such that ||f(q)||**2 + " 72 <<
c <<
" * 2*(f(q)^T * J * dq) is doing worse than " 73 "||f(q + alpha * dq)||**2");
77 solver.integrate (arg, u, arg);
81 template <
typename SolverType>
85 for (std::size_t i = 0; i < solver.stacks_.size (); ++i) {
86 typename SolverType::Data&
d = solver.datas_[i];
88 if (
df.size() < nrows)
df.resize(nrows);
89 df.head(nrows).noalias() =
d.reducedJ * solver.dqSmall_;
90 slope +=
df.head(nrows).dot(
d.activeRowsOfJ.keepRows().rview(
d.error).eval());
95 template <
typename SolverType>
100 solver.integrate (arg, darg, arg);
104 template <
typename SolverType>
107 const value_type r = solver.residualError() / solver.squaredErrorThreshold();
110 solver.integrate (arg, darg, arg);
115 template <
typename LineSearchType>
118 LineSearchType lineSearch)
const 120 hppDout (info,
"before projection: " << arg.transpose ());
125 std::numeric_limits<value_type>::infinity();
126 static const value_type dqMinSquaredNorm = Eigen::NumTraits<value_type>::dummy_precision();
129 computeValue<true> (arg);
132 if (squaredNorm_ > squaredErrorThreshold_
133 && reducedDimension_ == 0)
return INFEASIBLE;
136 while (squaredNorm_ > squaredErrorThreshold_ && errorDecreased &&
137 iter < maxIterations_) {
139 computeSaturation(arg);
140 computeDescentDirection ();
141 if (dq_.squaredNorm () < dqMinSquaredNorm) {
147 lineSearch (*
this, arg, dq_);
149 computeValue<true> (arg);
152 hppDout (info,
"squareNorm = " << squaredNorm_);
154 if (squaredNorm_ < previousSquaredNorm)
157 status = ERROR_INCREASED;
158 previousSquaredNorm = squaredNorm_;
163 hppDout (info,
"number of iterations: " << iter);
164 if (squaredNorm_ > squaredErrorThreshold_) {
165 hppDout (info,
"Projection failed.");
166 return (iter >= maxIterations_) ? MAX_ITERATION_REACHED : status;
168 hppDout (info,
"After projection: " << arg.transpose ());
176 #endif //HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH value_type computeLocalSlope(const SolverType &solver) const
Definition: hierarchical-iterative.hh:82
vector_t darg
Definition: hierarchical-iterative.hh:59
const Derived & d
Definition: matrix-view-operation.hh:126
value_type C
Definition: hierarchical-iterative.hh:89
Definition: active-set-differentiable-function.hh:24
value_type K
Definition: hierarchical-iterative.hh:89
value_type a
Definition: hierarchical-iterative.hh:89
value_type c
Definition: hierarchical-iterative.hh:58
value_type b
Definition: hierarchical-iterative.hh:89
value_type K
Definition: hierarchical-iterative.hh:73
Status
Definition: hierarchical-iterative.hh:224
value_type alpha
Definition: hierarchical-iterative.hh:72
assert(d.lhs()._blocks()==d.rhs()._blocks())
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:96
vector_t arg_darg
Definition: hierarchical-iterative.hh:59
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:30
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:105
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:37
value_type tau
Definition: hierarchical-iterative.hh:58
vector_t df
Definition: hierarchical-iterative.hh:59
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
value_type smallAlpha
Definition: hierarchical-iterative.hh:58
pinocchio::size_type size_type
Definition: fwd.hh:36
pinocchio::value_type value_type
Definition: fwd.hh:37
value_type alphaMax
Definition: hierarchical-iterative.hh:73