Coordinate.h

Go to the documentation of this file.

#include "ReferenceFrame.h"

#ifndef _GAMS_POSE_COORDINATE_H_
#define _GAMS_POSE_COORDINATE_H_

#include "gams/GamsExport.h"
#include <string>
#include <cfloat>
#include <utility>
#include "gams/CPP11_compat.h"
#include "madara/knowledge/containers/DoubleVector.h"
#include "madara/knowledge/containers/NativeDoubleVector.h"
#include "ReferenceFrameFwd.h"


#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#include "Eigen/Geometry"
#pragma GCC diagnostic pop
#endif

#define INVAL_COORD DBL_MAX

namespace gams { namespace pose {

namespace units {
  template<typename T>
  struct absolute {};

  struct length {};
  struct velocity {};
  struct acceleration {};
  struct plane_angle {};
  struct angular_velocity {};
  struct angular_acceleration {};

  template<typename T>
  struct base_of
  {
    using type = T;
  };

  template<typename T>
  struct base_of<absolute<T>>
  {
    using type = T;
  };

  template<typename L, typename R>
  struct same_base :
    std::is_same<typename base_of<L>::type,
                 typename base_of<R>::type> {};
}

template<typename Units>
struct unit_traits
{
  static_assert(sizeof(Units) < 0, "Unit type not supported by BasicVector");
};

struct default_unit_traits
{
  class storage_mixin
  {
  private:
    Eigen::Vector3d vec_;

  public:
    storage_mixin()
      : vec_(0, 0, 0) {}

    storage_mixin(double x, double y)
      : vec_(x, y, 0) {}

    storage_mixin(double x, double y, double z)
      : vec_(x, y, z) {}

    template<typename T, size_t N,
      typename std::enable_if<std::is_floating_point<T>::value, int>::type = 0>
    storage_mixin(T (&a)[N])
      : vec_(N>0 ? a[0] : 0, N>1 ? a[1] : 0, N>2 ? a[2] : 0) {}

    explicit storage_mixin(const Eigen::Vector3d &vec) : vec_(vec) {}

    explicit storage_mixin(const madara::knowledge::containers::DoubleVector &v)
      : vec_(v.size() > 0 ? v[0] : 0,
             v.size() > 1 ? v[1] : 0,
             v.size() > 2 ? v[2] : 0) {}

    explicit storage_mixin(const madara::knowledge::containers::NativeDoubleVector &v)
      : vec_(v.size() > 0 ? v[0] : 0,
             v.size() > 1 ? v[1] : 0,
             v.size() > 2 ? v[2] : 0) {}

    Eigen::Vector3d &vec() { return vec_; }
    const Eigen::Vector3d &vec() const { return vec_; }
  };
};

struct default_positional_unit_traits : default_unit_traits
{
  static const bool positional = true;
};

class Quaternion;

inline Eigen::Vector3d quat_to_axis_angle(const Eigen::Quaterniond &quat)
{
  double norm = sqrt(quat.x() * quat.x() +
                     quat.y() * quat.y() +
                     quat.z() * quat.z());
  double angle = 2 * atan2(norm, quat.w());
  double sin_half_angle = sin(angle / 2);
  if(sin_half_angle < 1e-10)
  {
    return {0, 0, 0};
  }
  return {(quat.x() / sin_half_angle) * angle,
          (quat.y() / sin_half_angle) * angle,
          (quat.z() / sin_half_angle) * angle};
}

struct default_rotational_unit_traits : default_unit_traits
{
  static const bool positional = false;

  class storage_mixin : public default_unit_traits::storage_mixin
  {
  public:
    using Base = default_unit_traits::storage_mixin;
    using Base::Base;

    storage_mixin() = default;

    storage_mixin(const Quaternion &quat);

    template<typename Units>
    storage_mixin(double rx, double ry, double rz, Units units)
      : Base(units.to_radians(rx),
             units.to_radians(ry),
             units.to_radians(rz)) {}

