by-substitution.hh

Go to the documentation of this file.

// Copyright (c) 2017, 2018 CNRS
// 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_BY_SUBSTITUTION_HH
#define HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH

#include <vector>

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

#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/explicit-constraint-set.hh>
#include <hpp/constraints/solver/hierarchical-iterative.hh>

namespace hpp {
  namespace constraints {
    namespace solver {

      class HPP_CONSTRAINTS_DLLAPI BySubstitution
        : public solver::HierarchicalIterative
      {
      public:
        BySubstitution (const LiegroupSpacePtr_t& configSpace);
        BySubstitution (const BySubstitution& other);

        virtual ~BySubstitution () {}


        ExplicitConstraintSet& explicitSolver() HPP_CONSTRAINTS_DEPRECATED
        {
          return explicit_;
        }

        const ExplicitConstraintSet& explicitSolver () const HPP_CONSTRAINTS_DEPRECATED
        {
          return explicit_;
        }

        void explicitSolverHasChanged() HPP_CONSTRAINTS_DEPRECATED
        {
          return explicitConstraintSetHasChanged ();
        }

        bool add (const ImplicitPtr_t& numericalConstraint,
                  const segments_t& passiveDofs = segments_t (0),
                  const std::size_t priority = 0);

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

        const NumericalConstraints_t& numericalConstraints () const
        {
          return constraints_;
        }

        ExplicitConstraintSet& explicitConstraintSet()
        {
          return explicit_;
        }

        const ExplicitConstraintSet& explicitConstraintSet () const
        {
          return explicit_;
        }

        virtual bool contains (const ImplicitPtr_t& numericalConstraint) const;

        size_type numberFreeVariables () const
        {
          return explicitConstraintSet().notOutDers().nbIndices();
        }

        void explicitConstraintSetHasChanged();

        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, optimize, ls);
          // }
        }

        void projectVectorOnKernel (ConfigurationIn_t from, vectorIn_t velocity,
                                    ConfigurationOut_t result) const;

        virtual void projectOnKernel (ConfigurationIn_t from,
                                      ConfigurationIn_t to,
                                      ConfigurationOut_t result);

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


        vector_t rightHandSideFromConfig (ConfigurationIn_t config);

        bool rightHandSideFromConfig (const ImplicitPtr_t& constraint,
                                      ConfigurationIn_t config);

        bool rightHandSide (const ImplicitPtr_t& constraint,
                            vectorIn_t rhs);

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

        void rightHandSide (vectorIn_t rhs);

        vector_t rightHandSide () const;

        size_type rightHandSideSize () const;


        size_type errorSize () const
        {
          return dimension() + explicit_.outDers().nbIndices();
        }

        bool isSatisfied (vectorIn_t arg) const
        {
          return
            solver::HierarchicalIterative::isSatisfied (arg)
            && explicit_.isSatisfied (arg);
        }

        bool isSatisfied (vectorIn_t arg, vectorOut_t error) const
        {
          assert (error.size() == dimension() + explicit_.outDers().nbIndices());
          bool iterative =
            solver::HierarchicalIterative::isSatisfied (arg);
          residualError(error.head(dimension()));
          bool _explicit =
            explicit_.isSatisfied (arg, error.tail(explicit_.outDers().nbIndices()));
          return iterative && _explicit;
        }

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

      template <typename LineSearchType>
          bool oneStep (vectorOut_t arg, LineSearchType& lineSearch) const
        {
          computeValue<true> (arg);
          updateJacobian (arg);
          computeDescentDirection ();
          lineSearch (*this, arg, dq_);
          explicit_.solve (arg);
          return solver::HierarchicalIterative::isSatisfied(arg);
        }

        void updateJacobian (vectorIn_t arg) const;

        void errorThreshold (const value_type& threshold)
        {
          solver::HierarchicalIterative::errorThreshold(threshold);
          explicit_.errorThreshold(threshold);
        }
        value_type errorThreshold () const
        {
          return solver::HierarchicalIterative::errorThreshold();
        }

        segments_t implicitDof () const;

        virtual std::ostream& print (std::ostream& os) const;

        bool integrate(vectorIn_t from, vectorIn_t velocity, vectorOut_t result)
          const
        {
          bool res = solver::HierarchicalIterative::integrate(from, velocity, result);
          explicit_.solve (result);
          return res;
        }

      protected:
        void computeActiveRowsOfJ (std::size_t iStack);

      private:
        typedef solver::HierarchicalIterative parent_t;

        template <typename LineSearchType>
          Status impl_solve (vectorOut_t arg, bool optimize, LineSearchType ls) const;

        ExplicitConstraintSet explicit_;
        mutable matrix_t Je_, JeExpanded_;
      }; // class BySubstitution

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

#endif // HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH