hpp-constraints  4.12.0
Definition of basic geometric constraints for motion planning
hierarchical-iterative.hh
Go to the documentation of this file.
1 // Copyright (c) 2017, Joseph Mirabel
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 // This file is part of hpp-constraints.
5 // hpp-constraints is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-constraints is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-constraints. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
18 #define HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
19 
20 #include <hpp/util/debug.hh>
21 
23 #include <hpp/constraints/svd.hh>
24 
25 namespace hpp {
26  namespace constraints {
27  namespace solver {
28  namespace lineSearch {
29  template <typename SolverType>
30  inline bool Constant::operator() (const SolverType& solver, vectorOut_t arg, vectorOut_t darg)
31  {
32  solver.integrate (arg, darg, arg);
33  return true;
34  }
35 
36  template <typename SolverType>
37  inline bool Backtracking::operator() (const SolverType& solver, vectorOut_t arg, vectorOut_t u)
38  {
39  arg_darg.resize(arg.size());
40 
41  const value_type slope = computeLocalSlope(solver);
42  const value_type t = 2 * c * slope;
43  const value_type f_arg_norm2 = solver.residualError();
44 
45  if (t > 0) {
46  hppDout (error, "The descent direction is not valid: " << t/c);
47  } else {
48  value_type alpha = 1;
49  /* TODO add a threshold to avoid too large steps.
50  const value_type u2 = u.squaredNorm();
51  if (u2 > 1.) alpha = 1. / std::sqrt(u2);
52  */
53 
54  while (alpha > smallAlpha) {
55  darg = alpha * u;
56  solver.integrate (arg, darg, arg_darg);
57  solver.template computeValue<false> (arg_darg);
58  solver.computeError ();
59  // Check if we are doing better than the linear approximation with coef
60  // multiplied by c < 1
61  // t < 0 must hold
62  const value_type f_arg_darg_norm2 = solver.residualError();
63  if (f_arg_norm2 - f_arg_darg_norm2 >= - alpha * t) {
64  arg = arg_darg;
65  u = darg;
66  return true;
67  }
68  // Prepare next step
69  alpha *= tau;
70  }
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");
74  }
75 
76  u *= smallAlpha;
77  solver.integrate (arg, u, arg);
78  return false;
79  }
80 
81  template <typename SolverType>
82  inline value_type Backtracking::computeLocalSlope(const SolverType& solver) const
83  {
84  value_type slope = 0;
85  for (std::size_t i = 0; i < solver.stacks_.size (); ++i) {
86  typename SolverType::Data& d = solver.datas_[i];
87  const size_type nrows = d.reducedJ.rows();
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());
91  }
92  return slope;
93  }
94 
95  template <typename SolverType>
96  inline bool FixedSequence::operator() (const SolverType& solver, vectorOut_t arg, vectorOut_t darg)
97  {
98  darg *= alpha;
99  alpha = alphaMax - K * (alphaMax - alpha);
100  solver.integrate (arg, darg, arg);
101  return true;
102  }
103 
104  template <typename SolverType>
105  inline bool ErrorNormBased::operator() (const SolverType& solver, vectorOut_t arg, vectorOut_t darg)
106  {
107  const value_type r = solver.residualError() / solver.squaredErrorThreshold();
108  const value_type alpha = C - K * std::tanh(a * r + b);
109  darg *= alpha;
110  solver.integrate (arg, darg, arg);
111  return true;
112  }
113  }
114 
115  template <typename LineSearchType>
117  vectorOut_t arg,
118  LineSearchType lineSearch) const
119  {
120  hppDout (info, "before projection: " << arg.transpose ());
121  assert (!arg.hasNaN());
122 
123  size_type errorDecreased = 3, iter = 0;
124  value_type previousSquaredNorm =
125  std::numeric_limits<value_type>::infinity();
126  static const value_type dqMinSquaredNorm = Eigen::NumTraits<value_type>::dummy_precision();
127 
128  // Fill value and Jacobian
129  computeValue<true> (arg);
130  computeError();
131 
132  if (squaredNorm_ > squaredErrorThreshold_
133  && reducedDimension_ == 0) return INFEASIBLE;
134 
135  Status status;
136  while (squaredNorm_ > squaredErrorThreshold_ && errorDecreased &&
137  iter < maxIterations_) {
138 
139  computeSaturation(arg);
140  computeDescentDirection ();
141  if (dq_.squaredNorm () < dqMinSquaredNorm) {
142  // TODO INFEASIBLE means that we have reached a local minima.
143  // The problem may still be feasible from a different starting point.
144  status = INFEASIBLE;
145  break;
146  }
147  lineSearch (*this, arg, dq_);
148 
149  computeValue<true> (arg);
150  computeError ();
151 
152  hppDout (info, "squareNorm = " << squaredNorm_);
153  --errorDecreased;
154  if (squaredNorm_ < previousSquaredNorm)
155  errorDecreased = 3;
156  else
157  status = ERROR_INCREASED;
158  previousSquaredNorm = squaredNorm_;
159  ++iter;
160 
161  }
162 
163  hppDout (info, "number of iterations: " << iter);
164  if (squaredNorm_ > squaredErrorThreshold_) {
165  hppDout (info, "Projection failed.");
166  return (iter >= maxIterations_) ? MAX_ITERATION_REACHED : status;
167  }
168  hppDout (info, "After projection: " << arg.transpose ());
169  assert (!arg.hasNaN());
170  return SUCCESS;
171  }
172  } // namespace solver
173  } // namespace constraints
174 } // namespace hpp
175 
176 #endif //HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
value_type computeLocalSlope(const SolverType &solver) const
Definition: hierarchical-iterative.hh:82
const Derived & d
Definition: matrix-view-operation.hh:126
Definition: active-set-differentiable-function.hh:24
Status
Definition: hierarchical-iterative.hh:224
assert(d.lhs()._blocks()==d.rhs()._blocks())
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition: hierarchical-iterative.hh:96
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
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
pinocchio::size_type size_type
Definition: fwd.hh:36
pinocchio::value_type value_type
Definition: fwd.hh:37