    template<typename T, typename Units, size_t N,
      typename std::enable_if<std::is_floating_point<T>::value, int>::type = 0>
    storage_mixin(T (&a)[N], Units units)
      : Base(
          N>0 ? units.to_radians(a[0]) : 0,
          N>1 ? units.to_radians(a[1]) : 0,
          N>2 ? units.to_radians(a[2]) : 0) {}

  public:
    storage_mixin(const Eigen::Quaterniond &quat)
      : Base(quat_to_axis_angle(quat)) {}
  };
};

template<typename Derived>
struct basic_positional_mixin
{
  Derived &self() { return static_cast<Derived&>(*this); }
  const Derived &self() const { return static_cast<const Derived&>(*this); }

  double x() const { return self().vec()[0]; }
  double y() const { return self().vec()[1]; }
  double z() const { return self().vec()[2]; }

  double x(double v) { return (self().vec()[0] = v); }
  double y(double v) { return (self().vec()[1] = v); }
  double z(double v) { return (self().vec()[2] = v); }
};

template<>
struct unit_traits<units::absolute<units::length>>
  : default_positional_unit_traits
{
  static const bool free = false;

  template<typename Derived>
  struct mixin : basic_positional_mixin<Derived> {
    using Base = basic_positional_mixin<Derived>;
    using Base::x;
    using Base::y;
    using Base::z;
    using Base::self;

    double lng () const { return x(); }
    double lon () const { return x(); }
    double longitude () const { return x(); }

    double lat () const { return y(); }
    double latitude () const { return y(); }

    double alt () const { return -z(); }
    double altitude () const { return -z(); }

    double lng (double v) { return x(v); }
    double lon (double v) { return x(v); }
    double longitude (double v) { return x(v); }

    double lat (double v) { return y(v); }
    double latitude (double v) { return y(v); }

    double alt (double v) { return z(-v); }
    double altitude (double v) { return z(-v); }

    Eigen::Vector3d &pos_vec() { return self().vec(); }
    const Eigen::Vector3d &pos_vec() const { return self().vec(); }
  };
};

template<>
struct unit_traits<units::length>
  : default_positional_unit_traits
{
  static const bool free = true;

  template<typename Derived>
  struct mixin : basic_positional_mixin<Derived> {
    using Base = basic_positional_mixin<Derived>;
    using Base::self;

    Eigen::Vector3d &dis_vec() { return self().vec(); }
    const Eigen::Vector3d &dis_vec() const { return self().vec(); }
  };
};

template<>
struct unit_traits<units::velocity>
  : default_positional_unit_traits
{
  static const bool free = true;

  template<typename Derived>
  struct mixin {
    Derived &self() { return static_cast<Derived&>(*this); }
    const Derived &self() const { return static_cast<const Derived&>(*this); }

    double dx() const { return self().vec()[0]; }
    double dy() const { return self().vec()[1]; }
    double dz() const { return self().vec()[2]; }

    double dx(double v) { return (self().vec()[0] = v); }
    double dy(double v) { return (self().vec()[1] = v); }
    double dz(double v) { return (self().vec()[2] = v); }

    Eigen::Vector3d &vel_vec() { return self().vec(); }
    const Eigen::Vector3d &vel_vec() const { return self().vec(); }
  };
};

template<>
struct unit_traits<units::acceleration>
  : default_positional_unit_traits
{
  static const bool positional = true;
  static const bool free = true;

  template<typename Derived>
  struct mixin {
    Derived &self() { return static_cast<Derived&>(*this); }
    const Derived &self() const { return static_cast<const Derived&>(*this); }

    double ddx() const { return self().vec()[0]; }
    double ddy() const { return self().vec()[1]; }
    double ddz() const { return self().vec()[2]; }

    double ddx(double v) { return (self().vec()[0] = v); }
    double ddy(double v) { return (self().vec()[1] = v); }
    double ddz(double v) { return (self().vec()[2] = v); }

    Eigen::Vector3d &acc_vec() { return self().vec(); }
    const Eigen::Vector3d &acc_vec() const { return self().vec(); }
  };
};

template<typename Derived>
struct common_rotational_mixin
{
  Derived &self() { return static_cast<Derived&>(*this); }
  const Derived &self() const { return static_cast<const Derived&>(*this); }

