hpp-constraints  4.12.0
Definition of basic geometric constraints for motion planning
by-substitution.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_BY_SUBSTITUTION_HH
18 #define HPP_CONSTRAINTS_SOLVER_IMPL_BY_SUBSTITUTION_HH
19 
20 namespace hpp {
21  namespace constraints {
22  namespace solver {
23  typedef std::numeric_limits<value_type> numeric_limits;
24  typedef Eigen::NumTraits<value_type> NumTraits;
25 
26  template <typename LineSearchType>
27  inline HierarchicalIterative::Status BySubstitution::impl_solve (
28  vectorOut_t arg,
29  bool _optimize,
30  LineSearchType lineSearch) const
31  {
32  bool optimize = _optimize && lastIsOptional_;
33  assert (!arg.hasNaN());
34 
35  explicit_.solve(arg);
36  assert (!arg.hasNaN());
37 
38  size_type errorDecreased = 3, iter = 0;
39  value_type previousSquaredNorm;
40  static const value_type dqMinSquaredNorm = NumTraits::dummy_precision();
41  value_type initSquaredNorm = 0;
42 
43  // Variables for optimization only
44  value_type previousCost = 0;
45  value_type scaling = 1.;
46  bool onlyLineSearch = false;
47  vector_t qopt;
48 
49  // Fill value and Jacobian
50  computeValue<true> (arg);
51  computeError();
52  if (optimize)
53  previousCost = datas_.back().error.squaredNorm();
54 
55  bool errorWasBelowThr = (squaredNorm_ < squaredErrorThreshold_);
56  vector_t initArg;
57  if (errorWasBelowThr) {
58  initArg = arg;
59  if (!optimize) iter = std::max (maxIterations_,size_type(2)) - 2;
60  initSquaredNorm = squaredNorm_;
61  }
62 
63  bool errorIsAboveThr = (squaredNorm_ > .25 * squaredErrorThreshold_);
64  if (errorIsAboveThr && reducedDimension_ == 0) return INFEASIBLE;
65  if (optimize && !errorIsAboveThr) qopt = arg;
66 
67  Status status = SUCCESS;
68  while ( (optimize || (errorIsAboveThr && errorDecreased))) {
69  // 1. Maximum iterations
70  if (iter >= maxIterations_) {
71  status = MAX_ITERATION_REACHED;
72  break;
73  }
74  status = SUCCESS;
75 
76  // 2. Compute step
77  // onlyLineSearch is true when we only reduced the scaling.
78  if (!onlyLineSearch) {
79  previousSquaredNorm = squaredNorm_;
80  // Update the jacobian using the jacobian of the explicit system.
81  updateJacobian(arg);
82  computeSaturation(arg);
84  }
85  // Apply scaling to avoid too large steps.
86  if (optimize) dq_ *= scaling;
87  if (dq_.squaredNorm () < dqMinSquaredNorm) {
88  // We assume that the algorithm reached a local minima.
89  status = INFEASIBLE;
90  break;
91  }
92  // 3. Apply line search algorithm for the computed step
93  lineSearch (*this, arg, dq_);
94  explicit_.solve(arg);
95  assert (!arg.hasNaN());
96 
97  // 4. Evaluate the error at the new point.
98  computeValue<true> (arg);
99  computeError ();
100 
101  --errorDecreased;
102  if (squaredNorm_ < previousSquaredNorm)
103  errorDecreased = 3;
104  else
105  status = ERROR_INCREASED;
106 
107  errorIsAboveThr = (squaredNorm_ > .25 * squaredErrorThreshold_);
108  // 5. In case of optimization,
109  // - if the constraints is satisfied and the cost decreased, increase
110  // the scaling (amount of confidence in the linear approximation)
111  // - if the constraints is not satisfied, decrease the scaling and
112  // and cancel this step.
113  if (optimize) {
114  if (!errorIsAboveThr) {
115  value_type cost = datas_.back().error.squaredNorm();
116  if (cost < previousCost) {
117  qopt = arg;
118  previousCost = cost;
119  if (scaling < 0.5) scaling *= 2;
120  }
121  onlyLineSearch = false;
122  } else {
123  dq_ /= scaling;
124  scaling *= 0.5;
125  if (qopt.size() > 0) arg = qopt;
126  onlyLineSearch = true;
127  }
128  }
129 
130  ++iter;
131  }
132 
133  if (!optimize && errorWasBelowThr) {
134  if (squaredNorm_ > initSquaredNorm) {
135  arg = initArg;
136  }
137  return SUCCESS;
138  }
139  // If optimizing, qopt is the visited configuration that satisfies the
140  // constraints and has lowest cost.
141  if (optimize && qopt.size() > 0) arg = qopt;
142 
143  assert (!arg.hasNaN());
144  return status;
145  }
146  } // namespace solver
147  } // namespace constraints
148 } // namespace hpp
149 
150 #endif // HPP_CONSTRAINTS_SOLVER_IMPL_BY_SUBSTITUTION_HH
pinocchio::vector_t vector_t
Definition: fwd.hh:47
bool lastIsOptional_
Definition: hierarchical-iterative.hh:637
size_type maxIterations_
Definition: hierarchical-iterative.hh:632
Definition: active-set-differentiable-function.hh:24
void updateJacobian(vectorIn_t arg) const
Status
Definition: hierarchical-iterative.hh:224
assert(d.lhs()._blocks()==d.rhs()._blocks())
Definition: hierarchical-iterative.hh:228
bool solve(vectorOut_t arg) const
size_type reducedDimension_
Definition: hierarchical-iterative.hh:636
std::numeric_limits< value_type > numeric_limits
Definition: by-substitution.hh:23
value_type squaredNorm_
Definition: hierarchical-iterative.hh:658
void computeSaturation(vectorIn_t arg) const
pinocchio::vectorOut_t vectorOut_t
Definition: fwd.hh:49
std::vector< Data > datas_
Definition: hierarchical-iterative.hh:659
pinocchio::size_type size_type
Definition: fwd.hh:36
Eigen::NumTraits< value_type > NumTraits
Definition: by-substitution.hh:24
Definition: hierarchical-iterative.hh:227
pinocchio::value_type value_type
Definition: fwd.hh:37
value_type squaredErrorThreshold_
Definition: hierarchical-iterative.hh:631
vector_t dq_
Definition: hierarchical-iterative.hh:653