29 #ifndef HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH 30 #define HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH 34 #include <hpp/util/debug.hh> 37 namespace constraints {
39 namespace lineSearch {
40 template <
typename SolverType>
43 solver.integrate(arg, darg, arg);
47 template <
typename SolverType>
50 arg_darg.resize(arg.size());
52 const value_type slope = computeLocalSlope(solver);
54 const value_type f_arg_norm2 = solver.residualError();
57 hppDout(error,
"The descent direction is not valid: " << t / c);
65 while (alpha > smallAlpha) {
67 solver.integrate(arg, darg, arg_darg);
68 solver.template computeValue<false>(arg_darg);
69 solver.computeError();
73 const value_type f_arg_darg_norm2 = solver.residualError();
74 if (f_arg_norm2 - f_arg_darg_norm2 >= -alpha * t) {
82 hppDout(error,
"Could find alpha such that ||f(q)||**2 + " 84 <<
" * 2*(f(q)^T * J * dq) is doing worse than " 85 "||f(q + alpha * dq)||**2");
89 solver.integrate(arg, u, arg);
93 template <
typename SolverType>
95 const SolverType& solver)
const {
97 for (std::size_t i = 0; i < solver.stacks_.size(); ++i) {
98 typename SolverType::Data&
d = solver.datas_[i];
99 const size_type nrows = d.reducedJ.rows();
100 if (df.size() < nrows) df.resize(nrows);
101 df.head(nrows).noalias() = d.reducedJ * solver.dqSmall_;
103 df.head(nrows).dot(d.activeRowsOfJ.keepRows().rview(d.error).eval());
108 template <
typename SolverType>
112 alpha = alphaMax - K * (alphaMax - alpha);
113 solver.integrate(arg, darg, arg);
117 template <
typename SolverType>
120 const value_type r = solver.residualError() / solver.squaredErrorThreshold();
121 const value_type alpha = C - K * std::tanh(a * r + b);
123 solver.integrate(arg, darg, arg);
128 template <
typename LineSearchType>
131 LineSearchType lineSearch)
const {
132 hppDout(info,
"before projection: " << arg.transpose());
136 value_type previousSquaredNorm = std::numeric_limits<value_type>::infinity();
138 Eigen::NumTraits<value_type>::dummy_precision();
141 computeValue<true>(arg);
144 if (squaredNorm_ > squaredErrorThreshold_ && reducedDimension_ == 0)
148 while (squaredNorm_ > squaredErrorThreshold_ && errorDecreased &&
149 iter < maxIterations_) {
150 computeSaturation(arg);
151 computeDescentDirection();
152 if (dq_.squaredNorm() < dqMinSquaredNorm) {
158 lineSearch(*
this, arg, dq_);
160 computeValue<true>(arg);
163 hppDout(info,
"squareNorm = " << squaredNorm_);
165 if (squaredNorm_ < previousSquaredNorm)
168 status = ERROR_INCREASED;
169 previousSquaredNorm = squaredNorm_;
173 hppDout(info,
"number of iterations: " << iter);
174 if (squaredNorm_ > squaredErrorThreshold_) {
175 hppDout(info,
"Projection failed.");
176 return (iter >= maxIterations_) ? MAX_ITERATION_REACHED : status;
178 hppDout(info,
"After projection: " << arg.transpose());
186 #endif // HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH value_type computeLocalSlope(const SolverType &solver) const
Definition: hierarchical-iterative.hh:94
const Derived & d
Definition: matrix-view-operation.hh:138
Definition: active-set-differentiable-function.hh:36
Status
Definition: hierarchical-iterative.hh:237
assert(d.lhs()._blocks()==d.rhs()._blocks())
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:109
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:41
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:118
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:48
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:61
pinocchio::size_type size_type
Definition: fwd.hh:47
pinocchio::value_type value_type
Definition: fwd.hh:48