  double angle_to(const Derived &target) const
  {
    return self().distance_to(target);
  }

  template<typename U>
  double angle_to (const Derived &target, U u) const
  {
    return u.from_radians (self().angle_to (target));
  }
};

template<typename Derived>
struct basic_rotational_mixin : common_rotational_mixin<Derived>
{
  using Base = common_rotational_mixin<Derived>;
  using Base::self;

  double rx() const { return self().vec()[0]; }
  double ry() const { return self().vec()[1]; }
  double rz() const { return self().vec()[2]; }

  double rx(double v) { return (self().vec()[0] = v); }
  double ry(double v) { return (self().vec()[1] = v); }
  double rz(double v) { return (self().vec()[2] = v); }

  Eigen::Quaterniond into_quat() const
  {
    double magnitude = sqrt(rx() * rx() + ry() * ry() + rz() * rz());
    if(magnitude == 0)
    {
      return {1, 0, 0, 0};
    }
    double half_mag = magnitude / 2;
    double cos_half_mag = cos(half_mag);
    double sin_half_mag = sin(half_mag);
    return {cos_half_mag,
            (rx() / magnitude) * sin_half_mag,
            (ry() / magnitude) * sin_half_mag,
            (rz() / magnitude) * sin_half_mag};
  }

  void from_quat(const Eigen::Quaterniond &quat)
  {
    this->vec() = quat_to_axis_angle(quat);
  }

  template<typename Other>
  auto slerp(double scale, const Other &other) ->
    typename std::decay<decltype(other.into_quat(),
        std::declval<Derived>())>::type
  {
    return Derived(into_quat().slerp(scale, other.into_quat()));
  }

  template<typename Other>
  auto slerp(const Other &other, double scale) ->
    typename std::decay<decltype(other.into_quat(),
        std::declval<Derived>())>::type
  {
    return slerp(scale, other);
  }

  template<typename Other>
  auto slerp_this(double scale, const Other &other) ->
    decltype(other.into_quat(), void())
  {
    self() = Derived(into_quat().slerp(scale, other.into_quat()));
  }

