hierarchical-iterative.hh

Go to the documentation of this file.

// Copyright (c) 2017, 2018
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
 
#ifndef HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
#define HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
 
#include <functional>
#include <hpp/constraints/config.hh>
#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/implicit-constraint-set.hh>
#include <hpp/constraints/matrix-view.hh>
#include <hpp/util/serialization-fwd.hh>
#include <map>
 
namespace hpp {
namespace constraints {
namespace solver {
namespace lineSearch {
struct Constant {
  template <typename SolverType>
  bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
};

struct Backtracking {
  Backtracking();
 
  template <typename SolverType>
  bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
 
  template <typename SolverType>
  inline value_type computeLocalSlope(const SolverType& solver) const;
 
  value_type c, tau, smallAlpha;  // 0.8 ^ 7 = 0.209, 0.8 ^ 8 = 0.1677
  mutable vector_t arg_darg, df, darg;
};

struct FixedSequence {
  FixedSequence();
 
  template <typename SolverType>
  bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
 
  value_type alpha;
  value_type alphaMax, K;
};

struct ErrorNormBased {
  ErrorNormBased(value_type alphaMin, value_type _a, value_type _b);
  ErrorNormBased(value_type alphaMin = 0.2);
 
  template <typename SolverType>
  bool operator()(const SolverType& solver, vectorOut_t arg, vectorOut_t darg);
 
  value_type C, K, a, b;
};
}  // namespace lineSearch
 
namespace saturation {
struct Base {
  virtual bool saturate(vectorIn_t q, vectorOut_t qSat,
                        Eigen::VectorXi& saturation);
  virtual ~Base() {}
};

struct Function : Base {
  typedef std::function<bool(vectorIn_t, vectorOut_t, Eigen::VectorXi&)>
      function_t;
  bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation) {
    return function(q, qSat, saturation);
  }
  Function() {}
  Function(const function_t& function) : function(function) {}
  function_t function;
};

struct Bounds : Base {
  bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation);
  Bounds() {}
  Bounds(const vector_t& lb, const vector_t& ub) : lb(lb), ub(ub), iq2iv_() {
    iq2iv_.resize(ub.size());
    for (size_type i = 0; i < ub.size(); ++i) {
      iq2iv_[i] = (Eigen::VectorXi::Scalar)i;
    }
  }
  Bounds(const vector_t& lb, const vector_t& ub, const Eigen::VectorXi& iq2iv)
      : lb(lb), ub(ub), iq2iv_(iq2iv) {}
  vector_t lb, ub;
  Eigen::VectorXi iq2iv_;
};

struct Device : Base {
  bool saturate(vectorIn_t q, vectorOut_t qSat, Eigen::VectorXi& saturation);
  Device() {}
  Device(const DevicePtr_t& device) : device(device) {}
  DevicePtr_t device;
};
}  // namespace saturation

class HPP_CONSTRAINTS_DLLAPI HierarchicalIterative {
 public:
  typedef Eigen::RowBlockIndices Indices_t;
  typedef lineSearch::FixedSequence DefaultLineSearch;
 
  enum Status { ERROR_INCREASED, MAX_ITERATION_REACHED, INFEASIBLE, SUCCESS };
  typedef shared_ptr<saturation::Base> Saturation_t;
 
  HierarchicalIterative(const LiegroupSpacePtr_t& configSpace);
 
  HierarchicalIterative(const HierarchicalIterative& other);
 
  virtual ~HierarchicalIterative() {}


  const LiegroupSpacePtr_t& configSpace() const { return configSpace_; }
  virtual bool contains(const ImplicitPtr_t& numericalConstraint) const;

  virtual bool add(const ImplicitPtr_t& constraint,
                   const std::size_t& priority);

  virtual void merge(const HierarchicalIterative& other);

  void saturation(const Saturation_t& saturate) { saturate_ = saturate; }

  const Saturation_t& saturation() const { return saturate_; }

  HierarchicalIterative extract(interval_t interval) const;



  template <typename LineSearchType>
  Status solve(vectorOut_t arg, LineSearchType ls = LineSearchType()) const;

  inline Status solve(vectorOut_t arg) const {
    return solve(arg, DefaultLineSearch());
  }

  bool isSatisfied(vectorIn_t arg) const {
    computeValue<false>(arg);
    computeError();
    return squaredNorm_ < squaredErrorThreshold_;
  }

  bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const {
    computeValue<false>(arg);
    computeError();
    return squaredNorm_ < errorThreshold * errorThreshold;
  }

  bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
                             vectorOut_t error, bool& constraintFound) const;

  const value_type& sigma() const { return sigma_; }



  void freeVariables(const segments_t intervals) {
    freeVariables_ = Indices_t();
    for (std::size_t i = 0; i < intervals.size(); ++i)
      freeVariables_.addRow(intervals[i].first, intervals[i].second);
    freeVariables_.updateIndices<true, true, true>();
    update();
  }

  void freeVariables(const Indices_t& indices) {
    freeVariables_ = indices;
    update();
  }

  const Indices_t& freeVariables() const { return freeVariables_; }

  void maxIterations(size_type iterations) { maxIterations_ = iterations; }
  size_type maxIterations() const { return maxIterations_; }

  void errorThreshold(const value_type& threshold) {
    squaredErrorThreshold_ = threshold * threshold;
  }

