operation-base.hpp

//
// Copyright (c) 2016-2018 CNRS
//
// This file is part of Pinocchio
// Pinocchio 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.
//
// Pinocchio 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
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

#ifndef __se3_lie_group_operation_base_hpp__
#define __se3_lie_group_operation_base_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/spatial/fwd.hpp" // struct traits

#include <Eigen/Core>
#include <limits>

namespace se3
{
  
#define SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,TYPENAME)               \
  typedef          LieGroupBase<Derived> Base;                        \
  typedef TYPENAME Base::Index Index;                                          \
  typedef TYPENAME traits<Derived>::Scalar Scalar;                             \
  enum {                                                                       \
    NQ = Base::NQ,                                                             \
    NV = Base::NV                                                              \
  };                                                                           \
  typedef TYPENAME Base::ConfigVector_t ConfigVector_t;                        \
  typedef TYPENAME Base::TangentVector_t TangentVector_t;                      \
  typedef TYPENAME Base::JacobianMatrix_t JacobianMatrix_t
  
#define SE3_LIE_GROUP_PUBLIC_INTERFACE(Derived)                                \
SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,PINOCCHIO_MACRO_EMPTY_ARG)
  
#define SE3_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived)                            \
SE3_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)

  template<typename Derived>
  struct LieGroupBase
  {
    typedef Derived LieGroupDerived;
    typedef int Index;
    typedef typename traits<LieGroupDerived>::Scalar Scalar;
    enum {
      NQ = traits<LieGroupDerived>::NQ,
      NV = traits<LieGroupDerived>::NV
    };

    typedef Eigen::Matrix<Scalar,NQ,1> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1> TangentVector_t;
    typedef Eigen::Matrix<Scalar,NV,NV> JacobianMatrix_t;


    template <class ConfigIn_t, class Tangent_t, class ConfigOut_t>
    void integrate(const Eigen::MatrixBase<ConfigIn_t> & q,
                   const Eigen::MatrixBase<Tangent_t>  & v,
                   const Eigen::MatrixBase<ConfigOut_t>& qout) const;

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    void dIntegrate_dq(const Eigen::MatrixBase<Config_t >  & q,
                       const Eigen::MatrixBase<Tangent_t>  & v,
                       const Eigen::MatrixBase<JacobianOut_t>& J) const;

    template <class Config_t, class Tangent_t, class JacobianOut_t>
    void dIntegrate_dv(const Eigen::MatrixBase<Config_t >  & q,
                       const Eigen::MatrixBase<Tangent_t>  & v,
                       const Eigen::MatrixBase<JacobianOut_t>& J) const;

    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    void interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
                     const Eigen::MatrixBase<ConfigR_t> & q1,
                     const Scalar& u,
                     const Eigen::MatrixBase<ConfigOut_t>& qout) const;
    
    template <class Config_t>
    void normalize (const Eigen::MatrixBase<Config_t>& qout) const;

    template <class Config_t>
    void random (const Eigen::MatrixBase<Config_t>& qout) const;

    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    void randomConfiguration
    (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
     const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
     const Eigen::MatrixBase<ConfigOut_t> & qout) const;

    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
    void difference(const Eigen::MatrixBase<ConfigL_t> & q0,
                    const Eigen::MatrixBase<ConfigR_t> & q1,
                    const Eigen::MatrixBase<Tangent_t>& v) const;

    template <class ConfigL_t, class ConfigR_t, class JacobianLOut_t, class JacobianROut_t>
    void Jdifference(const Eigen::MatrixBase<ConfigL_t> & q0,
                     const Eigen::MatrixBase<ConfigR_t> & q1,
                     const Eigen::MatrixBase<JacobianLOut_t>& J0,
                     const Eigen::MatrixBase<JacobianROut_t>& J1) const;

    template <class ConfigL_t, class ConfigR_t>
    Scalar squaredDistance(const Eigen::MatrixBase<ConfigL_t> & q0,
                           const Eigen::MatrixBase<ConfigR_t> & q1) const;

    template <class ConfigL_t, class ConfigR_t>
    Scalar distance(const Eigen::MatrixBase<ConfigL_t> & q0,
                    const Eigen::MatrixBase<ConfigR_t> & q1) const;

    template <class ConfigL_t, class ConfigR_t>
    bool isSameConfiguration(const Eigen::MatrixBase<ConfigL_t> & q0,
                             const Eigen::MatrixBase<ConfigR_t> & q1,
                             const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const;


    template <class Config_t, class Tangent_t>
    ConfigVector_t integrate(const Eigen::MatrixBase<Config_t>  & q,
                             const Eigen::MatrixBase<Tangent_t> & v) const ;

    template <class ConfigL_t, class ConfigR_t>
    ConfigVector_t interpolate(const Eigen::MatrixBase<ConfigL_t> & q0,
                               const Eigen::MatrixBase<ConfigR_t> & q1,
                               const Scalar& u) const;

    ConfigVector_t random() const;

    template <class ConfigL_t, class ConfigR_t>
    ConfigVector_t randomConfiguration
    (const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
     const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const;

    template <class ConfigL_t, class ConfigR_t>
    TangentVector_t difference(const Eigen::MatrixBase<ConfigL_t> & q0,
                               const Eigen::MatrixBase<ConfigR_t> & q1) const;



    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                          const Eigen::MatrixBase<ConfigR_t> & q1,
                          const Scalar& u,
                          const Eigen::MatrixBase<ConfigOut_t>& qout) const;
    
    template <class Config_t>
    void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const;

    template <class ConfigL_t, class ConfigR_t>
    Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                const Eigen::MatrixBase<ConfigR_t> & q1) const;

    template <class ConfigL_t, class ConfigR_t>
    bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
                                  const Eigen::MatrixBase<ConfigR_t> & q1,
                                  const Scalar & prec) const;
    Index nq () const;
    Index nv () const;
    ConfigVector_t neutral () const;

    std::string name () const;

    Derived& derived ()
    {
      return static_cast <Derived&> (*this);
    }

    const Derived& derived () const
    {
      return static_cast <const Derived&> (*this);
    }

  protected:
    LieGroupBase() {}

    LieGroupBase( const LieGroupBase & /*clone*/) {}
  }; // struct LieGroupBase

} // namespace se3

#include "pinocchio/multibody/liegroup/operation-base.hxx"

#endif // ifndef __se3_lie_group_operation_base_hpp__