cartesian-product-variant.hpp

Go to the documentation of this file.

//
// Copyright (c) 2018 CNRS
//
 
#ifndef __pinocchio_cartesian_product_variant_hpp__
#define __pinocchio_cartesian_product_variant_hpp__
 
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
 
#include "pinocchio/container/aligned-vector.hpp"
 
namespace pinocchio
{
 
  template<
    typename Scalar,
    int Options = context::Options,
    template<typename, int> class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>
  struct CartesianProductOperationVariantTpl;
  typedef CartesianProductOperationVariantTpl<
    context::Scalar,
    context::Options,
    LieGroupCollectionDefaultTpl>
    CartesianProductOperationVariant;
 
  template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
  struct traits<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>>
  {
    typedef _Scalar Scalar;
    enum
    {
      Options = _Options,
      NQ = Eigen::Dynamic,
      NV = Eigen::Dynamic
    };
  };
 
  template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
  struct CartesianProductOperationVariantTpl
  : public LieGroupBase<
      CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>>
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperationVariantTpl);
 
    typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;
    typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;
    typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;
 
    CartesianProductOperationVariantTpl()
    : m_nq(0)
    , m_nv(0)
    , lg_nqs(0)
    , lg_nvs(0)
    , m_neutral(0) {};
 
    explicit CartesianProductOperationVariantTpl(const LieGroupGeneric & lg)
    : m_nq(0)
    , m_nv(0)
    , lg_nqs(0)
    , lg_nvs(0)
    , m_neutral(0)
    {
      append(lg);
    };
 
    CartesianProductOperationVariantTpl(const LieGroupGeneric & lg1, const LieGroupGeneric & lg2)
    : m_nq(0)
    , m_nv(0)
    , lg_nqs(0)
    , lg_nvs(0)
    , m_neutral(0)
    {
      append(lg1);
      append(lg2);
    };
 
    void append(const LieGroupGeneric & lg);
 
    template<typename LieGroupDerived>
    void append(const LieGroupBase<LieGroupDerived> & lg)
    {
      LieGroupGeneric lgGeneric(lg);
      append(lgGeneric);
    }
 
    CartesianProductOperationVariantTpl
    operator*(const CartesianProductOperationVariantTpl & other) const;
 
    CartesianProductOperationVariantTpl &
    operator*=(const CartesianProductOperationVariantTpl & other);
 
    inline CartesianProductOperationVariantTpl & operator*=(const LieGroupGeneric & lg)
    {
      append(lg);
      return *this;
    }
 
    template<typename LieGroupDerived>
    inline CartesianProductOperationVariantTpl &
    operator*=(const LieGroupBase<LieGroupDerived> & lg)
    {
      append<LieGroupDerived>(lg);
      return *this;
    }
 
    int nq() const
    {
      return m_nq;
    }
    int nv() const
    {
      return m_nv;
    }
 
    std::string name() const
    {
      return m_name;
    }
 
    ConfigVector_t neutral() const
    {
      return m_neutral;
    }
 
    template<class ConfigL_t, class ConfigR_t, class Tangent_t>
    void difference_impl(
      const Eigen::MatrixBase<ConfigL_t> & q0,
      const Eigen::MatrixBase<ConfigR_t> & q1,
      const Eigen::MatrixBase<Tangent_t> & d) const;
 
    template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
    void dDifference_impl(
      const Eigen::MatrixBase<ConfigL_t> & q0,
      const Eigen::MatrixBase<ConfigR_t> & q1,
      const Eigen::MatrixBase<JacobianOut_t> & J) 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 ConfigIn_t, class Velocity_t, class ConfigOut_t>
    void integrate_impl(
      const Eigen::MatrixBase<ConfigIn_t> & q,
      const Eigen::MatrixBase<Velocity_t> & v,
      const Eigen::MatrixBase<ConfigOut_t> & qout) const;
 
    template<class Config_t, class Jacobian_t>
    void integrateCoeffWiseJacobian_impl(
      const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) const;
 
    template<class Config_t, class Tangent_t, class JacobianOut_t>
    void dIntegrate_dq_impl(
      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 JacobianOut_t>
    void dIntegrate_dv_impl(
      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_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<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrateTransport_dq_impl(
      const Eigen::MatrixBase<Config_t> & q,
      const Eigen::MatrixBase<Tangent_t> & v,
      const Eigen::MatrixBase<JacobianIn_t> & J_in,
      const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
 
    template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    void dIntegrateTransport_dv_impl(
      const Eigen::MatrixBase<Config_t> & q,
      const Eigen::MatrixBase<Tangent_t> & v,
      const Eigen::MatrixBase<JacobianIn_t> & J_in,
      const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
 
    template<class Config_t, class Tangent_t, class JacobianOut_t>
    void dIntegrateTransport_dq_impl(
      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 dIntegrateTransport_dv_impl(
      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>
    Scalar squaredDistance_impl(
      const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) 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> & qout, const Scalar & prec) const;
 
    template<class Config_t>
    void random_impl(const Eigen::MatrixBase<Config_t> & qout) const;
 
    template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    void randomConfiguration_impl(
      const Eigen::MatrixBase<ConfigL_t> & lower,
      const Eigen::MatrixBase<ConfigR_t> & upper,
      const Eigen::MatrixBase<ConfigOut_t> & qout) 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 CartesianProductOperationVariantTpl & other) const;
 
    template<typename LieGroup1, typename LieGroup2>
    bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;
 
  protected:
    PINOCCHIO_ALIGNED_STD_VECTOR(LieGroupGeneric) liegroups;
    Index m_nq, m_nv;
    std::vector<Index> lg_nqs, lg_nvs;
    std::string m_name;
 
    ConfigVector_t m_neutral;
  };
 
} // namespace pinocchio
 
#include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx>
 
#endif // ifndef __pinocchio_cartesian_product_variant_hpp__