Shape.hpp

#pragma once
 
#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"
 
namespace state_representation {
class Shape : public State {
public:
  Shape();
 
  explicit Shape(const std::string& name, const std::string& reference_frame = "world");
 
  Shape(const Shape& shape);
 
  static Shape Unit(const std::string& name, const std::string& reference_frame = "world");
 
  friend void swap(Shape& state1, Shape& state2);
 
  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);
 
protected:
  std::string to_string() const override;
 
private:
  CartesianState center_state_; 
};
 
inline void swap(Shape& state1, Shape& state2) {
  swap(static_cast<State&>(state1), static_cast<State&>(state2));
  std::swap(state1.center_state_, state2.center_state_);
}
}// namespace state_representation