  template<typename Other>
  auto slerp_this(const Other &other, double scale) ->
    decltype(other.into_quat(), void())
  {
    return slerp_this(scale, other);
  }
};

template<>
struct unit_traits<units::absolute<units::plane_angle>>
  : default_rotational_unit_traits
{
  static const bool free = false;

  template<typename Derived>
  struct mixin : basic_rotational_mixin<Derived> {
    using Base = basic_rotational_mixin<Derived>;
    using Base::self;

    Eigen::Vector3d &ori_vec() { return self().vec(); }
    const Eigen::Vector3d &ori_vec() const { return self().vec(); }
  };
};

template<>
struct unit_traits<units::plane_angle>
  : default_rotational_unit_traits
{
  static const bool free = true;

  template<typename Derived>
  struct mixin : basic_rotational_mixin<Derived> {
    using Base = basic_rotational_mixin<Derived>;
    using Base::self;

    Eigen::Vector3d &rot_vec() { return self().vec(); }
    const Eigen::Vector3d &rot_vec() const { return self().vec(); }
  };
};

template<>
struct unit_traits<units::angular_velocity>
  : default_rotational_unit_traits
{
  static const bool free = true;

  template<typename Derived>
  struct mixin {
    Derived &self() { return static_cast<Derived&>(*this); }
    const Derived &self() const { return static_cast<const Derived&>(*this); }

    double drx() const { return self().vec()[0]; }
    double dry() const { return self().vec()[1]; }
    double drz() const { return self().vec()[2]; }

    double drx(double v) { return (self().vec()[0] = v); }
    double dry(double v) { return (self().vec()[1] = v); }
    double drz(double v) { return (self().vec()[2] = v); }

    Eigen::Vector3d &ang_vel_vec() { return self().vec(); }
    const Eigen::Vector3d &ang_vel_vec() const { return self().vec(); }
  };
};

template<>
struct unit_traits<units::angular_acceleration>
  : default_rotational_unit_traits
{
  static const bool positional = true;
  static const bool free = true;

  template<typename Derived>
  struct mixin {
    Derived &self() { return static_cast<Derived&>(*this); }
    const Derived &self() const { return static_cast<const Derived&>(*this); }

    double ddrx() const { return self().vec()[0]; }
    double ddry() const { return self().vec()[1]; }
    double ddrz() const { return self().vec()[2]; }

    double ddrx(double v) { return (self().vec()[0] = v); }
    double ddry(double v) { return (self().vec()[1] = v); }
    double ddrz(double v) { return (self().vec()[2] = v); }

    Eigen::Vector3d &ang_acc_vec() { return self().vec(); }
    const Eigen::Vector3d &ang_acc_vec() const { return self().vec(); }
  };
};

template<typename Derived, typename Units>
class BasicVector
  : public unit_traits<Units>::storage_mixin,
    public unit_traits<Units>::template mixin<Derived>
{
private:
  Eigen::Vector3d vec_;

  using traits = unit_traits<Units>;
  using storage_mixin = typename unit_traits<Units>::storage_mixin;
  using mixin = typename unit_traits<Units>::template mixin<Derived>;

public:
  using storage_mixin::storage_mixin;
  using storage_mixin::vec;

  using mixin::self;

  using derived_type = Derived;
  using units_type = Units;

  BasicVector() = default;

  template<typename Derived2>
  BasicVector(const BasicVector<Derived2, Units> &v) : storage_mixin(v.vec()) {}

  static constexpr bool positional() { return traits::positional; }

  static constexpr bool rotational() { return !positional(); }

  static constexpr bool free() { return traits::free; }

  static constexpr bool fixed() { return !free(); }

  size_t size() const { return (size_t)vec().size(); }

  double get(size_t i) const { return vec()[i]; }

  double set(size_t i, double v) { return (vec()[i] = v); }

  bool is_set() const
  {
    for (int i = 0; i < vec().size(); ++i) {
      if (vec()[i] != INVAL_COORD) {
        return true;
      }
    }
    return false;
  }

  bool is_zero() const
  {
    for (int i = 0; i < vec().size(); ++i) {
      if (vec()[i] != 0) {
        return false;
      }
    }
    return true;
  }

  template<typename ContainType>
  void to_array(ContainType &out) const
  {
    for(size_t i = 0; i < size(); i++)
    {
      out[i] = get(i);
    }
  }

  template<typename ContainType>
  void from_array(const ContainType &in)
  {
    for(size_t i = 0; i < size(); i++)
    {
      set(i, in[i]);
    }
  }

  std::string to_string (
      const std::string & delimiter = ",",
      const std::string & unset_identifier = "<unset>") const
  {
    std::stringstream buffer;
    bool first = true;

    for (int i = 0; i < vec().size(); ++i)
    {
      auto x = vec()[i];

      if (!first) {
        buffer << delimiter;
      } else {
        first = false;
      }

      if (x != INVAL_COORD)
        buffer << x;
      else
        buffer << unset_identifier;
    }

    return buffer.str ();
  }

  void to_container (
    madara::knowledge::containers::NativeDoubleVector &container) const
  {
    container.set (0, this->get (0));
    container.set (1, this->get (1));
    container.set (2, this->get (2));
  }

  void from_container (
    const madara::knowledge::containers::NativeDoubleVector &container)
  {
    this->set (0, container[0]);
    this->set (1, container[1]);
    this->set (2, container[2]);
  }

  template<typename Other>
  double dot(const BasicVector<Other, Units> &other) const
  {
    return this->vec().dot(other.vec());
  }

  template<typename Other>
  Derived cross(const BasicVector<Other, Units> &other) const
  {
    Derived ret = this->self();
    ret.vec() = this->vec().cross(other.vec());
    return ret;
  }

  double norm() const
  {
    return this->vec().norm();
  }

  double squaredNorm() const
  {
    return this->vec().squaredNorm();
  }

  Derived normalized() const
  {
    Derived ret = this->self();
    ret.vec().normalize();
    return ret;
  }

  /*template<typename Derived2>
  bool approximately_equal(const BasicVector<Derived2, Units> &other,
      double epsilon) const
  {
    return std::fabs(self().distance_to(other.self())) < epsilon;
  }*/

  /*template<typename Derived2>
  double distance_to(const BasicVector<Derived2, Units> &target) const {
    return (target.vec() - vec()).norm();
  }*/
};

} }

#include "Coordinate.inl"

#include "Framed.h"
#include "Stamped.h"

namespace gams { namespace pose {

#define GAMS_POSE_MAKE_COMPOSITE_VEC_SCALAR_OP(op) \
template<typename Derived, typename Units, typename Scalar> \
inline auto operator op##=(BasicVector<Derived, Units> &vec, Scalar scalar) -> \
  typename std::enable_if<std::is_arithmetic<Scalar>::value && \
                          BasicVector<Derived, Units>::free(), \
                          typename BasicVector<Derived, Units>::derived_type&>::type \
{ \
  vec.vec() op##= scalar; \
  return vec.self(); \
}

#define GAMS_POSE_MAKE_BINARY_VEC_SCALAR_OP(op) \
template<typename Derived, typename Units, typename Scalar> \
inline auto operator op(const BasicVector<Derived, Units> &vec, Scalar scalar) -> \
  typename std::enable_if<std::is_arithmetic<Scalar>::value && \
                          BasicVector<Derived, Units>::free(), \
                          typename BasicVector<Derived, Units>::derived_type>::type \
{ \
  typename BasicVector<Derived, Units>::derived_type ret = vec.self(); \
  ret op##= scalar; \
  return ret; \
} \
\
template<typename Derived, typename Units, typename Scalar> \
inline auto operator op(Scalar scalar, const BasicVector<Derived, Units> &vec) -> \
  decltype(vec op scalar) \
{ \
  return vec op scalar; \
}

#define GAMS_POSE_MAKE_VEC_SCALAR_OPS(op) \
GAMS_POSE_MAKE_COMPOSITE_VEC_SCALAR_OP(op) \
GAMS_POSE_MAKE_BINARY_VEC_SCALAR_OP(op)

GAMS_POSE_MAKE_VEC_SCALAR_OPS(*)
GAMS_POSE_MAKE_VEC_SCALAR_OPS(/)

#define GAMS_POSE_MAKE_COMPOSITE_VECS_OP(op) \
template<typename LDerived, \
         typename RDerived, typename Units> \
inline auto operator op##=(BasicVector<LDerived, Units> &lhs, \
              const Eigen::MatrixBase<RDerived> &rhs) -> \
  typename std::enable_if<unit_traits<Units>::positional, \
                          LDerived &>::type \
{ \
  lhs.vec() op##= rhs; \
  return lhs.self(); \
} \
\
template<typename LDerived, \
         typename RDerived, typename Units> \
inline auto operator op##=(BasicVector<LDerived, Units> &lhs, \
                 const BasicVector<RDerived, Units> &rhs) -> \
  typename std::enable_if<unit_traits<Units>::positional && \
                          unit_traits<Units>::free, LDerived &>::type \
{ \
  lhs.vec() op##= rhs.vec(); \
  return lhs.self(); \
} \
\
template<typename LDerived, \
         typename RDerived, typename Units> \
