Shape.hpp

#pragma once
 
#include <eigen3/Eigen/Core>
#include "state_representation/State.hpp"
#include "state_representation/space/cartesian/CartesianState.hpp"
#include "state_representation/space/cartesian/CartesianPose.hpp"
#include "state_representation/space/cartesian/CartesianTwist.hpp"
#include "state_representation/exceptions/EmptyStateException.hpp"
#include "state_representation/exceptions/IncompatibleStatesException.hpp"
#include "state_representation/exceptions/IncompatibleReferenceFramesException.hpp"
 
namespace state_representation {
class Shape : public State {
private:
  CartesianState center_state_; 
 
public:
  explicit Shape(const StateType& type);
 
  explicit Shape(const StateType& type, const std::string& name, const std::string& reference_frame = "world");
 
  explicit Shape(const Shape& shape);
 
  Shape& operator=(const Shape& state);
 
  const CartesianState& get_center_state() const;
 
  const CartesianPose& get_center_pose() const;
 
  const Eigen::Vector3d get_center_position() const;
 
  const Eigen::Quaterniond get_center_orientation() const;
 
  const CartesianTwist& get_center_twist() const;
 
  void set_center_state(const CartesianState& state);
 
  void set_center_pose(const CartesianPose& pose);
 
  void set_center_position(const Eigen::Vector3d& position);
 
  void set_center_orientation(const Eigen::Quaterniond& orientation);
 
  friend std::ostream& operator<<(std::ostream& os, const Shape& shape);
};
 
inline Shape& Shape::operator=(const Shape& state) {
  State::operator=(state);
  this->center_state_ = state.center_state_;
  return (*this);
}
 
inline const CartesianState& Shape::get_center_state() const {
  return this->center_state_;
}
 
inline const CartesianPose& Shape::get_center_pose() const {
  return static_cast<const CartesianPose&>(this->center_state_);
}
 
inline const Eigen::Vector3d Shape::get_center_position() const {
  return this->center_state_.get_position();
}
 
inline const Eigen::Quaterniond Shape::get_center_orientation() const {
  return this->center_state_.get_orientation();
}
 
inline const CartesianTwist& Shape::get_center_twist() const {
  return static_cast<const CartesianTwist&>(this->center_state_);
}
 
inline void Shape::set_center_state(const CartesianState& state) {
  this->set_name(state.get_name());
  this->center_state_ = state;
}
 
inline void Shape::set_center_pose(const CartesianPose& pose) {
  if (!this->center_state_.is_empty() && this->center_state_.get_reference_frame() != pose.get_reference_frame()) {
    throw exceptions::IncompatibleReferenceFramesException(
        "The shape state and the given pose are not expressed in the same reference frame");
  }
  this->center_state_.set_pose(pose.get_position(), pose.get_orientation());
}
 
inline void Shape::set_center_position(const Eigen::Vector3d& position) {
  if (this->get_center_state().is_empty()) {
    throw exceptions::EmptyStateException("The center state of the Shape is not set yet.");
  }
  this->center_state_.set_position(position);
}
 
inline void Shape::set_center_orientation(const Eigen::Quaterniond& orientation) {
  if (this->get_center_state().is_empty()) {
    throw exceptions::EmptyStateException("The center state of the Shape is not set yet.");
  }
  this->center_state_.set_orientation(orientation);
}
}