AdmittanceVec.h

Go to the documentation of this file.

/*
 * Copyright 2012-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include "EigenTypedef.h"
#include "fwd.h"
#include <type_traits>
 
namespace sva
{
 
template<typename T>
class AdmittanceVec
{
public:
  typedef Eigen::Vector3<T> vector3_t;
  typedef Eigen::Vector6<T> vector6_t;
 
public:
  static AdmittanceVec<T> Zero()
  {
    return AdmittanceVec<T>(vector3_t::Zero(), vector3_t::Zero());
  }
 
public:
  AdmittanceVec() : angular_(), linear_() {}
 
  AdmittanceVec(const vector6_t & vec) : angular_(vec.template head<3>()), linear_(vec.template tail<3>()) {}
 
  AdmittanceVec(const vector3_t & angular, const vector3_t & linear) : angular_(angular), linear_(linear) {}
 
  AdmittanceVec(T angular, T linear) : angular_(angular, angular, angular), linear_(linear, linear, linear) {}
 
  // Accessor
  const vector3_t & angular() const
  {
    return angular_;
  }
 
  vector3_t & angular()
  {
    return angular_;
  }
 
  const vector3_t & linear() const
  {
    return linear_;
  }
 
  vector3_t & linear()
  {
    return linear_;
  }
 
  vector6_t vector() const
  {
    return ((vector6_t() << angular_, linear_).finished());
  }
 
  template<typename T2>
  AdmittanceVec<T2> cast() const
  {
    return AdmittanceVec<T2>(angular_.template cast<T2>(), linear_.template cast<T2>());
  }
 
  // Operators
  AdmittanceVec<T> operator+(const AdmittanceVec<T> & av) const
  {
    return AdmittanceVec<T>(angular_ + av.angular_, linear_ + av.linear_);
  }
 
  AdmittanceVec<T> & operator+=(const AdmittanceVec<T> & av)
  {
    angular_ += av.angular_;
    linear_ += av.linear_;
    return *this;
  }
 
  template<typename T2, typename std::enable_if<std::is_arithmetic<T2>::value, int>::type = 0>
  AdmittanceVec<T> operator*(T2 scalar) const
  {
    return AdmittanceVec<T>(scalar * angular_, scalar * linear_);
  }
 
  template<typename T2>
  AdmittanceVec<T> & operator*=(T2 scalar)
  {
    angular_ *= scalar;
    linear_ *= scalar;
    return *this;
  }
 
  template<typename T2>
  AdmittanceVec<T> operator/(T2 scalar) const
  {
    return AdmittanceVec<T>(angular_ / scalar, linear_ / scalar);
  }
 
  template<typename T2>
  AdmittanceVec<T> & operator/=(T2 scalar)
  {
    angular_ /= scalar;
    linear_ /= scalar;
    return *this;
  }
 
  bool operator==(const AdmittanceVec<T> & av) const
  {
    return (angular_ == av.angular_) && (linear_ == av.linear_);
  }
 
  bool operator!=(const AdmittanceVec<T> & av) const
  {
    return !(*this == av);
  }
 
private:
  vector3_t angular_;
  vector3_t linear_;
};
 
template<typename T, typename T2>
inline AdmittanceVec<T> operator*(T2 scalar, const AdmittanceVec<T> & av)
{
  return av * scalar;
}
 
template<typename T>
inline MotionVec<T> operator*(const AdmittanceVec<T> & av, const ForceVec<T> & fv)
{
  return MotionVec<T>(av.angular().cwiseProduct(fv.couple()), av.linear().cwiseProduct(fv.force()));
}
 
template<typename T>
inline MotionVec<T> operator*(const ForceVec<T> & fv, const AdmittanceVec<T> & av)
{
  return av * fv;
}
 
template<typename T>
inline std::ostream & operator<<(std::ostream & out, const AdmittanceVec<T> & av)
{
  out << av.vector().transpose();
  return out;
}
 
} // namespace sva