config-projector.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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-core 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-core  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_CORE_CONFIG_PROJECTOR_HH
# define HPP_CORE_CONFIG_PROJECTOR_HH

# include <Eigen/SVD>

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

# include <hpp/core/config.hh>
# include <hpp/core/constraint.hh>
# include <hpp/core/deprecated.hh>

namespace hpp {
  namespace core {
    typedef constraints::solver::BySubstitution BySubstitution;


    class HPP_CORE_DLLAPI ConfigProjector : public Constraint
    {
    public:
      enum LineSearchType {
        Backtracking,
        ErrorNormBased,
        FixedSequence,
        Constant
      };

      static ConfigProjectorPtr_t create (const DevicePtr_t& robot,
                      const std::string& name,
                      value_type errorThreshold,
                      size_type maxIterations);

      static ConfigProjectorPtr_t createCopy (const ConfigProjectorPtr_t cp);

      virtual ConstraintPtr_t copy () const;

      virtual ~ConfigProjector ();

      bool contains (const constraints::ImplicitPtr_t& numericalConstraint)
        const;

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

      bool add (const constraints::ImplicitPtr_t& numericalConstraint,
        const std::size_t priority)
      {
        return add (numericalConstraint, segments_t(0), priority);
      }

      void lastIsOptional (bool optional);

      bool lastIsOptional () const;

      bool optimize (ConfigurationOut_t config,
          std::size_t maxIter = 0);

      const DevicePtr_t& robot () const
      {
    return robot_;
      }

      void projectVectorOnKernel (ConfigurationIn_t from,
                vectorIn_t velocity, vectorOut_t result);

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

      void computeValueAndJacobian (ConfigurationIn_t configuration,
                    vectorOut_t value,
                    matrixOut_t reducedJacobian);

      bool oneStep (ConfigurationOut_t config, vectorOut_t dq,
          const value_type& alpha) HPP_CORE_DEPRECATED;


      size_type numberFreeVariables () const;

      size_type numberNonLockedDof () const HPP_CORE_DEPRECATED;

      size_type dimension () const;

      void compressVector (vectorIn_t normal, vectorOut_t small) const;

      void uncompressVector (vectorIn_t small, vectorOut_t normal) const;

      void compressMatrix (matrixIn_t normal, matrixOut_t small,
               bool rows = true) const;

      void uncompressMatrix (matrixIn_t small, matrixOut_t normal,
                 bool rows = true) const;


      void maxIterations (size_type iterations);
      size_type maxIterations () const;

      void errorThreshold (const value_type& threshold);
      value_type errorThreshold () const;

      value_type residualError() const;

      const value_type& sigma() const;


      vector_t rightHandSideFromConfig (ConfigurationIn_t config);

      void rightHandSideFromConfig (const constraints::ImplicitPtr_t& nm,
                                    ConfigurationIn_t config);

      void rightHandSide (const vector_t& param);

      void rightHandSide (const constraints::ImplicitPtr_t& nm, vectorIn_t rhs);

      vector_t rightHandSide () const;

      void rightHandSideAt (const value_type& s);


      virtual bool isSatisfied (ConfigurationIn_t config);

      virtual bool isSatisfied (ConfigurationIn_t config, vector_t& error);

      ::hpp::statistics::SuccessStatistics& statistics()
      {
        return statistics_;
      }

      const NumericalConstraints_t& numericalConstraints () const;

      const BySubstitution& solver () const
      {
        return *solver_;
      }

      void lineSearchType (LineSearchType ls)
      {
        lineSearchType_ = ls;
      }

      LineSearchType lineSearchType () const
      {
        return lineSearchType_;
      }

      static void defaultLineSearch (LineSearchType ls);

    protected:
      ConfigProjector (const DevicePtr_t& robot, const std::string& name,
               value_type errorThreshold, size_type maxIterations);
      ConfigProjector (const ConfigProjector& cp);

      void init (const ConfigProjectorPtr_t& self)
      {
    Constraint::init (self);
    weak_ = self;
      }
      virtual bool impl_compute (ConfigurationOut_t configuration);

    private:
      virtual std::ostream& print (std::ostream& os) const;
      virtual void addToConstraintSet (const ConstraintSetPtr_t& constraintSet);

      DevicePtr_t robot_;
      static LineSearchType defaultLineSearch_;
      LineSearchType lineSearchType_;
      BySubstitution* solver_;

      bool solverOneStep (ConfigurationOut_t config) const;
      int  solverSolve   (ConfigurationOut_t config) const;

      ConfigProjectorWkPtr_t weak_;
      ::hpp::statistics::SuccessStatistics statistics_;
    }; // class ConfigProjector
  } // namespace core
} // namespace hpp
#endif // HPP_CORE_CONFIG_PROJECTOR_HH