explicit-constraint-set.hh

Go to the documentation of this file.

// Copyright (c) 2017, Joseph Mirabel
// 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_EXPLICIT_CONSTRAINT_SET_HH
#define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH

#include <vector>

#include <boost/function.hpp>

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

#include <hpp/constraints/matrix-view.hh>
#include <hpp/constraints/differentiable-function-set.hh>

namespace hpp {
  namespace constraints {

    class HPP_CONSTRAINTS_DLLAPI ExplicitConstraintSet
    {
      public:
        typedef Eigen::RowBlockIndices RowBlockIndices;
        typedef Eigen::ColBlockIndices ColBlockIndices;
        typedef Eigen::MatrixBlockView<matrix_t, Eigen::Dynamic, Eigen::Dynamic, false, false> MatrixBlockView;


        bool solve (vectorOut_t arg) const;

        bool isSatisfied (vectorIn_t arg) const;

        bool isSatisfied (vectorIn_t arg, vectorOut_t error) const;

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


        size_type add (const ExplicitPtr_t& constraint);

        bool contains (const ExplicitPtr_t& numericalConstraint) const;

        ExplicitConstraintSet (const LiegroupSpacePtr_t& space)
          : configSpace_ (space)
          ,   inArgs_ (), notOutArgs_ ()
          ,   inDers_ (), notOutDers_ ()
          ,  outArgs_ (),  outDers_ ()
          , argFunction_ (Eigen::VectorXi::Constant(space->nq (), -1))
          , derFunction_ (Eigen::VectorXi::Constant(space->nv (), -1))
          , squaredErrorThreshold_ (Eigen::NumTraits<value_type>::epsilon())
          // , Jg (nv, nv)
          , arg_ (space->nq ()), diff_(space->nv ()), diffSmall_()
        {
          notOutArgs_.addRow(0, space->nq ());
          notOutDers_.addCol(0, space->nv ());
        }



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



        const RowBlockIndices& inArgs () const
        {
          return inArgs_;
        }

        const ColBlockIndices& inDers () const
        {
          return inDers_;
        }

        const RowBlockIndices& freeArgs () const HPP_CONSTRAINTS_DEPRECATED
        {
          return notOutArgs_;
        }

        const ColBlockIndices& freeDers () const HPP_CONSTRAINTS_DEPRECATED
        {
          return notOutDers_;
        }

        const RowBlockIndices& notOutArgs () const
        {
          return notOutArgs_;
        }

        const ColBlockIndices& notOutDers () const
        {
          return notOutDers_;
        }

        ColBlockIndices activeParameters () const;

        const ColBlockIndices& activeDerivativeParameters () const;

        const Eigen::MatrixXi& inOutDependencies () const
        {
          return inOutDependencies_;
        }

        Eigen::MatrixXi inOutDofDependencies () const;

        const Eigen::VectorXi& derFunction () const
        {
          return derFunction_;
        }

        const RowBlockIndices& outArgs () const
        {
          return outArgs_;
        }

        const RowBlockIndices& outDers () const
        {
          return outDers_;
        }

        std::size_t argSize () const HPP_CONSTRAINTS_DEPRECATED
        {
          return configSpace_->nq ();
        }

        std::size_t derSize () const HPP_CONSTRAINTS_DEPRECATED
        {
          return configSpace_->nv ();
        }

        LiegroupSpacePtr_t configSpace () const
        {
          return configSpace_;
        }

        std::size_t nq () const
        {
          return configSpace_->nq ();
        }

        std::size_t nv () const
        {
          return configSpace_->nv ();
        }


        inline MatrixBlockView viewJacobian(matrix_t& jacobian) const
          HPP_CONSTRAINTS_DEPRECATED
        {
          return MatrixBlockView(jacobian,
              outDers_.nbIndices() , outDers_.indices(),
              notOutDers_.nbIndices(), notOutDers_.indices());
        }

        inline MatrixBlockView jacobianNotOutToOut (matrix_t& jacobian) const
        {
          return MatrixBlockView(jacobian,
              outDers_.nbIndices() , outDers_.indices(),
              notOutDers_.nbIndices(), notOutDers_.indices());
        }

        void jacobian(matrixOut_t jacobian, vectorIn_t q) const;


        vector_t rightHandSideFromInput (vectorIn_t p);

        bool rightHandSideFromInput (const ExplicitPtr_t& constraint,
                                     vectorIn_t p);

        void rightHandSideFromInput (const size_type& i, vectorIn_t p);

        void rightHandSide (vectorIn_t rhs);

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

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

        void rightHandSide (const size_type& i, vectorIn_t rhs);

        vector_t rightHandSide () const;

        size_type rightHandSideSize () const;


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

      private:
        typedef std::vector<bool> Computed_t;

        void solveExplicitConstraint(const std::size_t& i, vectorOut_t arg)
          const;
        void computeJacobian(const std::size_t& i, matrixOut_t J) const;
        void computeOrder(const std::size_t& iF, std::size_t& iOrder, Computed_t& computed);

        LiegroupSpacePtr_t configSpace_;

        struct Data {
          Data (const ExplicitPtr_t& constraint);
          ExplicitPtr_t constraint;
          RowBlockIndices equalityIndices;
          vector_t rhs_implicit;
          vector_t rhs_explicit;
          // implicit formulation
          mutable LiegroupElement h_value;
          // explicit formulation
          mutable vector_t qin, qout;
          mutable LiegroupElement f_value, res_qout;
          // jacobian of f
          mutable matrix_t jacobian;
        }; // struct Data

        RowBlockIndices inArgs_, notOutArgs_;
        ColBlockIndices inDers_, notOutDers_;
        RowBlockIndices outArgs_, outDers_;

        Eigen::MatrixXi inOutDependencies_;

        std::vector<Data> data_;
        std::vector<std::size_t> computationOrder_;
        Eigen::VectorXi argFunction_, derFunction_;
        value_type squaredErrorThreshold_;
        // mutable matrix_t Jg;
        mutable vector_t arg_, diff_, diffSmall_;
    }; // class ExplicitConstraintSet
  } // namespace constraints
} // namespace hpp

#endif // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH