liegroup-base.hpp

Go to the documentation of this file.

//
// 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
{
  constexpr int SELF = 0;
 
#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 & /*other*/)
    {
      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__