cartesian_position.h

Go to the documentation of this file.

#ifndef OPEN_WALKER_CORE_CARTESIAN_POSITION_REF_H
#define OPEN_WALKER_CORE_CARTESIAN_POSITION_REF_H

#include <ow_core/angular_position_ref.h>
#include <ow_core/linear_position_ref.h>
#include <geometry_msgs/Pose.h>

namespace ow_core
{
template <typename _Scalar>
class CartesianPosition :
    public Eigen::Matrix<_Scalar, 7, 1>
{
public:
  typedef _Scalar Scalar;
  typedef Eigen::Matrix<Scalar, 7, 1> Base;
  typedef Eigen::Transform<Scalar, 3, Eigen::Affine> Transform;

  static const CartesianPosition& Identity()
  {
    static const CartesianPosition v = (Base() << 0,0,0,1,0,0,0).finished();
    return v;
  }

  static const CartesianPosition& Default()
  {
    return Identity();
  }

public:
  explicit CartesianPosition()
    : Base(Default())
  {
  }

  template <typename OtherDerived>
  CartesianPosition(const Eigen::EigenBase<OtherDerived>& other) :
    Base(other.derived())
  {
  }

  CartesianPosition(const Transform& other)
  {
    operator=(other);
  }

  explicit CartesianPosition(const Scalar& x, const Scalar& y, const Scalar& z, 
                             const Scalar& qw, const Scalar& qx, const Scalar& qy, const Scalar& qz)
  {
    position().x() = x;
    position().y() = y;
    position().z() = z;
    orientation().w() = qw;
    orientation().x() = qx;
    orientation().y() = qy;
    orientation().z() = qz;
  }

  explicit CartesianPosition(const typename Transform::TranslationType& other)
  {
    operator=(other);
  }

  template <typename OtherDerived>
  explicit CartesianPosition(const Eigen::RotationBase<OtherDerived, 3>& other)
  {
    operator=(other);
  }

  explicit CartesianPosition(const tf::Transform& other)
  {
    operator=(other);
  }

  explicit CartesianPosition(const geometry_msgs::Pose& other)
  {
    operator=(other);
  }

  void setIdentity()
  {
    *this = Identity();
  }

  using Base::operator=;

  void operator=(const Transform& T)
  {
    position() = T.translation();
    orientation() = T.linear();
  }

  void operator=(const tf::Transform& T)
  {
    position() = T.getOrigin();
    orientation() = T.getRotation();
  }

  void operator=(const geometry_msgs::Pose& T)
  {
    position() = T.position;
    orientation() = T.orientation;
  }

  template <typename OtherDerived>
  void operator=(const Eigen::RotationBase<OtherDerived, 3>& R)
  {
    position().setZero();
    orientation() = R.toRotationMatrix();
  }

  void operator=(const typename Transform::TranslationType& t)
  {
    position() = t.vector();
    orientation().setIdentity();
  }

  operator Transform() const
  {
    return typename Transform::TranslationType(position())*orientation();
  }

  operator tf::Transform() const
  {
    return tf::Transform(orientation().toQuaternionTF(), position());
  }

  operator geometry_msgs::Pose() const
  {
    geometry_msgs::Pose X;
    X.position = position();
    X.orientation = orientation();
    return X;
  }

  Transform toTransformEigen()
  {
    return static_cast<Transform>(*this);
  }

  tf::Transform toTransformTf()
  {
    return static_cast<tf::Transform>(*this);
  }

  geometry_msgs::Pose toPoseMsg()
  {
    return static_cast<geometry_msgs::Pose>(*this);
  }

  LinearPositionRef<Base> position()
  {
    return LinearPositionRef<Base>(*this);
  }

  LinearPositionRef<const Base> position() const
  {
    return LinearPositionRef<const Base>(*this);
  }

  AngularPositionRef<Base> orientation()
  {
    return AngularPositionRef<Base>(*this,3);
  }

  AngularPositionRef<const Base> orientation() const
  {
    return AngularPositionRef<const Base>(*this,3);
  }

  std::string toString() const
  {
    std::ostringstream out;
    out << this->transpose();
    return out.str();
  }
};

}  // namespace ow_core

#endif  // OPEN_WALKER_CORE_CARTESIAN_POSITION_REF_H