vector-space.hpp

//
// Copyright (c) 2016-2020 CNRS INRIA
//
 
#ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__
#define __pinocchio_multibody_liegroup_vector_space_operation_hpp__
 
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
 
#include <stdexcept>
#include <boost/integer/static_min_max.hpp>
 
namespace pinocchio
{
  template<int Dim, typename Scalar, int Options = context::Options>
  struct VectorSpaceOperationTpl;
 
  template<int Dim, typename _Scalar, int _Options>
  struct traits<VectorSpaceOperationTpl<Dim, _Scalar, _Options>>
  {
    typedef _Scalar Scalar;
    enum
    {
      Options = _Options,
      NQ = Dim,
      NV = Dim
    };
  };
 
  template<int Dim, typename _Scalar, int _Options>
  struct VectorSpaceOperationTpl
  : public LieGroupBase<VectorSpaceOperationTpl<Dim, _Scalar, _Options>>
  {
    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(VectorSpaceOperationTpl);
 
    VectorSpaceOperationTpl(int size = boost::static_signed_max<0, Dim>::value)
    : size_(size)
    {
      assert(size_.value() >= 0);
    }
 
    VectorSpaceOperationTpl(const VectorSpaceOperationTpl & other)
    : Base()
    , size_(other.size_.value())
    {
      assert(size_.value() >= 0);
    }
 
    VectorSpaceOperationTpl & operator=(const VectorSpaceOperationTpl & other)
    {
      size_.setValue(other.size_.value());
      assert(size_.value() >= 0);
      return *this;
    }
 
    Index nq() const
    {
      return size_.value();
    }
    Index nv() const
    {
      return size_.value();
    }
 
    ConfigVector_t neutral() const
    {
      return ConfigVector_t::Zero(size_.value());
    }
 
    std::string name() const
    {
      std::ostringstream oss;
      oss << "R^" << nq();
      return oss.str();
    }
 
    template<class ConfigL_t, class ConfigR_t, class Tangent_t>
    static void difference_impl(
      const Eigen::MatrixBase<ConfigL_t> & q0,
      const Eigen::MatrixBase<ConfigR_t> & q1,
      const Eigen::MatrixBase<Tangent_t> & d)
    {
      PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d) = q1 - q0;
    }
 
    template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
    void dDifference_impl(
      const Eigen::MatrixBase<ConfigL_t> &,
      const Eigen::MatrixBase<ConfigR_t> &,
      const Eigen::MatrixBase<JacobianOut_t> & J) const
    {
      if (arg == ARG0)
        PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J) =
          -JacobianMatrix_t::Identity(size_.value(), size_.value());
      else if (arg == ARG1)
        PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setIdentity();
    }
 
    template<
      ArgumentPosition arg,
      class ConfigL_t,
      class ConfigR_t,
      class JacobianIn_t,
      class JacobianOut_t>
    static void dDifference_product_impl(
      const ConfigL_t &,
      const ConfigR_t &,
      const JacobianIn_t & Jin,
      JacobianOut_t & Jout,
      bool,
      const AssignmentOperatorType op)
    {
      switch (op)
      {
      case SETTO:
        if (arg == ARG0)
          Jout = -Jin;
        else
          Jout = Jin;
        return;
      case ADDTO:
        if (arg == ARG0)
          Jout -= Jin;
        else
          Jout += Jin;
        return;
      case RMTO:
        if (arg == ARG0)
          Jout += Jin;
        else
          Jout -= Jin;
        return;
      }
    }
 
    template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
    static void integrate_impl(
      const Eigen::MatrixBase<ConfigIn_t> & q,
      const Eigen::MatrixBase<Velocity_t> & v,
      const Eigen::MatrixBase<ConfigOut_t> & qout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout) = q + v;
    }
 
    template<class Config_t, class Jacobian_t>
    static void integrateCoeffWiseJacobian_impl(
      const Eigen::MatrixBase<Config_t> &, const Eigen::MatrixBase<Jacobian_t> & J)
    {
      PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).setIdentity();
    }
 
    template<class Config_t, class Tangent_t, class JacobianOut_t>
    static 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)
    {
      Eigen::MatrixBase<JacobianOut_t> & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
      switch (op)
      {
      case SETTO:
        Jout.setIdentity();
        break;
      case ADDTO:
        Jout.diagonal().array() += Scalar(1);
        break;
      case RMTO:
        Jout.diagonal().array() -= Scalar(1);
        break;
      default:
        assert(false && "Wrong Op requesed value");
        break;
      }
    }
 
    template<class Config_t, class Tangent_t, class JacobianOut_t>
    static 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)
    {
      Eigen::MatrixBase<JacobianOut_t> & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
      switch (op)
      {
      case SETTO:
        Jout.setIdentity();
        break;
      case ADDTO:
        Jout.diagonal().array() += Scalar(1);
        break;
      case RMTO:
        Jout.diagonal().array() -= Scalar(1);
        break;
      default:
        assert(false && "Wrong Op requesed value");
        break;
      }
    }
 
    template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrate_product_impl(
      const Config_t &,
      const Tangent_t &,
      const JacobianIn_t & Jin,
      JacobianOut_t & Jout,
      bool,
      const ArgumentPosition,
      const AssignmentOperatorType op)
    {
      switch (op)
      {
      case SETTO:
        Jout = Jin;
        break;
      case ADDTO:
        Jout += Jin;
        break;
      case RMTO:
        Jout -= Jin;
        break;
      default:
        assert(false && "Wrong Op requesed value");
        break;
      }
    }
 
    template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrateTransport_dq_impl(
      const Eigen::MatrixBase<Config_t> & /*q*/,
      const Eigen::MatrixBase<Tangent_t> & /*v*/,
      const Eigen::MatrixBase<JacobianIn_t> & Jin,
      const Eigen::MatrixBase<JacobianOut_t> & Jout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
    }
 
    template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
    static void dIntegrateTransport_dv_impl(
      const Eigen::MatrixBase<Config_t> & /*q*/,
      const Eigen::MatrixBase<Tangent_t> & /*v*/,
      const Eigen::MatrixBase<JacobianIn_t> & Jin,
      const Eigen::MatrixBase<JacobianOut_t> & Jout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
    }
 
    template<class Config_t, class Tangent_t, class Jacobian_t>
    static void dIntegrateTransport_dq_impl(
      const Eigen::MatrixBase<Config_t> & /*q*/,
      const Eigen::MatrixBase<Tangent_t> & /*v*/,
      const Eigen::MatrixBase<Jacobian_t> & /*J*/)
    {
    }
 
    template<class Config_t, class Tangent_t, class Jacobian_t>
    static void dIntegrateTransport_dv_impl(
      const Eigen::MatrixBase<Config_t> & /*q*/,
      const Eigen::MatrixBase<Tangent_t> & /*v*/,
      const Eigen::MatrixBase<Jacobian_t> & /*J*/)
    {
    }
 
    // template <class ConfigL_t, class ConfigR_t>
    // static context::Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
    // const Eigen::MatrixBase<ConfigR_t> & q1)
 
    template<class Config_t>
    static void normalize_impl(const Eigen::MatrixBase<Config_t> & /*qout*/)
    {
    }
 
    template<class Config_t>
    static bool
    isNormalized_impl(const Eigen::MatrixBase<Config_t> & /*qout*/, const Scalar & /*prec*/)
    {
      return true;
    }
 
    template<class Config_t>
    static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
    {
      PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).setRandom();
    }
 
    template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
    void randomConfiguration_impl(
      const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
      const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
      const Eigen::MatrixBase<ConfigOut_t> & qout) const
    {
      ConfigOut_t & res = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).derived();
      for (int i = 0; i < nq(); ++i)
      {
        if (check_expression_if_real<Scalar, false>(
              lower_pos_limit[i] == -std::numeric_limits<typename ConfigL_t::Scalar>::infinity()
              || upper_pos_limit[i] == std::numeric_limits<typename ConfigR_t::Scalar>::infinity()))
        {
          std::ostringstream error;
          error << "non bounded limit. Cannot uniformly sample joint at rank " << i;
          throw std::range_error(error.str());
        }
        res[i] =
          lower_pos_limit[i] + ((upper_pos_limit[i] - lower_pos_limit[i]) * rand()) / RAND_MAX;
      }
    }
 
    bool isEqual_impl(const VectorSpaceOperationTpl & other) const
    {
      return size_.value() == other.size_.value();
    }
 
  private:
    Eigen::internal::variable_if_dynamic<Index, Dim> size_;
  }; // struct VectorSpaceOperationTpl
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_multibody_liegroup_vector_space_operation_hpp__