ImpedanceVec.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 ImpedanceVec
{
public:
  typedef Eigen::Vector3<T> vector3_t;
  typedef Eigen::Vector6<T> vector6_t;
 
public:
  static ImpedanceVec<T> Zero()
  {
    return ImpedanceVec<T>(vector3_t::Zero(), vector3_t::Zero());
  }
 
public:
  ImpedanceVec() : angular_(), linear_() {}
 
  ImpedanceVec(const vector6_t & vec) : angular_(vec.template head<3>()), linear_(vec.template tail<3>()) {}
 
  ImpedanceVec(const vector3_t & angular, const vector3_t & linear) : angular_(angular), linear_(linear) {}
 
  ImpedanceVec(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>
  ImpedanceVec<T2> cast() const
  {
    return ImpedanceVec<T2>(angular_.template cast<T2>(), linear_.template cast<T2>());
  }
 
  // Operators
  ImpedanceVec<T> operator+(const ImpedanceVec<T> & iv) const
  {
    return ImpedanceVec<T>(angular_ + iv.angular_, linear_ + iv.linear_);
  }
 
  ImpedanceVec<T> & operator+=(const ImpedanceVec<T> & iv)
  {
    angular_ += iv.angular_;
    linear_ += iv.linear_;
    return *this;
  }
 
  template<typename T2, typename std::enable_if<std::is_arithmetic<T2>::value, int>::type = 0>
  ImpedanceVec<T> operator*(T2 scalar) const
  {
    return ImpedanceVec<T>(scalar * angular_, scalar * linear_);
  }
 
  template<typename T2>
  ImpedanceVec<T> & operator*=(T2 scalar)
  {
    angular_ *= scalar;
    linear_ *= scalar;
    return *this;
  }
 
  template<typename T2>
  ImpedanceVec<T> operator/(T2 scalar) const
  {
    return ImpedanceVec<T>(angular_ / scalar, linear_ / scalar);
  }
 
  template<typename T2>
  ImpedanceVec<T> & operator/=(T2 scalar)
  {
    angular_ /= scalar;
    linear_ /= scalar;
    return *this;
  }
 
  bool operator==(const ImpedanceVec<T> & iv) const
  {
    return (angular_ == iv.angular_) && (linear_ == iv.linear_);
  }
 
  bool operator!=(const ImpedanceVec<T> & iv) const
  {
    return !(*this == iv);
  }
 
private:
  vector3_t angular_;
  vector3_t linear_;
};
 
template<typename T, typename T2>
inline ImpedanceVec<T> operator*(T2 scalar, const ImpedanceVec<T> & iv)
{
  return iv * scalar;
}
 
template<typename T>
inline ForceVec<T> operator*(const ImpedanceVec<T> & iv, const MotionVec<T> & mv)
{
  return ForceVec<T>(iv.angular().cwiseProduct(mv.angular()), iv.linear().cwiseProduct(mv.linear()));
}
 
template<typename T>
inline ForceVec<T> operator*(const MotionVec<T> & mv, const ImpedanceVec<T> & iv)
{
  return iv * mv;
}
 
template<typename T>
inline std::ostream & operator<<(std::ostream & out, const ImpedanceVec<T> & iv)
{
  out << iv.vector().transpose();
  return out;
}
 
} // namespace sva