BodySensor.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rbdyn/Device.h>
#include <mc_rtc/deprecated.h>
 
#include <Eigen/StdVector>
 
namespace mc_rbdyn
{
 
struct MC_RBDYN_DLLAPI BodySensor : public Device
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  inline BodySensor() : BodySensor("", "", sva::PTransformd::Identity()) {}
 
  inline BodySensor(const std::string & name, const std::string & bodyName, const sva::PTransformd & X_b_s)
  : Device(name, bodyName, X_b_s)
  {
    type_ = "BodySensor";
  }
 
  BodySensor(const BodySensor & bs) : BodySensor(bs.name(), bs.parent(), bs.X_b_s())
  {
    position_ = bs.position_;
    orientation_ = bs.orientation_;
    linear_velocity_ = bs.linear_velocity_;
    angular_velocity_ = bs.angular_velocity_;
    linear_acceleration_ = bs.linear_acceleration_;
    angular_acceleration_ = bs.angular_acceleration_;
  }
 
  BodySensor & operator=(const BodySensor & bs)
  {
    name_ = bs.name_;
    parent_ = bs.parent_;
    X_p_s_ = bs.X_p_s_;
    position_ = bs.position_;
    orientation_ = bs.orientation_;
    linear_velocity_ = bs.linear_velocity_;
    angular_velocity_ = bs.angular_velocity_;
    linear_acceleration_ = bs.linear_acceleration_;
    angular_acceleration_ = bs.angular_acceleration_;
    return *this;
  }
 
  BodySensor(BodySensor &&) = default;
  BodySensor & operator=(BodySensor &&) = default;
 
  ~BodySensor() noexcept override;
 
  inline const std::string & parentBody() const { return Device::parent(); }
 
  inline const sva::PTransformd & X_b_s() const { return Device::X_p_s(); }
 
  inline const Eigen::Vector3d & position() const { return position_; }
 
  inline void position(const Eigen::Vector3d & position) { position_ = position; }
 
  inline const Eigen::Quaterniond & orientation() const { return orientation_; }
 
  inline void orientation(const Eigen::Quaterniond & orientation) { orientation_ = orientation; }
 
  inline const Eigen::Vector3d & linearVelocity() const { return linear_velocity_; }
 
  inline void linearVelocity(const Eigen::Vector3d & linear_velocity) { linear_velocity_ = linear_velocity; }
 
  inline const Eigen::Vector3d & angularVelocity() const { return angular_velocity_; }
 
  inline void angularVelocity(const Eigen::Vector3d & angular_velocity) { angular_velocity_ = angular_velocity; }
 
  MC_RTC_DEPRECATED inline const Eigen::Vector3d & acceleration() const { return linearAcceleration(); }
 
  MC_RTC_DEPRECATED inline void acceleration(const Eigen::Vector3d & acceleration) { linearAcceleration(acceleration); }
 
  inline const Eigen::Vector3d & linearAcceleration() const { return linear_acceleration_; }
 
  inline void linearAcceleration(const Eigen::Vector3d & linear_acceleration)
  {
    linear_acceleration_ = linear_acceleration;
  }
 
  inline const Eigen::Vector3d & angularAcceleration() const { return angular_acceleration_; }
 
  inline void angularAcceleration(const Eigen::Vector3d & angular_acceleration)
  {
    angular_acceleration_ = angular_acceleration;
  }
 
  DevicePtr clone() const override;
 
private:
  Eigen::Vector3d position_ = Eigen::Vector3d::Zero();
  Eigen::Quaterniond orientation_ = Eigen::Quaterniond::Identity();
  Eigen::Vector3d linear_velocity_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_velocity_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d linear_acceleration_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d angular_acceleration_ = Eigen::Vector3d::Zero();
};
 
typedef std::vector<BodySensor, Eigen::aligned_allocator<BodySensor>> BodySensorVector;
 
inline bool operator==(const mc_rbdyn::BodySensor & lhs, const mc_rbdyn::BodySensor & rhs)
{
  return lhs.name() == rhs.name() && lhs.parentBody() == rhs.parentBody() && lhs.X_b_s() == rhs.X_b_s();
}
 
} // namespace mc_rbdyn