hpp-constraints 6.0.0
Definition of basic geometric constraints for motion planning
Loading...
Searching...
No Matches
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
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are
7// met:
8//
9// 1. Redistributions of source code must retain the above copyright
10// notice, this list of conditions and the following disclaimer.
11//
12// 2. Redistributions in binary form must reproduce the above copyright
13// notice, this list of conditions and the following disclaimer in the
14// documentation and/or other materials provided with the distribution.
15//
16// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27// DAMAGE.
28
29#ifndef HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
30#define HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
31
34#include <hpp/util/debug.hh>
35
36namespace hpp {
37namespace constraints {
38namespace solver {
39namespace lineSearch {
40template <typename SolverType>
41inline bool Constant::operator()(const SolverType& solver, vectorOut_t arg,
42 vectorOut_t darg) {
43 solver.integrate(arg, darg, arg);
44 return true;
45}
46
47template <typename SolverType>
48inline bool Backtracking::operator()(const SolverType& solver, vectorOut_t arg,
49 vectorOut_t u) {
50 arg_darg.resize(arg.size());
51
52 const value_type slope = computeLocalSlope(solver);
53 const value_type t = 2 * c * slope;
54 const value_type f_arg_norm2 = solver.residualError();
55
56 if (t > 0) {
57 hppDout(error, "The descent direction is not valid: " << t / c);
58 } else {
59 value_type alpha = 1;
60 /* TODO add a threshold to avoid too large steps.
61 const value_type u2 = u.squaredNorm();
62 if (u2 > 1.) alpha = 1. / std::sqrt(u2);
63 */
64
65 while (alpha > smallAlpha) {
66 darg = alpha * u;
67 solver.integrate(arg, darg, arg_darg);
68 solver.template computeValue<false>(arg_darg);
69 solver.computeError();
70 // Check if we are doing better than the linear approximation with coef
71 // multiplied by c < 1
72 // t < 0 must hold
73 const value_type f_arg_darg_norm2 = solver.residualError();
74 if (f_arg_norm2 - f_arg_darg_norm2 >= -alpha * t) {
75 arg = arg_darg;
76 u = darg;
77 return true;
78 }
79 // Prepare next step
80 alpha *= tau;
81 }
82 hppDout(error, "Could find alpha such that ||f(q)||**2 + "
83 << c
84 << " * 2*(f(q)^T * J * dq) is doing worse than "
85 "||f(q + alpha * dq)||**2");
86 }
87
88 u *= smallAlpha;
89 solver.integrate(arg, u, arg);
90 return false;
91}
92
93template <typename SolverType>
95 const SolverType& solver) const {
96 value_type slope = 0;
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_;
102 slope +=
103 df.head(nrows).dot(d.activeRowsOfJ.keepRows().rview(d.error).eval());
104 }
105 return slope;
106}
107
108template <typename SolverType>
109inline bool FixedSequence::operator()(const SolverType& solver, vectorOut_t arg,
110 vectorOut_t darg) {
111 darg *= alpha;
112 alpha = alphaMax - K * (alphaMax - alpha);
113 solver.integrate(arg, darg, arg);
114 return true;
115}
116
117template <typename SolverType>
118inline bool ErrorNormBased::operator()(const SolverType& solver,
119 vectorOut_t arg, vectorOut_t darg) {
120 const value_type r = solver.residualError() / solver.squaredErrorThreshold();
121 const value_type alpha = C - K * std::tanh(a * r + b);
122 darg *= alpha;
123 solver.integrate(arg, darg, arg);
124 return true;
125}
126} // namespace lineSearch
127
128template <typename LineSearchType>
131 LineSearchType lineSearch) const {
132 hppDout(info, "before projection: " << arg.transpose());
133 assert(!arg.hasNaN());
134
135 size_type errorDecreased = 3, iter = 0;
136 value_type previousSquaredNorm = std::numeric_limits<value_type>::infinity();
137 static const value_type dqMinSquaredNorm =
138 Eigen::NumTraits<value_type>::dummy_precision();
139
140 // Fill value and Jacobian
141 computeValue<true>(arg);
142 computeError();
143
144 if (squaredNorm_ > squaredErrorThreshold_ && reducedDimension_ == 0)
145 return INFEASIBLE;
146
147 Status status;
148 while (squaredNorm_ > squaredErrorThreshold_ && errorDecreased &&
149 iter < maxIterations_) {
150 computeSaturation(arg);
151 computeDescentDirection();
152 if (dq_.squaredNorm() < dqMinSquaredNorm) {
153 // TODO INFEASIBLE means that we have reached a local minima.
154 // The problem may still be feasible from a different starting point.
155 status = INFEASIBLE;
156 break;
157 }
158 lineSearch(*this, arg, dq_);
159
160 computeValue<true>(arg);
161 computeError();
162
163 hppDout(info, "squareNorm = " << squaredNorm_);
164 --errorDecreased;
165 if (squaredNorm_ < previousSquaredNorm)
166 errorDecreased = 3;
167 else
168 status = ERROR_INCREASED;
169 previousSquaredNorm = squaredNorm_;
170 ++iter;
171 }
172
173 hppDout(info, "number of iterations: " << iter);
174 if (squaredNorm_ > squaredErrorThreshold_) {
175 hppDout(info, "Projection failed.");
176 return (iter >= maxIterations_) ? MAX_ITERATION_REACHED : status;
177 }
178 hppDout(info, "After projection: " << arg.transpose());
179 assert(!arg.hasNaN());
180 return SUCCESS;
181}
182} // namespace solver
183} // namespace constraints
184} // namespace hpp
185
186#endif // HPP_CONSTRAINTS_SOLVER_IMPL_HIERARCHICAL_ITERATIVE_HH
Status solve(vectorOut_t arg, LineSearchType ls=LineSearchType()) const
Status
Definition hierarchical-iterative.hh:245
pinocchio::size_type size_type
Definition fwd.hh:47
pinocchio::value_type value_type
Definition fwd.hh:48
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:61
Definition active-set-differentiable-function.hh:36
vector_t df
Definition hierarchical-iterative.hh:64
vector_t arg_darg
Definition hierarchical-iterative.hh:64
value_type smallAlpha
Definition hierarchical-iterative.hh:63
value_type tau
Definition hierarchical-iterative.hh:63
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition hierarchical-iterative.hh:48
value_type computeLocalSlope(const SolverType &solver) const
Definition hierarchical-iterative.hh:94
value_type c
Definition hierarchical-iterative.hh:63
vector_t darg
Definition hierarchical-iterative.hh:64
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition hierarchical-iterative.hh:41
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition hierarchical-iterative.hh:118
value_type b
Definition hierarchical-iterative.hh:92
value_type K
Definition hierarchical-iterative.hh:92
value_type a
Definition hierarchical-iterative.hh:92
value_type C
Definition hierarchical-iterative.hh:92
value_type alphaMax
Definition hierarchical-iterative.hh:77
bool operator()(const SolverType &solver, vectorOut_t arg, vectorOut_t darg)
Definition hierarchical-iterative.hh:109
value_type K
Definition hierarchical-iterative.hh:77
value_type alpha
Definition hierarchical-iterative.hh:76