cartesian_acceleration.h

Go to the documentation of this file.

#ifndef OPEN_WALKER_CORE_CARTESIAN_ACCELERATION_REF_H
#define OPEN_WALKER_CORE_CARTESIAN_ACCELERATION_REF_H

#include <ow_core/linear_acceleration_ref.h>
#include <ow_core/angular_acceleration_ref.h>
#include <geometry_msgs/Accel.h>

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

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

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

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

  explicit CartesianAcceleration(const geometry_msgs::Accel& other)
  {
    operator=(other);
  }

  using Base::operator=;

  void operator=(const geometry_msgs::Accel& Xpp)
  {
    linear() = Xpp.linear;
    angular() = Xpp.angular;
  }

  operator geometry_msgs::Accel() const
  {
    geometry_msgs::Accel Xpp;
    Xpp.linear = linear();
    Xpp.angular = angular();
    return Xpp;
  }

  geometry_msgs::Accel toAccelMsg() const
  {
    return static_cast<geometry_msgs::Accel>(*this);
  }

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

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

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

  AngularAccelerationRef<const Base> angular() const
  {
    return AngularAccelerationRef<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_ACCELERATION_REF_H