hierarchical-iterative.hh

Go to the documentation of this file.

// Copyright (c) 2017, 2018
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-constraints.
// hpp-constraints is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-constraints is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-constraints. If not, see <http://www.gnu.org/licenses/>.

#ifndef HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH
#define HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH

#include <boost/function.hpp>

#include <hpp/statistics/success-bin.hh>

#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/config.hh>
#include <hpp/constraints/deprecated.hh>

#include <hpp/constraints/matrix-view.hh>
#include <hpp/constraints/implicit-constraint-set.hh>

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

      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 boost::function<bool (vectorIn_t q, vectorOut_t qSat,
                                      Eigen::VectorXi& saturation)>
          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;

        void add (const DifferentiableFunctionPtr_t& f, const std::size_t& priority) HPP_CONSTRAINTS_DEPRECATED;

        void add (const DifferentiableFunctionPtr_t& f, const std::size_t& priority,
                  const ComparisonTypes_t& comp) HPP_CONSTRAINTS_DEPRECATED;

        void 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_;
        }



        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 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_;
        }



        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 {
          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_;
        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_;


        mutable ::hpp::statistics::SuccessStatistics statistics_;

        friend struct lineSearch::Backtracking;
      }; // class HierarchicalIterative
    } // namespace solver
    namespace lineSearch {
      typedef ::hpp::constraints::solver::lineSearch::Constant Constant
      HPP_CONSTRAINTS_DEPRECATED;
      typedef ::hpp::constraints::solver::lineSearch::Backtracking Backtracking
      HPP_CONSTRAINTS_DEPRECATED;
      typedef ::hpp::constraints::solver::lineSearch::FixedSequence
      FixedSequence HPP_CONSTRAINTS_DEPRECATED;
      typedef ::hpp::constraints::solver::lineSearch::ErrorNormBased
      ErrorNormBased HPP_CONSTRAINTS_DEPRECATED;
    } // namespace lineSearch
  } // namespace constraints
} // namespace hpp

#endif // HPP_CONSTRAINTS_SOLVER_HIERARCHICAL_ITERATIVE_HH