CartesianState.hpp

#pragma once
 
#include "state_representation/space/SpatialState.hpp"
#include "state_representation/exceptions/IncompatibleSizeException.hpp"
 
namespace state_representation {
 
class CartesianState;
 
enum class CartesianStateVariable {
  POSITION,
  ORIENTATION,
  POSE,
  LINEAR_VELOCITY,
  ANGULAR_VELOCITY,
  TWIST,
  LINEAR_ACCELERATION,
  ANGULAR_ACCELERATION,
  ACCELERATION,
  FORCE,
  TORQUE,
  WRENCH,
  ALL
};
 
double dist(
    const CartesianState& s1, const CartesianState& s2,
    const CartesianStateVariable& state_variable_type = CartesianStateVariable::ALL
);
 
class CartesianState : public SpatialState {
public:
  CartesianState();
 
  explicit CartesianState(const std::string& name, const std::string& reference = "world");
 
  CartesianState(const CartesianState& state);
 
  static CartesianState Identity(const std::string& name, const std::string& reference = "world");
 
  static CartesianState Random(const std::string& name, const std::string& reference = "world");
 
  friend void swap(CartesianState& state1, CartesianState& state2);
 
  CartesianState& operator=(const CartesianState& state);
 
  const Eigen::Vector3d& get_position() const;
 
  const Eigen::Quaterniond& get_orientation() const;
 
  Eigen::Vector4d get_orientation_coefficients() const;
 
  Eigen::Matrix<double, 7, 1> get_pose() const;
 
  Eigen::Matrix4d get_transformation_matrix() const;
 
  const Eigen::Vector3d& get_linear_velocity() const;
 
  const Eigen::Vector3d& get_angular_velocity() const;
 
  Eigen::Matrix<double, 6, 1> get_twist() const;
 
  const Eigen::Vector3d& get_linear_acceleration() const;
 
  const Eigen::Vector3d& get_angular_acceleration() const;
 
  Eigen::Matrix<double, 6, 1> get_acceleration() const;
 
  const Eigen::Vector3d& get_force() const;
 
  const Eigen::Vector3d& get_torque() const;
 
  Eigen::Matrix<double, 6, 1> get_wrench() const;
 
  virtual Eigen::VectorXd data() const;
 
  Eigen::ArrayXd array() const;
 
  std::vector<double> to_std_vector() const;
 
  void set_position(const Eigen::Vector3d& position);
 
  void set_position(const std::vector<double>& position);
 
  void set_position(const double& x, const double& y, const double& z);
 
  void set_orientation(const Eigen::Quaterniond& orientation);
 
  void set_orientation(const Eigen::Vector4d& orientation);
 
  void set_orientation(const std::vector<double>& orientation);
 
  void set_orientation(const double& w, const double& x, const double& y, const double& z);
 
  void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation);
 
  void set_pose(const Eigen::Matrix<double, 7, 1>& pose);
 
  void set_pose(const std::vector<double>& pose);
 
  void set_linear_velocity(const Eigen::Vector3d& linear_velocity);
 
  void set_linear_velocity(const std::vector<double>& linear_velocity);
 
  void set_linear_velocity(const double& x, const double& y, const double& z);
 
  void set_angular_velocity(const Eigen::Vector3d& angular_velocity);
 
  void set_angular_velocity(const std::vector<double>& angular_velocity);
 
  void set_angular_velocity(const double& x, const double& y, const double& z);
 
  void set_twist(const Eigen::Matrix<double, 6, 1>& twist);
 
  void set_twist(const std::vector<double>& twist);
 