inline auto operator op##=(BasicVector<LDerived, units::absolute<Units>> &lhs, \
                 const BasicVector<RDerived, Units> &rhs) -> \
  typename std::enable_if<unit_traits<Units>::positional && \
                          unit_traits<Units>::free, LDerived &>::type \
{ \
  lhs.vec() op##= rhs.vec(); \
  return lhs.self(); \
} \

GAMS_POSE_MAKE_COMPOSITE_VECS_OP(+)
GAMS_POSE_MAKE_COMPOSITE_VECS_OP(-)

template<typename LDerived, typename LUnits,
         typename RDerived, typename RUnits>
inline auto operator+(const BasicVector<LDerived, LUnits> &lhs,
               const BasicVector<RDerived, RUnits> &rhs) ->
  typename std::enable_if<RDerived::free(), LDerived>::type
{
  LDerived ret = lhs.self();
  ret += rhs;
  return ret;
}

template<typename LDerived, typename LUnits,
         typename RDerived, typename RUnits>
inline auto operator+(const BasicVector<LDerived, LUnits> &lhs,
               const BasicVector<RDerived, RUnits> &rhs) ->
  typename std::enable_if<LDerived::free() && RDerived::fixed(), RDerived>::type
{
  RDerived ret = rhs.self();
  ret += lhs;
  return ret;
}

template<typename LDerived, typename LUnits,
         typename RDerived, typename RUnits>
inline auto operator-(const BasicVector<LDerived, LUnits> &lhs,
               const BasicVector<RDerived, RUnits> &rhs) ->
  typename std::enable_if<LDerived::free() && RDerived::free(), LDerived>::type
{
  LDerived ret = lhs.self();
  ret -= rhs;
  return ret;
}

template<typename T>
struct fixed_into_free {};

template<typename LDerived, typename LUnits,
         typename RDerived, typename RUnits>
inline auto operator-(const BasicVector<LDerived, LUnits> &lhs,
               const BasicVector<RDerived, RUnits> &rhs) ->
  typename std::enable_if<LDerived::fixed() && RDerived::fixed(),
    typename fixed_into_free<LDerived>::type>::type
{
  typename fixed_into_free<LDerived>::type ret(lhs);
  ret.vec() -= rhs.vec();
  return ret;
}

#define GAMS_POSE_MAKE_COORDINATE_COMPARE_OPS(op) \
  template<typename LDerived, typename RDerived, typename Units> \
  inline bool operator op ( \
      const BasicVector<LDerived, Units> &lhs, \
      const BasicVector<RDerived, Units> &rhs) \
  { \
    for (int i = 0; i < lhs.vec().size(); ++i) { \
      if (!(lhs.vec()[i] op rhs.vec()[i])) { \
        return false; \
      } \
    } \
    return true; \
  } \
 \
  template<typename LDerived, typename RDerived, typename Units> \
  inline bool operator op ( \
      const Framed<BasicVector<LDerived, Units>> &lhs, \
      const Framed<BasicVector<RDerived, Units>> &rhs) \
  { \
    return lhs.frame() op rhs.frame() && \
      static_cast<const BasicVector<LDerived, Units> &>(lhs) op \
      static_cast<const BasicVector<RDerived, Units> &>(rhs); \
  } \
 \
  template<typename LDerived, typename RDerived, typename Units> \
  inline bool operator op ( \
      const Stamped<Framed<BasicVector<LDerived, Units>>> &lhs, \
      const Stamped<Framed<BasicVector<RDerived, Units>>> &rhs) \
  { \
    return lhs.time() op rhs.time() && \
      static_cast<const Framed<BasicVector<LDerived, Units> &>>(lhs) op \
      static_cast<const Framed<BasicVector<RDerived, Units> &>>(rhs); \
  }

