differentiable-function.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
 
#ifndef HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
#define HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
 
#include <hpp/constraints/config.hh>
#include <hpp/constraints/fwd.hh>
#include <hpp/pinocchio/liegroup-element.hh>
#include <hpp/util/debug.hh>
#include <hpp/util/serialization-fwd.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.space()->nq() == 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;
 
  bool operator==(DifferentiableFunction const& other) const;
  bool operator!=(DifferentiableFunction const& b) const;

  virtual std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
      DeviceConstPtr_t /*robot*/) const {
    return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
  };

  static DifferentiableFunctionPtr_t extract(
      DifferentiableFunctionPtr_t original, interval_t interval);
 
 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;
 
  virtual bool isEqual(const DifferentiableFunction& other) const {
    if (name_ != other.name_) return false;
    if (inputSize_ != other.inputSize_) return false;
    if (inputDerivativeSize_ != other.inputDerivativeSize_) return false;
    if (*outputSpace_ != *(other.outputSpace_)) return false;
 
    return true;
  }

  size_type inputSize_;
  size_type inputDerivativeSize_;
  LiegroupSpacePtr_t outputSpace_;

  ArrayXb activeParameters_;
  ArrayXb activeDerivativeParameters_;
 
 private:
  std::string name_;
  std::string context_;
 
  friend class DifferentiableFunctionSet;
 
 protected:
  DifferentiableFunction() {}
 
 private:
  HPP_SERIALIZABLE();
};  // class DifferentiableFunction
inline std::ostream& operator<<(std::ostream& os,
                                const DifferentiableFunction& f) {
  return f.print(os);
}

}  // namespace constraints
}  // namespace hpp
 
/*
// This will override boost::shared_ptr 's equality operator
// between 2 DifferentiableFunctionPtr_t
namespace boost {
  using namespace hpp::constraints;
  typedef DifferentiableFunction T;
  typedef shared_ptr<T> TPtr;
 
  template<> inline bool operator==<T, T> (TPtr const & a, TPtr const & b)
BOOST_SP_NOEXCEPT
  {
    return *a == *b;
  }
 
  template<> inline bool operator!=<T, T> (TPtr const & a, TPtr const & b)
BOOST_SP_NOEXCEPT
  {
    return !(a == b);
  }
}
*/
 
#endif  // HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH