force-ref.hpp

//
// Copyright (c) 2017-2019 CNRS INRIA
//
 
#ifndef __pinocchio_spatial_force_ref_hpp__
#define __pinocchio_spatial_force_ref_hpp__
 
namespace pinocchio
{
 
  template<typename Vector6ArgType>
  struct traits<ForceRef<Vector6ArgType>>
  {
    typedef typename Vector6ArgType::Scalar Scalar;
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
    enum
    {
      LINEAR = 0,
      ANGULAR = 3,
      Options = Vector6::Options
    };
    typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
    typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
    typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
    typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
    typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
    typedef ForceTpl<Scalar, Options> ForcePlain;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
    typedef DataRefType ToVectorReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
    typedef ConstDataRefType ToVectorConstReturnType;
    typedef ForceRef<Vector6ArgType> ForceRefType;
 
  }; // traits ForceRef
 
  template<typename Vector6ArgType>
  struct SE3GroupAction<ForceRef<Vector6ArgType>>
  {
    typedef typename traits<ForceRef<Vector6ArgType>>::ForcePlain ReturnType;
  };
 
  template<typename Vector6ArgType, typename MotionDerived>
  struct MotionAlgebraAction<ForceRef<Vector6ArgType>, MotionDerived>
  {
    typedef typename traits<ForceRef<Vector6ArgType>>::ForcePlain ReturnType;
  };
 
  template<typename Vector6ArgType>
  class ForceRef : public ForceDense<ForceRef<Vector6ArgType>>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef ForceDense<ForceRef> Base;
    typedef typename traits<ForceRef>::DataRefType DataRefType;
    FORCE_TYPEDEF_TPL(ForceRef);
 
    using Base::operator=;
    using Base::operator==;
    using Base::operator!=;
 
    ForceRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) f_like)
    : m_ref(f_like)
    {
      EIGEN_STATIC_ASSERT(
        Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
      assert(f_like.size() == 6);
    }
 
    ForceRef(const ForceRef & other)
    : m_ref(other.m_ref)
    {
    }
 
    ToVectorConstReturnType toVector_impl() const
    {
      return m_ref;
    }
    ToVectorReturnType toVector_impl()
    {
      return m_ref;
    }
 
    // Getters
    ConstAngularType angular_impl() const
    {
      return ConstAngularType(m_ref.derived(), ANGULAR);
    }
    ConstLinearType linear_impl() const
    {
      return ConstLinearType(m_ref.derived(), LINEAR);
    }
    AngularType angular_impl()
    {
      return m_ref.template segment<3>(ANGULAR);
    }
    LinearType linear_impl()
    {
      return m_ref.template segment<3>(LINEAR);
    }
 
    template<typename V3>
    void angular_impl(const Eigen::MatrixBase<V3> & w)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
      angular_impl() = w;
    }
 
    template<typename V3>
    void linear_impl(const Eigen::MatrixBase<V3> & v)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
      linear_impl() = v;
    }
 
    ForceRef & ref()
    {
      return *this;
    }
 
  protected:
    DataRefType m_ref;
 
  }; // class ForceRef<Vector6Like>
 
  template<typename Vector6ArgType>
  struct traits<ForceRef<const Vector6ArgType>>
  {
    typedef typename Vector6ArgType::Scalar Scalar;
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
    enum
    {
      LINEAR = 0,
      ANGULAR = 3,
      Options = Vector6::Options
    };
    typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
    typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
    typedef Matrix6 ActionMatrixType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
    typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
    typedef ConstLinearType LinearType;
    typedef ConstAngularType AngularType;
    typedef ForceTpl<Scalar, Options> ForcePlain;
    typedef ForcePlain PlainReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
    typedef ConstDataRefType ToVectorConstReturnType;
    typedef ConstDataRefType DataRefType;
    typedef DataRefType ToVectorReturnType;
    typedef ForceRef<const Vector6ArgType> ForceRefType;
 
  }; // traits ForceRef<const Vector6ArgType>
 
  template<typename Vector6ArgType>
  class ForceRef<const Vector6ArgType> : public ForceDense<ForceRef<const Vector6ArgType>>
  {
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    typedef ForceDense<ForceRef> Base;
    typedef typename traits<ForceRef>::DataRefType DataRefType;
    FORCE_TYPEDEF_TPL(ForceRef);
 
    ForceRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) f_like)
    : m_ref(f_like)
    {
      EIGEN_STATIC_ASSERT(
        Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
      assert(f_like.size() == 6);
    }
 
    ToVectorConstReturnType toVector_impl() const
    {
      return m_ref;
    }
 
    // Getters
    ConstAngularType angular_impl() const
    {
      return ConstAngularType(m_ref.derived(), ANGULAR);
    }
    ConstLinearType linear_impl() const
    {
      return ConstLinearType(m_ref.derived(), LINEAR);
    }
 
    const ForceRef & ref() const
    {
      return *this;
    }
 
  protected:
    DataRefType m_ref;
 
  }; // class ForceRef<Vector6Like>
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_spatial_force_ref_hpp__