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