GAMS_POSE_MAKE_COORDINATE_COMPARE_OPS(==)
GAMS_POSE_MAKE_COORDINATE_COMPARE_OPS(!=)
GAMS_POSE_MAKE_COORDINATE_COMPARE_OPS(<)
GAMS_POSE_MAKE_COORDINATE_COMPARE_OPS(<=)
GAMS_POSE_MAKE_COORDINATE_COMPARE_OPS(>)
GAMS_POSE_MAKE_COORDINATE_COMPARE_OPS(>=)

#define GAMS_POSE_MAKE_COORDINATE_TYPE(name, units) \
class name##Vector : \
  public BasicVector<name##Vector, units> \
{ \
public: \
  using Base = BasicVector<name##Vector, units>; \
  using Base::Base; \
  static constexpr const char * type_name = #name "Vector"; \
}; \
\
class name : \
  public Framed<BasicVector<name, units>> \
{ \
public: \
  using Base = Framed<BasicVector<name, units>>; \
  using Base::Base; \
  static constexpr const char * type_name = #name; \
  operator name##Vector () const { return name##Vector(this->vec()); } \
  name(name##Vector o) : Base(o.vec()) {} \
  name() = default; \
}; \
\
class Stamped##name : \
  public Stamped<Framed<BasicVector<Stamped##name, units>>> \
{ \
public: \
  using Base = Stamped<Framed<BasicVector<Stamped##name, units>>>; \
  using Base::Base; \
  static constexpr const char * type_name = "Stamped" #name; \
  operator name##Vector () const { return name##Vector(this->vec()); } \
  operator name () const { return name(this->frame(), this->vec()); } \
  Stamped##name(name##Vector o) : Base(o.vec()) {} \
  Stamped##name(name o) : Base(o.frame(), o.vec()) {} \
  Stamped##name() = default; \
}

class Position;

class Position;

class Velocity;

class Accelerations;

class Orientation;

class Rotation;

class AngularVelocity;

class AngularAccelerations;

GAMS_POSE_MAKE_COORDINATE_TYPE(Position, units::absolute<units::length>);
GAMS_POSE_MAKE_COORDINATE_TYPE(Displacement, units::length);
GAMS_POSE_MAKE_COORDINATE_TYPE(Velocity, units::velocity);
GAMS_POSE_MAKE_COORDINATE_TYPE(Accleration, units::acceleration);

GAMS_POSE_MAKE_COORDINATE_TYPE(Orientation, units::absolute<units::plane_angle>);
GAMS_POSE_MAKE_COORDINATE_TYPE(Rotation, units::plane_angle);
GAMS_POSE_MAKE_COORDINATE_TYPE(AngularVelocity, units::angular_velocity);
GAMS_POSE_MAKE_COORDINATE_TYPE(AngularAccleration, units::angular_acceleration);

template<>
struct fixed_into_free<PositionVector>
{
  using type = DisplacementVector;
};

template<>
struct fixed_into_free<Position>
{
  using type = Displacement;
};

template<>
struct fixed_into_free<StampedPosition>
{
  using type = StampedDisplacement;
};

template<>
struct fixed_into_free<OrientationVector>
{
  using type = RotationVector;
};

template<>
struct fixed_into_free<Orientation>
{
  using type = Rotation;
};

template<>
struct fixed_into_free<StampedOrientation>
{
  using type = StampedRotation;
};

inline std::ostream &operator<<(std::ostream &o, const Eigen::Vector3d &v)
{
  bool first = true;
  for (int i = 0; i < v.size(); ++i)
  {
    auto x = v[i];

    if (!first) {
      o << ",";
    } else {
      first = false;
    }

    if (x != INVAL_COORD) {
      o << x;
    } else {
      o << "<unset>";
    }
  }
  return o;
}

template<typename Derived, typename Units>
inline std::ostream &operator<<(std::ostream &o, const BasicVector<Derived, Units> &v)
{
  o << Derived::type_name << "(";
  o << v.vec();
  o << ")";
  return o;
}

} }

#endif