From 1722503d4dac7ea1ca14bb2c78a213e5e44fd735 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Mon, 17 Dec 2018 10:53:18 +0100 Subject: [PATCH] Add argument 'optimize' to BySubstitution::solve --- .../hpp/constraints/solver/by-substitution.hh | 10 ++- .../solver/impl/by-substitution.hh | 88 ++++++++++++++----- src/solver/by-substitution.cc | 8 +- 3 files changed, 79 insertions(+), 27 deletions(-) diff --git a/include/hpp/constraints/solver/by-substitution.hh b/include/hpp/constraints/solver/by-substitution.hh index cdf619b2..3e20ee15 100644 --- a/include/hpp/constraints/solver/by-substitution.hh +++ b/include/hpp/constraints/solver/by-substitution.hh @@ -156,6 +156,12 @@ namespace hpp { template Status solve (vectorOut_t arg, LineSearchType ls = LineSearchType()) const + { + return solve (arg, false, ls); + } + + template + Status solve (vectorOut_t arg, bool optimize, LineSearchType ls = LineSearchType()) const { // TODO when there are only locked joint explicit constraints, // there is no need for this intricated loop. @@ -163,7 +169,7 @@ namespace hpp { // explicit_.solve(arg); // iterative_.solve(arg, ls); // } else { - return impl_solve (arg, ls); + return impl_solve (arg, optimize, ls); // } } @@ -315,7 +321,7 @@ namespace hpp { typedef solver::HierarchicalIterative parent_t; template - Status impl_solve (vectorOut_t arg, LineSearchType ls) const; + Status impl_solve (vectorOut_t arg, bool optimize, LineSearchType ls) const; ExplicitConstraintSet explicit_; mutable matrix_t Je_, JeExpanded_; diff --git a/include/hpp/constraints/solver/impl/by-substitution.hh b/include/hpp/constraints/solver/impl/by-substitution.hh index f15c6c2f..0e582e92 100644 --- a/include/hpp/constraints/solver/impl/by-substitution.hh +++ b/include/hpp/constraints/solver/impl/by-substitution.hh @@ -20,21 +20,31 @@ namespace hpp { namespace constraints { namespace solver { + typedef std::numeric_limits numeric_limits; + typedef Eigen::NumTraits NumTraits; + template inline HierarchicalIterative::Status BySubstitution::impl_solve ( vectorOut_t arg, + bool _optimize, LineSearchType lineSearch) const { + bool optimize = _optimize && lastIsOptional_; assert (!arg.hasNaN()); explicit_.solve(arg); size_type errorDecreased = 3, iter = 0; - value_type previousSquaredNorm = - std::numeric_limits::infinity(); - static const value_type dqMinSquaredNorm = Eigen::NumTraits::dummy_precision(); + value_type previousSquaredNorm; + static const value_type dqMinSquaredNorm = NumTraits::dummy_precision(); value_type initSquaredNorm = 0; + // Variables for optimization only + value_type previousCost = numeric_limits::infinity(); + value_type scaling = 1.; + bool onlyLineSearch = false; + vector_t qopt; + // Fill value and Jacobian computeValue (arg); computeError(); @@ -43,30 +53,44 @@ namespace hpp { vector_t initArg; if (errorWasBelowThr) { initArg = arg; - iter = std::max (maxIterations_,size_type(2)) - 2; + if (!optimize) iter = std::max (maxIterations_,size_type(2)) - 2; initSquaredNorm = squaredNorm_; } - if (squaredNorm_ > .25 * squaredErrorThreshold_ - && reducedDimension_ == 0) return INFEASIBLE; + bool errorIsAboveThr = (squaredNorm_ > .25 * squaredErrorThreshold_); + if (errorIsAboveThr && reducedDimension_ == 0) return INFEASIBLE; + if (optimize && !errorIsAboveThr) qopt = arg; Status status; - while (squaredNorm_ > .25 * squaredErrorThreshold_ && errorDecreased && - iter < maxIterations_) { + while ( (optimize || (errorIsAboveThr && errorDecreased))) { + // 1. Maximum iterations + if (iter >= maxIterations_) { + status = MAX_ITERATION_REACHED; + break; + } + status = SUCCESS; - // Update the jacobian using the jacobian of the explicit system. - updateJacobian(arg); - computeSaturation(arg); - computeDescentDirection (); + // 2. Compute step + // onlyLineSearch is true when we only reduced the scaling. + if (!onlyLineSearch) { + previousSquaredNorm = squaredNorm_; + // Update the jacobian using the jacobian of the explicit system. + updateJacobian(arg); + computeSaturation(arg); + computeDescentDirection (); + } + // Apply scaling to avoid too large steps. + if (optimize) dq_ *= scaling; if (dq_.squaredNorm () < dqMinSquaredNorm) { - // TODO INFEASIBLE means that we have reached a local minima. - // The problem may still be feasible from a different starting point. + // We assume that the algorithm reached a local minima. status = INFEASIBLE; break; } + // 3. Apply line search algorithm for the computed step lineSearch (*this, arg, dq_); explicit_.solve(arg); + // 4. Evaluate the error at the new point. computeValue (arg); computeError (); @@ -75,23 +99,45 @@ namespace hpp { errorDecreased = 3; else status = ERROR_INCREASED; - previousSquaredNorm = squaredNorm_; - ++iter; + errorIsAboveThr = (squaredNorm_ > .25 * squaredErrorThreshold_); + // 5. In case of optimization, + // - if the constraints is satisfied and the cost decreased, increase + // the scaling (amount of confidence in the linear approximation) + // - if the constraints is not satisfied, decrease the scaling and + // and cancel this step. + if (optimize) { + if (!errorIsAboveThr) { + value_type cost = datas_.back().error.squaredNorm(); + if (cost < previousCost) { + qopt = arg; + previousCost = cost; + if (scaling < 0.5) scaling *= 2; + } + onlyLineSearch = false; + } else { + dq_ /= scaling; + scaling *= 0.5; + arg = qopt; + onlyLineSearch = true; + } + } + + ++iter; } - if (errorWasBelowThr) { + if (!optimize && errorWasBelowThr) { if (squaredNorm_ > initSquaredNorm) { arg = initArg; } return SUCCESS; } + // If optimizing, qopt is the visited configuration that satisfies the + // constraints and has lowest cost. + if (optimize && qopt.size() > 0) arg = qopt; - if (squaredNorm_ > squaredErrorThreshold_) { - return (iter >= maxIterations_) ? MAX_ITERATION_REACHED : status; - } assert (!arg.hasNaN()); - return SUCCESS; + return status; } } // namespace solver } // namespace constraints diff --git a/src/solver/by-substitution.cc b/src/solver/by-substitution.cc index 1ce011a7..1c21e1c9 100644 --- a/src/solver/by-substitution.cc +++ b/src/solver/by-substitution.cc @@ -351,13 +351,13 @@ namespace hpp { /// \} template BySubstitution::Status BySubstitution::impl_solve - (vectorOut_t arg, lineSearch::Constant lineSearch) const; + (vectorOut_t arg, bool optimize, lineSearch::Constant lineSearch) const; template BySubstitution::Status BySubstitution::impl_solve - (vectorOut_t arg, lineSearch::Backtracking lineSearch) const; + (vectorOut_t arg, bool optimize, lineSearch::Backtracking lineSearch) const; template BySubstitution::Status BySubstitution::impl_solve - (vectorOut_t arg, lineSearch::FixedSequence lineSearch) const; + (vectorOut_t arg, bool optimize, lineSearch::FixedSequence lineSearch) const; template BySubstitution::Status BySubstitution::impl_solve - (vectorOut_t arg, lineSearch::ErrorNormBased lineSearch) const; + (vectorOut_t arg, bool optimize, lineSearch::ErrorNormBased lineSearch) const; } // namespace solver } // namespace constraints } // namespace hpp