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