wrench.h

Go to the documentation of this file.

#ifndef OPEN_WALKER_CORE_WRENCH_REF_H
#define OPEN_WALKER_CORE_WRENCH_REF_H

#include <geometry_msgs/Wrench.h>

#include <ow_core/force_ref.h>
#include <ow_core/moment_ref.h>

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

  static const Wrench& Default()
  {
    static const Wrench v = Base::Zero();
    return v;
  }

public:
  explicit Wrench()
  {
  }

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

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

  using Base::operator=;

  void operator=(const geometry_msgs::Wrench& W)
  {
    force() = W.force;
    moment() = W.torque;
  }

  operator geometry_msgs::Wrench() const
  {
    geometry_msgs::Wrench W;
    W.force = force();
    W.torque = moment();
    return W;
  }

  geometry_msgs::Wrench toWrenchMsg() const
  {
    return static_cast<geometry_msgs::Wrench>(*this);
  }

  ForceRef<Base> force()
  {
    return ForceRef<Base>(*this);
  }

  ForceRef<const Base> force() const
  {
    return ForceRef<const Base>(*this);
  }

  MomentRef<Base> moment()
  {
    return MomentRef<Base>(*this,3);
  }

  MomentRef<const Base> moment() const
  {
    return MomentRef<const Base>(*this,3);
  }

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

}  // namespace ow_core

#endif  // OPEN_WALKER_CORE_WRENCH_REF_H