cartesian_velocity.h

Go to the documentation of this file.

#ifndef OPEN_WALKER_CORE_CARTESIAN_VELOCITY_REF_H
#define OPEN_WALKER_CORE_CARTESIAN_VELOCITY_REF_H

#include <ow_core/linear_velocity_ref.h>
#include <ow_core/angular_velocity_ref.h>
#include <geometry_msgs/Twist.h>

namespace ow_core
{
template <typename _Scalar>
class CartesianVelocity :
    public Eigen::Matrix<_Scalar, 6, 1>
{
public:
  typedef _Scalar Scalar;
  typedef Eigen::Matrix<Scalar, 6, 1> Base;

  static const CartesianVelocity<Scalar>& Default()
  {
    static const CartesianVelocity v = Base::Zero();
    return v;
  }

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

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

  explicit CartesianVelocity(const geometry_msgs::Twist& other)
  {
    operator=(other);
  }

  using Base::operator=;

  void operator=(const geometry_msgs::Twist& Xp)
  {
    linear() = Xp.linear;
    angular() = Xp.angular;
  }

  operator geometry_msgs::Twist() const
  {
    geometry_msgs::Twist Xp;
    Xp.linear = linear();
    Xp.angular = angular();
    return Xp;
  }

  geometry_msgs::Twist toTwistMsg() const
  {
    return static_cast<geometry_msgs::Twist>(*this);
  }

  LinearVelocityRef<Base> linear()
  {
    return LinearVelocityRef<Base>(*this);
  }

  LinearVelocityRef<const Base> linear() const
  {
    return LinearVelocityRef<const Base>(*this);
  }

  AngularVelocityRef<Base> angular()
  {
    return AngularVelocityRef<Base>(*this,3);
  }

  AngularVelocityRef<const Base> angular() const
  {
    return AngularVelocityRef<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_VELOCITY_REF_H