Add argument 'optimize' to BySubstitution::solve

include/hpp/constraints/solver/by-substitution.hh

@@ -156,14 +156,20 @@ namespace hpp {
 
         template <typename LineSearchType>
           Status solve (vectorOut_t arg, LineSearchType ls = LineSearchType()) const
+        {
+          return solve <LineSearchType> (arg, false, ls);
+        }
+
+        template <typename LineSearchType>
+          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.
           // if (explicit_.isConstant()) {
           // 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 <typename LineSearchType>
-          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_;

include/hpp/constraints/solver/impl/by-substitution.hh

@@ -20,21 +20,31 @@
 namespace hpp {
   namespace constraints {
     namespace solver {
+    typedef std::numeric_limits<value_type> numeric_limits;
+    typedef Eigen::NumTraits<value_type> NumTraits;
+
     template <typename LineSearchType>
     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<value_type>::infinity();
-      static const value_type dqMinSquaredNorm = Eigen::NumTraits<value_type>::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<true> (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<true> (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

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