implicit-function.hh

Go to the documentation of this file.

// Copyright (c) 2015 - 2018 LAAS-CNRS
// Authors: Florent Lamiraux, Joseph Mirabel
//
// 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_IMPLICIT_FUNCTION_HH
# define HPP_CONSTRAINTS_EXPLICIT_IMPLICIT_FUNCTION_HH

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

namespace hpp {
  namespace constraints {
    namespace explicit_ {
    class ImplicitFunction : public DifferentiableFunction
    {
    public:
      typedef boost::shared_ptr <ImplicitFunction> Ptr_t;
      static Ptr_t create
      (const DevicePtr_t& robot, const DifferentiableFunctionPtr_t& function,
       const segments_t& inputConf, const segments_t& outputConf,
       const segments_t& inputVelocity, const segments_t& outputVelocity)
        HPP_CONSTRAINTS_DEPRECATED;

      static Ptr_t create
      (const DevicePtr_t& robot, const DifferentiableFunctionPtr_t& function,
       const DifferentiableFunctionPtr_t& g,
       const segments_t& inputConf, const segments_t& outputConf,
       const segments_t& inputVelocity, const segments_t& outputVelocity)
        HPP_CONSTRAINTS_DEPRECATED;


      static Ptr_t create
      (const LiegroupSpacePtr_t& configSpace,
       const DifferentiableFunctionPtr_t& function,
       const segments_t& inputConf, const segments_t& outputConf,
       const segments_t& inputVelocity, const segments_t& outputVelocity);

      const DifferentiableFunctionPtr_t& inputToOutput () const;

    protected:
      ImplicitFunction (const DevicePtr_t& robot,
            const DifferentiableFunctionPtr_t& function,
            const segments_t& inputConf,
                        const segments_t& outputConf,
            const segments_t& inputVelocity,
            const segments_t& outputVelocity)
        HPP_CONSTRAINTS_DEPRECATED;

      ImplicitFunction (const LiegroupSpacePtr_t& configSpace,
                const DifferentiableFunctionPtr_t& function,
                const segments_t& inputConf,
                const segments_t& outputConf,
                const segments_t& inputVelocity,
                const segments_t& outputVelocity);
      void impl_compute (LiegroupElementRef result, vectorIn_t argument) const;

      void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const;

    private:
      void computeJacobianBlocks ();

      DevicePtr_t robot_;
      DifferentiableFunctionPtr_t inputToOutput_;
      Eigen::RowBlockIndices inputConfIntervals_;
      Eigen::RowBlockIndices outputConfIntervals_;
      Eigen::RowBlockIndices inputDerivIntervals_;
      Eigen::RowBlockIndices outputDerivIntervals_;
      std::vector <Eigen::MatrixBlocks <false, false> > outJacobian_;
      std::vector <Eigen::MatrixBlocks <false, false> > inJacobian_;
      mutable vector_t qIn_;
      mutable LiegroupElement f_qIn_, qOut_;
      mutable LiegroupElement result_;
      // Jacobian of explicit function
      mutable matrix_t Jf_;
    }; // class ImplicitFunction

    } // namespace explicit_
  } // namespace constraints
} // namespace hpp

#endif // HPP_CONSTRAINTS_EXPLICIT_IMPLICIT_FUNCTION_HH