CartesianTwist.hpp

#pragma once
 
#include "state_representation/space/cartesian/CartesianState.hpp"
#include "state_representation/space/cartesian/CartesianPose.hpp"
#include "state_representation/space/cartesian/CartesianAcceleration.hpp"
 
namespace state_representation {
class CartesianPose;
class CartesianAcceleration;
 
class CartesianTwist : public CartesianState {
private:
  using CartesianState::clamp_state_variable;
 
public:
  // delete inaccessible getter and setters
  const Eigen::Vector3d& get_position() const = delete;
  const Eigen::Quaterniond& get_orientation() const = delete;
  Eigen::Vector4d get_orientation_coefficients() const = delete;
  Eigen::Matrix<double, 7, 1> get_pose() const = delete;
  Eigen::Matrix4d get_transformation_matrix() const = delete;
  const Eigen::Vector3d& get_linear_acceleration() const = delete;
  const Eigen::Vector3d& get_angular_acceleration() const = delete;
  Eigen::Matrix<double, 6, 1> get_acceleration() const = delete;
  const Eigen::Vector3d& get_force() const = delete;
  const Eigen::Vector3d& get_torque() const = delete;
  Eigen::Matrix<double, 6, 1> get_wrench() const = delete;
  void set_position(const Eigen::Vector3d& position) = delete;
  void set_position(const std::vector<double>& position) = delete;
  void set_position(const double& x, const double& y, const double& z) = delete;
  void set_orientation(const Eigen::Quaterniond& orientation) = delete;
  void set_orientation(const Eigen::Vector4d& orientation) = delete;
  void set_orientation(const std::vector<double>& orientation) = delete;
  void set_orientation(const double& w, const double& x, const double& y, const double& z) = delete;
  void set_pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation) = delete;
  void set_pose(const Eigen::Matrix<double, 7, 1>& pose) = delete;
  void set_pose(const std::vector<double>& pose) = delete;
  void set_linear_acceleration(const Eigen::Vector3d& linear_acceleration) = delete;
  void set_linear_acceleration(const std::vector<double>& linear_acceleration) = delete;
  void set_linear_acceleration(const double& x, const double& y, const double& z) = delete;
  void set_angular_acceleration(const Eigen::Vector3d& angular_acceleration) = delete;
  void set_angular_acceleration(const std::vector<double>& angular_acceleration) = delete;
  void set_angular_acceleration(const double& x, const double& y, const double& z) = delete;
  void set_acceleration(const Eigen::Matrix<double, 6, 1>& acceleration) = delete;
  void set_acceleration(const std::vector<double>& acceleration) = delete;
  void set_force(const Eigen::Vector3d& force) = delete;
  void set_force(const std::vector<double>& force) = delete;
  void set_force(const double& x, const double& y, const double& z) = delete;
  void set_torque(const Eigen::Vector3d& torque) = delete;
  void set_torque(const std::vector<double>& torque) = delete;
  void set_torque(const double& x, const double& y, const double& z) = delete;
  void set_wrench(const Eigen::Matrix<double, 6, 1>& wrench) = delete;
  void set_wrench(const std::vector<double>& wrench) = delete;
  CartesianState operator*=(const CartesianState& state) = delete;
  CartesianState operator*(const CartesianState& state) = delete;
  friend CartesianState operator*=(const CartesianState& state, const CartesianTwist& twist) = delete;
 
  explicit CartesianTwist();
 
  explicit CartesianTwist(const std::string& name, const std::string& reference = "world");
 
  CartesianTwist(const CartesianTwist& twist);
 
  CartesianTwist(const CartesianState& state);
 
  CartesianTwist(const CartesianPose& pose);
 
  CartesianTwist(const CartesianAcceleration& acceleration);
 
  explicit CartesianTwist(
      const std::string& name, const Eigen::Vector3d& linear_velocity, const std::string& reference = "world"
  );
 
  explicit CartesianTwist(
      const std::string& name, const Eigen::Vector3d& linear_velocity, const Eigen::Vector3d& angular_velocity,
      const std::string& reference = "world"
  );
 
  explicit CartesianTwist(
      const std::string& name, const Eigen::Matrix<double, 6, 1>& twist, const std::string& reference = "world"
  );
 
  static CartesianTwist Zero(const std::string& name, const std::string& reference = "world");
 
  static CartesianTwist Random(const std::string& name, const std::string& reference = "world");
 
  CartesianTwist& operator=(const CartesianTwist& twist) = default;
 
  CartesianTwist& operator+=(const CartesianTwist& twist);
 
  CartesianTwist operator+(const CartesianTwist& twist) const;
 
  CartesianTwist& operator-=(const CartesianTwist& twist);
 
  CartesianTwist operator-(const CartesianTwist& twist) const;
 
  CartesianTwist& operator*=(double lambda);
 
  CartesianTwist operator*(double lambda) const;
 
  CartesianTwist& operator/=(double lambda);
 
  CartesianTwist operator/(double lambda) const;
 
  CartesianTwist& operator*=(const Eigen::Matrix<double, 6, 6>& lambda);
 
  CartesianPose operator*(const std::chrono::nanoseconds& dt) const;
 
  CartesianAcceleration operator/(const std::chrono::nanoseconds& dt) const;
 
  void clamp(double max_linear, double max_angular, double linear_noise_ratio = 0, double angular_noise_ratio = 0);
 
  CartesianTwist clamped(
      double max_linear, double max_angular, double noise_ratio = 0, double angular_noise_ratio = 0
  ) const;
 
  CartesianTwist copy() const;
 
  Eigen::VectorXd data() const override;
 
  void set_data(const Eigen::VectorXd& data) override;
 
  void set_data(const std::vector<double>& data) override;
 
  CartesianTwist inverse() const;
 
  std::vector<double>
  norms(const CartesianStateVariable& state_variable_type = CartesianStateVariable::TWIST) const override;
 
  CartesianTwist normalized(const CartesianStateVariable& state_variable_type = CartesianStateVariable::TWIST) const;
 
  friend std::ostream& operator<<(std::ostream& os, const CartesianTwist& twist);
 
  friend CartesianTwist operator*(const CartesianState& state, const CartesianTwist& twist);
 
  friend CartesianTwist operator*(double lambda, const CartesianTwist& twist);
 
  friend CartesianTwist operator*(const Eigen::Matrix<double, 6, 6>& lambda, const CartesianTwist& twist);
 
  friend CartesianPose operator*(const std::chrono::nanoseconds& dt, const CartesianTwist& twist);
};
 
inline std::vector<double> CartesianTwist::norms(const CartesianStateVariable& state_variable_type) const {
  return CartesianState::norms(state_variable_type);
}
 
inline CartesianTwist CartesianTwist::normalized(const CartesianStateVariable& state_variable_type) const {
  return CartesianState::normalized(state_variable_type);
}
}// namespace state_representation