  void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration);
 
  void set_linear_acceleration(const std::vector<double>& linear_acceleration);
 
  void set_linear_acceleration(const double& x, const double& y, const double& z);
 
  void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration);
 
  void set_angular_acceleration(const std::vector<double>& angular_acceleration);
 
  void set_angular_acceleration(const double& x, const double& y, const double& z);
 
  void set_acceleration(const Eigen::Matrix<double, 6, 1>& acceleration);
 
  void set_acceleration(const std::vector<double>& acceleration);
 
  void set_force(const Eigen::Vector3d& force);
 
  void set_force(const std::vector<double>& force);
 
  void set_force(const double& x, const double& y, const double& z);
 
  void set_torque(const Eigen::Vector3d& torque);
 
  void set_torque(const std::vector<double>& torque);
 
  void set_torque(const double& x, const double& y, const double& z);
 
  void set_wrench(const Eigen::Matrix<double, 6, 1>& wrench);
 
  void set_wrench(const std::vector<double>& wrench);
 
  virtual void set_data(const Eigen::VectorXd& data) override;
 
  virtual void set_data(const std::vector<double>& data) override;
 
  void set_zero();
 
  void clamp_state_variable(double max_norm, const CartesianStateVariable& state_variable_type, double noise_ratio = 0);
 
  CartesianState copy() const;
 
  double dist(
      const CartesianState& state, const CartesianStateVariable& state_variable_type = CartesianStateVariable::ALL
  ) const;
 
  friend double dist(
      const CartesianState& s1, const CartesianState& s2, const CartesianStateVariable& state_variable_type
  );
 
  void reset() override;
 
  CartesianState inverse() const;
 
  void normalize(const CartesianStateVariable& state_variable_type = CartesianStateVariable::ALL);
 
  CartesianState normalized(const CartesianStateVariable& state_variable_type = CartesianStateVariable::ALL) const;
 
  virtual std::vector<double>
  norms(const CartesianStateVariable& state_variable_type = CartesianStateVariable::ALL) const;
 
  CartesianState& operator*=(const CartesianState& state);
 
  CartesianState operator*(const CartesianState& state) const;
 
  CartesianState& operator*=(double lambda);
 
  CartesianState operator*(double lambda) const;
 
  friend CartesianState operator*(double lambda, const CartesianState& state);
 
  Eigen::Vector3d operator*(const Eigen::Vector3d& vector) const;
 
  CartesianState& operator/=(double lambda);
 
  CartesianState operator/(double lambda) const;
 
  CartesianState& operator+=(const CartesianState& state);
 
  CartesianState operator+(const CartesianState& state) const;
 
  CartesianState operator-() const;
 
  CartesianState& operator-=(const CartesianState& state);
 
  CartesianState operator-(const CartesianState& state) const;
 
  friend std::ostream& operator<<(std::ostream& os, const CartesianState& state);
 
protected:
  Eigen::VectorXd get_state_variable(const CartesianStateVariable& state_variable_type) const;
 
  void set_state_variable(const Eigen::VectorXd& new_value, const CartesianStateVariable& state_variable_type);
 
  void set_state_variable(const std::vector<double>& new_value, const CartesianStateVariable& state_variable_type);
 
  std::string to_string() const override;
 
private:
  Eigen::Vector3d position_;            
  Eigen::Quaterniond orientation_;      
  Eigen::Vector3d linear_velocity_;     
  Eigen::Vector3d angular_velocity_;    
  Eigen::Vector3d linear_acceleration_; 
  Eigen::Vector3d angular_acceleration_;
  Eigen::Vector3d force_;               
  Eigen::Vector3d torque_;              
};
 
inline void swap(CartesianState& state1, CartesianState& state2) {
  swap(static_cast<SpatialState&>(state1), static_cast<SpatialState&>(state2));
  std::swap(state1.position_, state2.position_);
  std::swap(state1.orientation_, state2.orientation_);
  std::swap(state1.linear_velocity_, state2.linear_velocity_);
  std::swap(state1.angular_velocity_, state2.angular_velocity_);
  std::swap(state1.linear_acceleration_, state2.linear_acceleration_);
  std::swap(state1.angular_acceleration_, state2.angular_acceleration_);
  std::swap(state1.force_, state2.force_);
  std::swap(state1.torque_, state2.torque_);
}
}// namespace state_representation