hpp-constraints: include/hpp/constraints/solver/impl/by-substitution.hh Source File

hpp-constraints 9.0.2 Definition of basic geometric constraints for motion planning

Loading...
Searching...
No Matches
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 // An explicit constraint may be defined over only a subspace of its input
45 // space. If the input value is outside the definition interval, the explicit
46 // constraint throws an exception of type FunctionNotDefinedForThisValue.
47 try {
48 explicit_.solve(arg);
49 } catch (const FunctionNotDefinedForThisValue& exc) {
50 return INFEASIBLE;
51 }
52 assert(!arg.hasNaN());
53 // An explicit constraint may be defined over only a subspace of its input
54 // space. If the input value is outside the definition interval, the explicit
55 // constraint throws an exception of type FunctionNotDefinedForThisValue.
56
57 size_type errorDecreased = 3, iter = 0;
58 value_type previousSquaredNorm;
59 static const value_type dqMinSquaredNorm = NumTraits::dummy_precision();
60 value_type initSquaredNorm = 0;
61
62 // Variables for optimization only
63 value_type previousCost = 0;
64 value_type scaling = 1.;
65 bool onlyLineSearch = false;
66 vector_t qopt;
67
68 // Fill value and Jacobian
69 computeValue<true>(arg);
70 computeError();
71 if (optimize) previousCost = datas_.back().error.squaredNorm();
72
73 bool errorWasBelowThr = (squaredNorm_ < squaredErrorThreshold_);
74 vector_t initArg;
75 if (errorWasBelowThr) {
76 initArg = arg;
77 if (!optimize) iter = std::max(maxIterations_, size_type(2)) - 2;
78 initSquaredNorm = squaredNorm_;
79 }
80
81 bool errorIsAboveThr = (squaredNorm_ > .25 * squaredErrorThreshold_);
82 if (errorIsAboveThr && reducedDimension_ == 0) return INFEASIBLE;
83 if (optimize && !errorIsAboveThr) qopt = arg;
84
85 Status status = SUCCESS;
86 while ((optimize || (errorIsAboveThr && errorDecreased))) {
87 // 1. Maximum iterations
88 if (iter >= maxIterations_) {
89 status = MAX_ITERATION_REACHED;
90 break;
91 }
92 status = SUCCESS;
93
94 // 2. Compute step
95 // onlyLineSearch is true when we only reduced the scaling.
96 if (!onlyLineSearch) {
97 previousSquaredNorm = squaredNorm_;
98 // Update the jacobian using the jacobian of the explicit system.
99 updateJacobian(arg);
100 computeSaturation(arg);
101 computeDescentDirection();
102 }
103 // Apply scaling to avoid too large steps.
104 if (optimize) dq_ *= scaling;
105 if (dq_.squaredNorm() < dqMinSquaredNorm) {
106 // We assume that the algorithm reached a local minima.
107 status = INFEASIBLE;
108 break;
109 }
110 // 3. Apply line search algorithm for the computed step
111 try {
112 lineSearch(*this, arg, dq_);
113 explicit_.solve(arg);
114 } catch (const FunctionNotDefinedForThisValue& exc) {
115 return INFEASIBLE;
116 }
117 assert(!arg.hasNaN());
118
119 // 4. Evaluate the error at the new point.
120 computeValue<true>(arg);
121 computeError();
122
123 --errorDecreased;
124 if (squaredNorm_ < previousSquaredNorm)
125 errorDecreased = 3;
126 else
127 status = ERROR_INCREASED;
128
129 errorIsAboveThr = (squaredNorm_ > .25 * squaredErrorThreshold_);
130 // 5. In case of optimization,
131 // - if the constraints is satisfied and the cost decreased, increase
132 // the scaling (amount of confidence in the linear approximation)
133 // - if the constraints is not satisfied, decrease the scaling and
134 // and cancel this step.
135 if (optimize) {
136 if (!errorIsAboveThr) {
137 value_type cost = datas_.back().error.squaredNorm();
138 if (cost < previousCost) {
139 qopt = arg;
140 previousCost = cost;
141 if (scaling < 0.5) scaling *= 2;
142 }
143 onlyLineSearch = false;
144 } else {
145 dq_ /= scaling;
146 scaling *= 0.5;
147 if (qopt.size() > 0) arg = qopt;
148 onlyLineSearch = true;
149 }
150 }
151
152 ++iter;
153 }
154
155 if (!optimize && errorWasBelowThr) {
156 if (squaredNorm_ > initSquaredNorm) {
157 arg = initArg;
158 }
159 return SUCCESS;
160 }
161 // If optimizing, qopt is the visited configuration that satisfies the
162 // constraints and has lowest cost.
163 if (optimize && qopt.size() > 0) arg = qopt;
164
165 assert(!arg.hasNaN());
166 return status;
167}
168} // namespace solver
169} // namespace constraints
170} // namespace hpp
171
172#endif // HPP_CONSTRAINTS_SOLVER_IMPL_BY_SUBSTITUTION_HH
hpp::constraints::ExplicitConstraintSet::solve
bool solve(vectorOut_t arg) const
hpp::constraints::FunctionNotDefinedForThisValue
Exception thrown when a function is evaluated outside its definition domain.
Definition explicit.hh:41
hpp::constraints::solver::BySubstitution::updateJacobian
void updateJacobian(vectorIn_t arg) const
hpp::constraints::solver::HierarchicalIterative::reducedDimension_
size_type reducedDimension_
Definition hierarchical-iterative.hh:618
hpp::constraints::solver::HierarchicalIterative::maxIterations_
size_type maxIterations_
Definition hierarchical-iterative.hh:614
hpp::constraints::solver::HierarchicalIterative::squaredNorm_
value_type squaredNorm_
Definition hierarchical-iterative.hh:641
hpp::constraints::solver::HierarchicalIterative::computeSaturation
void computeSaturation(vectorIn_t arg) const
hpp::constraints::solver::HierarchicalIterative::squaredErrorThreshold_
value_type squaredErrorThreshold_
Definition hierarchical-iterative.hh:613
hpp::constraints::solver::HierarchicalIterative::dq_
vector_t dq_
Definition hierarchical-iterative.hh:636
hpp::constraints::solver::HierarchicalIterative::Status
Status
Definition hierarchical-iterative.hh:245
hpp::constraints::solver::HierarchicalIterative::ERROR_INCREASED
@ ERROR_INCREASED
Definition hierarchical-iterative.hh:245
hpp::constraints::solver::HierarchicalIterative::SUCCESS
@ SUCCESS
Definition hierarchical-iterative.hh:245
hpp::constraints::solver::HierarchicalIterative::MAX_ITERATION_REACHED
@ MAX_ITERATION_REACHED
Definition hierarchical-iterative.hh:245
hpp::constraints::solver::HierarchicalIterative::INFEASIBLE
@ INFEASIBLE
Definition hierarchical-iterative.hh:245
hpp::constraints::solver::HierarchicalIterative::datas_
std::vector< Data > datas_
Definition hierarchical-iterative.hh:642
hpp::constraints::solver::HierarchicalIterative::computeError
void computeError() const
hpp::constraints::solver::HierarchicalIterative::computeDescentDirection
void computeDescentDirection() const
hpp::constraints::solver::HierarchicalIterative::lastIsOptional_
bool lastIsOptional_
Definition hierarchical-iterative.hh:619
hpp::constraints::solver::NumTraits
Eigen::NumTraits< value_type > NumTraits
Definition by-substitution.hh:36
hpp::constraints::solver::numeric_limits
std::numeric_limits< value_type > numeric_limits
Definition by-substitution.hh:35
hpp::constraints::size_type
pinocchio::size_type size_type
Definition fwd.hh:48
hpp::constraints::value_type
pinocchio::value_type value_type
Definition fwd.hh:49
hpp::constraints::vectorOut_t
pinocchio::vectorOut_t vectorOut_t
Definition fwd.hh:62
hpp::constraints::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:60
hpp