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 {
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_;              
 
  void set_all_state_variables(const Eigen::VectorXd& new_values);
 
  void set_state_variable(Eigen::Vector3d& state_variable, const Eigen::Vector3d& new_value);
 
  void set_state_variable(Eigen::Vector3d& state_variable, const std::vector<double>& new_value);
 
  void set_state_variable(
      Eigen::Vector3d& linear_state_variable, Eigen::Vector3d& angular_state_variable,
      const Eigen::Matrix<double, 6, 1>& new_value
  );
 
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);
 
public:
  explicit CartesianState();
 
  explicit CartesianState(const std::string& name, const std::string& reference = "world");
 
  CartesianState(const CartesianState& state) = default;
 
  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;
 
  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);
 
  void initialize() override;
 
  void set_zero();
 
  void clamp_state_variable(double max_norm, const CartesianStateVariable& state_variable_type, double noise_ratio = 0);
 
  CartesianState copy() const;
 
  virtual Eigen::VectorXd data() const;
 
  Eigen::ArrayXd array() const;
 
  virtual void set_data(const Eigen::VectorXd& data) override;
 
  virtual void set_data(const std::vector<double>& data) override;
 
  CartesianState& operator*=(const CartesianState& state);
 
  CartesianState operator*(const CartesianState& state) const;
 
  CartesianState& operator*=(double lambda);
 
  CartesianState operator*(double lambda) 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& state);
 
  CartesianState operator-(const CartesianState& state) const;
 
  CartesianState inverse() const;
 
  double dist(
      const CartesianState& state, const CartesianStateVariable& state_variable_type = CartesianStateVariable::ALL
  ) const;
 
  virtual std::vector<double>
  norms(const CartesianStateVariable& state_variable_type = CartesianStateVariable::ALL) const;
 
  void normalize(const CartesianStateVariable& state_variable_type = CartesianStateVariable::ALL);
 
  CartesianState normalized(const CartesianStateVariable& state_variable_type = CartesianStateVariable::ALL) const;
 
  friend std::ostream& operator<<(std::ostream& os, const CartesianState& state);
 
  friend CartesianState operator*(double lambda, const CartesianState& state);
 
  friend double dist(
      const CartesianState& s1, const CartesianState& s2, const CartesianStateVariable& state_variable_type
  );
 
  std::vector<double> to_std_vector() const;
};
 
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_);
}
 
inline CartesianState& CartesianState::operator=(const CartesianState& state) {
  CartesianState tmp(state);
  swap(*this, tmp);
  return *this;
}
 
inline Eigen::VectorXd CartesianState::get_state_variable(const CartesianStateVariable& state_variable_type) const {
  switch (state_variable_type) {
    case CartesianStateVariable::POSITION:
      return this->get_position();
 
    case CartesianStateVariable::ORIENTATION:
      return this->get_orientation_coefficients();
 
    case CartesianStateVariable::POSE:
      return this->get_pose();
 
    case CartesianStateVariable::LINEAR_VELOCITY:
      return this->get_linear_velocity();
 
    case CartesianStateVariable::ANGULAR_VELOCITY:
      return this->get_angular_velocity();
 
    case CartesianStateVariable::TWIST:
      return this->get_twist();
 
    case CartesianStateVariable::LINEAR_ACCELERATION:
      return this->get_linear_acceleration();
 
    case CartesianStateVariable::ANGULAR_ACCELERATION:
      return this->get_angular_acceleration();
 
    case CartesianStateVariable::ACCELERATION:
      return this->get_acceleration();
 
    case CartesianStateVariable::FORCE:
      return this->get_force();
 
    case CartesianStateVariable::TORQUE:
      return this->get_torque();
 
    case CartesianStateVariable::WRENCH:
      return this->get_wrench();
 
    case CartesianStateVariable::ALL:
      Eigen::VectorXd all_fields(25);
      all_fields << this->get_pose(), this->get_twist(), this->get_acceleration(), this->get_wrench();
      return all_fields;
  }
  // this never goes here but is compulsory to avoid a warning
  return Eigen::Vector3d::Zero();
}
 
inline void CartesianState::set_all_state_variables(const Eigen::VectorXd& new_values) {
  if (new_values.size() != 25) {
    throw exceptions::IncompatibleSizeException(
        "Input is of incorrect size: expected 25, given " + std::to_string(new_values.size()));
  }
  this->set_pose(new_values.segment(0, 7));
  this->set_twist(new_values.segment(7, 6));
  this->set_acceleration(new_values.segment(13, 6));
  this->set_wrench(new_values.segment(19, 6));
}
 
inline void CartesianState::set_state_variable(Eigen::Vector3d& state_variable, const Eigen::Vector3d& new_value) {
  this->set_filled();
  state_variable = new_value;
}
 
inline void CartesianState::set_state_variable(Eigen::Vector3d& state_variable, const std::vector<double>& new_value) {
  if (new_value.size() != 3) {
    throw exceptions::IncompatibleSizeException(
        "Input vector is of incorrect size: expected 3, given " + std::to_string(new_value.size()));
  }
  this->set_state_variable(state_variable, Eigen::Vector3d::Map(new_value.data(), new_value.size()));
}
 
inline void CartesianState::set_state_variable(
    Eigen::Vector3d& linear_state_variable, Eigen::Vector3d& angular_state_variable,
    const Eigen::Matrix<double, 6, 1>& new_value
) {
  this->set_state_variable(linear_state_variable, new_value.head(3));
  this->set_state_variable(angular_state_variable, new_value.tail(3));
}
 
inline void CartesianState::set_state_variable(
    const Eigen::VectorXd& new_value, const CartesianStateVariable& state_variable_type
) {
  switch (state_variable_type) {
    case CartesianStateVariable::POSITION:
      this->set_position(new_value);
      break;
 
    case CartesianStateVariable::ORIENTATION:
      this->set_orientation(new_value);
      break;
 
    case CartesianStateVariable::POSE:
      this->set_pose(new_value);
      break;
 
    case CartesianStateVariable::LINEAR_VELOCITY:
      this->set_linear_velocity(new_value);
      break;
 
    case CartesianStateVariable::ANGULAR_VELOCITY:
      this->set_angular_velocity(new_value);
      break;
 
    case CartesianStateVariable::TWIST:
      this->set_twist(new_value);
      break;
 
    case CartesianStateVariable::LINEAR_ACCELERATION:
      this->set_linear_acceleration(new_value);
      break;
 
    case CartesianStateVariable::ANGULAR_ACCELERATION:
      this->set_angular_acceleration(new_value);
      break;
 
    case CartesianStateVariable::ACCELERATION:
      this->set_acceleration(new_value);
      break;
 
    case CartesianStateVariable::FORCE:
      this->set_force(new_value);
      break;
 
    case CartesianStateVariable::TORQUE:
      this->set_torque(new_value);
      break;
 
    case CartesianStateVariable::WRENCH:
      this->set_wrench(new_value);
      break;
 
    case CartesianStateVariable::ALL:
      this->set_pose(new_value.segment(0, 7));
      this->set_twist(new_value.segment(7, 6));
      this->set_acceleration(new_value.segment(13, 6));
      this->set_wrench(new_value.segment(19, 6));
      break;
  }
}
 
inline std::vector<double> CartesianState::to_std_vector() const {
  Eigen::VectorXd data = this->data();
  return std::vector<double>(data.data(), data.data() + data.size());
}
}// namespace state_representation