  value_type errorThreshold() const { return sqrt(squaredErrorThreshold_); }
  value_type squaredErrorThreshold() const { return squaredErrorThreshold_; }

  value_type inequalityThreshold() const { return inequalityThreshold_; }
  void inequalityThreshold(const value_type& it) { inequalityThreshold_ = it; }
 
  void lastIsOptional(bool optional) { lastIsOptional_ = optional; }
 
  bool lastIsOptional() const { return lastIsOptional_; }
 
  // Set whether the constraints are solved level by level
  //
  // If true, the solver will consider only the higher level of
  // priority that is not satisfied when computing the direction
  // of descent.
  void solveLevelByLevel(bool solveLevelByLevel) {
    solveLevelByLevel_ = solveLevelByLevel;
  }
 
  // Get whether the constraints are solved level by level
  bool solveLevelByLevel() const { return solveLevelByLevel_; }


  const ImplicitConstraintSet& constraints(const std::size_t priority) {
    assert(priority < stacks_.size());
    return stacks_[priority];
  }

  const NumericalConstraints_t& constraints() const { return constraints_; }
 
  std::size_t numberStacks() const { return stacks_.size(); }
 
  const size_type& dimension() const { return dimension_; }

  const size_type& reducedDimension() const { return reducedDimension_; }

  ArrayXb activeParameters() const;

  ArrayXb activeDerivativeParameters() const;


  value_type residualError() const { return squaredNorm_; }

  void residualError(vectorOut_t error) const;

  bool definesSubmanifoldOf(const HierarchicalIterative& solver) const;


  vector_t rightHandSideFromConfig(ConfigurationIn_t config);

  virtual bool rightHandSideFromConfig(const ImplicitPtr_t& constraint,
                                       ConfigurationIn_t config);
  virtual bool rightHandSide(const ImplicitPtr_t& constraint, vectorIn_t rhs);

  virtual bool getRightHandSide(const ImplicitPtr_t& constraint,
                                vectorOut_t rhs) const;

  virtual void rightHandSide(vectorIn_t rhs);

  void rightHandSideAt(const value_type& s);

  vector_t rightHandSide() const;

  size_type rightHandSideSize() const;



  template <bool ComputeJac>
  void computeValue(vectorIn_t arg) const;
  void computeSaturation(vectorIn_t arg) const;
  void getValue(vectorOut_t v) const;
  void getReducedJacobian(matrixOut_t J) const;
  void computeError() const;

  const vector_t& lastStep() const { return dq_; }
 
  virtual bool integrate(vectorIn_t from, vectorIn_t velocity,
                         vectorOut_t result) const;
 
  virtual std::ostream& print(std::ostream& os) const;
 
 protected:
  typedef Eigen::JacobiSVD<matrix_t> SVD_t;
 
  struct Data {
    Data()
        : output(),
          rightHandSide(),
          error(),
          jacobian(),
          reducedJ(),
          svd(),
          PK(),
          comparison(),
          inequalityIndices(),
          equalityIndices(),
          activeRowsOfJ() {}

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    LiegroupElement output, rightHandSide;
    vector_t error;
    matrix_t jacobian, reducedJ;
 
    SVD_t svd;
    matrix_t PK;
 
    mutable size_type maxRank;
 
    ComparisonTypes_t comparison;
    std::vector<std::size_t> inequalityIndices;
    Eigen::RowBlockIndices equalityIndices;
    Eigen::MatrixBlocks<false, false> activeRowsOfJ;
  };

  void update();

  virtual void computeActiveRowsOfJ(std::size_t iStack);

  void computeDescentDirection() const;
  void expandDqSmall() const;
  void saturate(vectorOut_t arg) const;
 
  value_type squaredErrorThreshold_, inequalityThreshold_;
  size_type maxIterations_;
 
  std::vector<ImplicitConstraintSet> stacks_;
  LiegroupSpacePtr_t configSpace_;
  size_type dimension_, reducedDimension_;
  bool lastIsOptional_;
  bool solveLevelByLevel_;
  Indices_t freeVariables_;
  Saturation_t saturate_;
  NumericalConstraints_t constraints_;
  std::map<DifferentiableFunctionPtr_t, size_type> iq_;
  std::map<DifferentiableFunctionPtr_t, size_type> iv_;
  std::map<DifferentiableFunctionPtr_t, std::size_t> priority_;

  mutable value_type sigma_;
 
  mutable vector_t dq_, dqSmall_;
  mutable matrix_t reducedJ_;
  mutable Eigen::VectorXi saturation_, reducedSaturation_;
  mutable Configuration_t qSat_;
  mutable ArrayXb tmpSat_;
  mutable value_type squaredNorm_;
  mutable std::vector<Data> datas_;
  mutable SVD_t svd_;
  mutable vector_t OM_;
  mutable vector_t OP_;
 
  friend struct lineSearch::Backtracking;
 
 protected:
  HierarchicalIterative() {}
 
 private:
  HPP_SERIALIZABLE_SPLIT();
};  // class HierarchicalIterative

inline std::ostream& operator<<(std::ostream& os,
                                const HierarchicalIterative& hs) {
  return hs.print(os);
}
}  // namespace solver
}  // namespace constraints
}  // namespace hpp
 
#endif  // HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH