17 #ifndef HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
18 #define HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
23 namespace constraints {
25 namespace lineSearch {
26 template <
typename SolverType>
29 solver.integrate (arg, darg, arg);
33 template <
typename SolverType>
40 const value_type f_arg_norm2 = solver.residualError();
43 hppDout (error,
"The descent direction is not valid: " << t/
c);
54 solver.template computeValue<false> (
arg_darg);
55 solver.computeError ();
59 const value_type f_arg_darg_norm2 = solver.residualError();
60 if (f_arg_norm2 - f_arg_darg_norm2 >= - alpha * t) {
68 hppDout (error,
"Could find alpha such that ||f(q)||**2 + "
69 <<
c <<
" * 2*(f(q)^T * J * dq) is doing worse than "
70 "||f(q + alpha * dq)||**2");
74 solver.integrate (arg, u, arg);
78 template <
typename SolverType>
82 for (std::size_t i = 0; i < solver.stacks_.size (); ++i) {
83 typename SolverType::Data&
d = solver.datas_[i];
85 if (
df.size() < nrows)
df.resize(nrows);
86 df.head(nrows).noalias() =
d.reducedJ * solver.dqSmall_;
87 slope +=
df.head(nrows).dot(
d.activeRowsOfJ.keepRows().rview(
d.error).eval());
92 template <
typename SolverType>
97 solver.integrate (arg, darg, arg);
101 template <
typename SolverType>
104 const value_type r = solver.residualError() / solver.squaredErrorThreshold();
107 solver.integrate (arg, darg, arg);
112 template <
typename LineSearchType>
115 LineSearchType lineSearch)
const
117 hppDout (info,
"before projection: " << arg.transpose ());
122 std::numeric_limits<value_type>::infinity();
123 static const value_type dqMinSquaredNorm = Eigen::NumTraits<value_type>::dummy_precision();
126 computeValue<true> (arg);
129 if (squaredNorm_ > squaredErrorThreshold_
130 && reducedDimension_ == 0)
return INFEASIBLE;
133 while (squaredNorm_ > squaredErrorThreshold_ && errorDecreased &&
134 iter < maxIterations_) {
136 computeSaturation(arg);
137 computeDescentDirection ();
138 if (dq_.squaredNorm () < dqMinSquaredNorm) {
144 lineSearch (*
this, arg, dq_);
146 computeValue<true> (arg);
149 hppDout (info,
"squareNorm = " << squaredNorm_);
151 if (squaredNorm_ < previousSquaredNorm)
154 status = ERROR_INCREASED;
155 previousSquaredNorm = squaredNorm_;
160 hppDout (info,
"number of iterations: " << iter);
161 if (squaredNorm_ > squaredErrorThreshold_) {
162 hppDout (info,
"Projection failed.");
163 return (iter >= maxIterations_) ? MAX_ITERATION_REACHED : status;
165 hppDout (info,
"After projection: " << arg.transpose ());
173 #endif //HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH