Program Listing for File liegroup-base.hpp

↰ Return to documentation for file (include/pinocchio/multibody/liegroup/liegroup-base.hpp)

//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__
#define __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__

#include "pinocchio/multibody/liegroup/fwd.hpp"

#include <limits>

namespace pinocchio
{
#ifdef PINOCCHIO_WITH_CXX11_SUPPORT
  constexpr int SELF = 0;
#else
  enum { SELF = 0 };
#endif

#define PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,TYPENAME)               \
  typedef          LieGroupBase<Derived> Base;                        \
  typedef TYPENAME Base::Index Index;                                          \
  typedef TYPENAME traits<Derived>::Scalar Scalar;                             \
  enum {                                                                       \
    Options = traits<Derived>::Options,                                        \
    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 PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE(Derived)                                \
PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,PINOCCHIO_MACRO_EMPTY_ARG)

#define PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(Derived)                            \
PINOCCHIO_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
    {
      Options = traits<LieGroupDerived>::Options,
      NQ = traits<LieGroupDerived>::NQ,
      NV = traits<LieGroupDerived>::NV
    };

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> 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 Jacobian_t>
    void integrateCoeffWiseJacobian(const Eigen::MatrixBase<Config_t >  & q,
                                    const Eigen::MatrixBase<Jacobian_t> & J) const;

    template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
    void dIntegrate(const Eigen::MatrixBase<Config_t >  & q,
                    const Eigen::MatrixBase<Tangent_t>  & v,
                    const Eigen::MatrixBase<JacobianOut_t> & J,
                    AssignmentOperatorType op = SETTO) const
    {
      PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
      return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg,op);
    }

    template<class Config_t, class Tangent_t, class JacobianOut_t>
    void dIntegrate(const Eigen::MatrixBase<Config_t >  & q,
                    const Eigen::MatrixBase<Tangent_t>  & v,
                    const Eigen::MatrixBase<JacobianOut_t> & J,
                    const ArgumentPosition arg,
                    const AssignmentOperatorType op = SETTO) 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 AssignmentOperatorType op = SETTO) const;

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrate_dq(const Eigen::MatrixBase<Config_t >  & q,
                       const Eigen::MatrixBase<Tangent_t>  & v,
                       const Eigen::MatrixBase<JacobianIn_t> & Jin,
                       int self,
                       const Eigen::MatrixBase<JacobianOut_t> & Jout,
                       const AssignmentOperatorType op = SETTO) const;

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrate_dq(const Eigen::MatrixBase<Config_t >  & q,
                       const Eigen::MatrixBase<Tangent_t>  & v,
                       int self,
                       const Eigen::MatrixBase<JacobianIn_t> & Jin,
                       const Eigen::MatrixBase<JacobianOut_t> & Jout,
                       const AssignmentOperatorType op = SETTO) 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 AssignmentOperatorType op = SETTO) const;

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrate_dv(const Eigen::MatrixBase<Config_t >  & q,
                       const Eigen::MatrixBase<Tangent_t>  & v,
                       int self,
                       const Eigen::MatrixBase<JacobianIn_t> & Jin,
                       const Eigen::MatrixBase<JacobianOut_t> & Jout,
                       const AssignmentOperatorType op = SETTO) const;

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrate_dv(const Eigen::MatrixBase<Config_t >  & q,
                       const Eigen::MatrixBase<Tangent_t>  & v,
                       const Eigen::MatrixBase<JacobianIn_t> & Jin,
                       int self,
                       const Eigen::MatrixBase<JacobianOut_t> & Jout,
                       const AssignmentOperatorType op = SETTO) const;

    template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrateTransport(const Eigen::MatrixBase<Config_t >  & q,
                             const Eigen::MatrixBase<Tangent_t>  & v,
                             const Eigen::MatrixBase<JacobianIn_t> & Jin,
                             const Eigen::MatrixBase<JacobianOut_t> & Jout,
                             const ArgumentPosition arg) const;

    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t >  & q,
                                const Eigen::MatrixBase<Tangent_t>  & v,
                                const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                const Eigen::MatrixBase<JacobianOut_t> & Jout) const;
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t >  & q,
                                const Eigen::MatrixBase<Tangent_t>  & v,
                                const Eigen::MatrixBase<JacobianIn_t> & Jin,
                                const Eigen::MatrixBase<JacobianOut_t> & Jout) const;


    template<class Config_t, class Tangent_t, class Jacobian_t>
    void dIntegrateTransport(const Eigen::MatrixBase<Config_t >  & q,
                             const Eigen::MatrixBase<Tangent_t>  & v,
                             const Eigen::MatrixBase<Jacobian_t> & J,
                             const ArgumentPosition arg) const;

    template <class Config_t, class Tangent_t, class Jacobian_t>
    void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t >  & q,
                                const Eigen::MatrixBase<Tangent_t>  & v,
                                const Eigen::MatrixBase<Jacobian_t> & J) const;
    template <class Config_t, class Tangent_t, class Jacobian_t>
    void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t >  & q,
                                const Eigen::MatrixBase<Tangent_t>  & v,
                                const Eigen::MatrixBase<Jacobian_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>
    bool isNormalized(const Eigen::MatrixBase<Config_t> & qin,
                      const Scalar& prec = Eigen::NumTraits<Scalar>::dummy_precision()) 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 <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
                     const Eigen::MatrixBase<ConfigR_t> & q1,
                     const Eigen::MatrixBase<JacobianOut_t> & J) const;

    template<class ConfigL_t, class ConfigR_t, class JacobianOut_t>
    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
                     const Eigen::MatrixBase<ConfigR_t> & q1,
                     const Eigen::MatrixBase<JacobianOut_t> & J,
                     const ArgumentPosition arg) const;

    template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
                     const Eigen::MatrixBase<ConfigR_t> & q1,
                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                     int self,
                     const Eigen::MatrixBase<JacobianOut_t> & Jout,
                     const AssignmentOperatorType op = SETTO) const;

    template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
    void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
                     const Eigen::MatrixBase<ConfigR_t> & q1,
                     int self,
                     const Eigen::MatrixBase<JacobianIn_t> & Jin,
                     const Eigen::MatrixBase<JacobianOut_t> & Jout,
                     const AssignmentOperatorType op = SETTO) 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;

    bool operator== (const LieGroupBase& other) const
    {
      return derived().isEqual_impl(other.derived());
    }

    bool operator!= (const LieGroupBase& other) const
    {
      return derived().isDifferent_impl(other.derived());
    }


    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 Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrate_product_impl(const Config_t & q,
                                 const Tangent_t & v,
                                 const JacobianIn_t & Jin,
                                 JacobianOut_t & Jout,
                                 bool dIntegrateOnTheLeft,
                                 const ArgumentPosition arg,
                                 const AssignmentOperatorType op) const;

    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
    void dDifference_product_impl(const ConfigL_t & q0,
                                  const ConfigR_t & q1,
                                  const JacobianIn_t & Jin,
                                  JacobianOut_t & Jout,
                                  bool dDifferenceOnTheLeft,
                                  const AssignmentOperatorType op) 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 Config_t>
    bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin,
                           const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) 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;

    bool isEqual_impl (const LieGroupBase& /*other*/) const { return true; }
    bool isDifferent_impl (const LieGroupBase& other) const
    {
      return !derived().isEqual_impl(other.derived());
    }

    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*/) {}
    LieGroupBase& operator=(const LieGroupBase & /*x*/) { return *this; }

    // C++11
    // LieGroupBase(const LieGroupBase &) = delete;
    // LieGroupBase& operator=(const LieGroupBase & /*x*/) = delete;
  }; // struct LieGroupBase

} // namespace pinocchio

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

#endif // ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hpp__