differentiable-function.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// 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_DIFFERENTIABLE_FUNCTION_HH
# define HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH

# include <hpp/constraints/fwd.hh>
# include <hpp/constraints/config.hh>
# include <hpp/pinocchio/liegroup-element.hh>

namespace hpp {
  namespace constraints {


    class HPP_CONSTRAINTS_DLLAPI DifferentiableFunction
    {
    public:
      virtual ~DifferentiableFunction () {}
      LiegroupElement operator () (vectorIn_t argument) const
      {
    assert (argument.size () == inputSize ());
        LiegroupElement result (outputSpace_);
    impl_compute (result, argument);
        return result;
      }
      void value (LiegroupElementRef result,
                  vectorIn_t argument) const
      {
    assert (result.size () == outputSize ());
    assert (argument.size () == inputSize ());
    impl_compute (result, argument);
      }
      void jacobian (matrixOut_t jacobian, vectorIn_t argument) const
      {
    assert (argument.size () == inputSize ());
    assert (jacobian.rows () == outputDerivativeSize ());
    assert (jacobian.cols () == inputDerivativeSize ());
    impl_jacobian (jacobian, argument);
      }

      const ArrayXb& activeParameters () const
      {
        return activeParameters_;
      }

      const ArrayXb& activeDerivativeParameters () const
      {
        return activeDerivativeParameters_;
      }

      size_type inputSize () const
      {
    return inputSize_;
      }
      size_type inputDerivativeSize () const
      {
    return inputDerivativeSize_;
      }
      LiegroupSpacePtr_t outputSpace () const
      {
        return outputSpace_;
      }
      size_type  outputSize () const
      {
    return outputSpace_->nq ();
      }
      size_type  outputDerivativeSize () const
      {
    return outputSpace_->nv ();
      }
      const std::string& name () const
      {
    return name_;
      }

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

      std::string context () const {
        return context_;
      }

      void context (const std::string& c) {
        context_ = c;
      }

      void finiteDifferenceForward (matrixOut_t jacobian, vectorIn_t arg,
          DevicePtr_t robot = DevicePtr_t (),
          value_type eps = std::sqrt(Eigen::NumTraits<value_type>::epsilon())) const;

      void finiteDifferenceCentral (matrixOut_t jacobian, vectorIn_t arg,
          DevicePtr_t robot = DevicePtr_t (),
          value_type eps = std::sqrt(Eigen::NumTraits<value_type>::epsilon())) const;

    protected:
      DifferentiableFunction (size_type sizeInput,
                  size_type sizeInputDerivative,
                  size_type sizeOutput,
                  std::string name = std::string ());

      DifferentiableFunction (size_type sizeInput,
                  size_type sizeInputDerivative,
                  const LiegroupSpacePtr_t& outputSpace,
                  std::string name = std::string ());

      virtual void impl_compute (LiegroupElementRef result,
                 vectorIn_t argument) const = 0;

      virtual void impl_jacobian (matrixOut_t jacobian,
                  vectorIn_t arg) const = 0;

      size_type inputSize_;
      size_type inputDerivativeSize_;
      LiegroupSpacePtr_t outputSpace_;

      ArrayXb activeParameters_;
      ArrayXb activeDerivativeParameters_;

    private:
      std::string name_;
      std::string context_;

      friend class DifferentiableFunctionSet;
    }; // class DifferentiableFunction
    inline std::ostream&
    operator<< (std::ostream& os, const DifferentiableFunction& f)
    {
      return f.print (os);
    }
  } // namespace constraints
} // namespace hpp


#endif // HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH