CartesianPose.hpp

#pragma once
 
#include "state_representation/space/cartesian/CartesianState.hpp"
#include "state_representation/space/cartesian/CartesianTwist.hpp"
#include "state_representation/space/cartesian/CartesianAcceleration.hpp"
#include "state_representation/space/cartesian/CartesianWrench.hpp"
 
namespace state_representation {
class CartesianTwist;
class CartesianAcceleration;
class CartesianWrench;
 
class CartesianPose : public CartesianState {
private:
  using CartesianState::clamp_state_variable;
 
public:
  // delete inaccessible getter and setters
  const Eigen::Vector3d& get_linear_velocity() const = delete;
  const Eigen::Vector3d& get_angular_velocity() const = delete;
  Eigen::Matrix<double, 6, 1> get_twist() 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_linear_velocity(const Eigen::Vector3d& linear_velocity) = delete;
  void set_linear_velocity(const std::vector<double>& linear_velocity) = delete;
  void set_linear_velocity(const double& x, const double& y, const double& z) = delete;
  void set_angular_velocity(const Eigen::Vector3d& angular_velocity) = delete;
  void set_angular_velocity(const std::vector<double>& angular_velocity) = delete;
  void set_angular_velocity(const double& x, const double& y, const double& z) = delete;
  void set_twist(const Eigen::Matrix<double, 6, 1>& twist) = delete;
  void set_twist(const std::vector<double>& twist) = 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;
  friend CartesianState operator*=(const CartesianState& state, const CartesianPose& pose) = delete;
 
  explicit CartesianPose();
 
  explicit CartesianPose(const std::string& name, const std::string& reference = "world");
 
  CartesianPose(const CartesianPose& pose);
 
  CartesianPose(const CartesianState& state);
 
  CartesianPose(const CartesianTwist& twist);
 
  explicit CartesianPose(
      const std::string& name, const Eigen::Vector3d& position, const std::string& reference = "world"
  );
 
  explicit CartesianPose(
      const std::string& name, double x, double y, double z, const std::string& reference = "world"
  );
 
  explicit CartesianPose(
      const std::string& name, const Eigen::Quaterniond& orientation, const std::string& reference = "world"
  );
 
  explicit CartesianPose(
      const std::string& name, const Eigen::Vector3d& position, const Eigen::Quaterniond& orientation,
      const std::string& reference = "world"
  );
 
  static CartesianPose Identity(const std::string& name, const std::string& reference = "world");
 
  static CartesianPose Random(const std::string& name, const std::string& reference = "world");
 
  CartesianPose& operator=(const CartesianPose& pose) = default;
 
  Eigen::Vector3d operator*(const Eigen::Vector3d& vector) const;
 
  CartesianPose& operator*=(const CartesianPose& pose);
 
  CartesianPose operator*(const CartesianPose& pose) const;
 
  CartesianState operator*(const CartesianState& state) const;
 
  CartesianTwist operator*(const CartesianTwist& twist) const;
 
  CartesianAcceleration operator*(const CartesianAcceleration& acceleration) const;
 
  CartesianWrench operator*(const CartesianWrench& wrench) const;
 
  CartesianPose& operator*=(double lambda);
 
  CartesianPose operator*(double lambda) const;
 
  CartesianPose& operator/=(double lambda);
 
  CartesianPose operator/(double lambda) const;
 
  CartesianPose& operator+=(const CartesianPose& pose);
 
  CartesianPose operator+(const CartesianPose& pose) const;
 
  CartesianPose& operator-=(const CartesianPose& pose);
 
  CartesianPose operator-(const CartesianPose& pose) const;
 
  CartesianTwist operator/(const std::chrono::nanoseconds& dt) const;
 
  CartesianPose copy() const;
 
  Eigen::VectorXd data() const override;
 
  void set_data(const Eigen::VectorXd& data) override;
 
  void set_data(const std::vector<double>& data) override;
 
  std::vector<double>
  norms(const CartesianStateVariable& state_variable_type = CartesianStateVariable::POSE) const override;
 
  CartesianPose inverse() const;
 
  CartesianPose normalized(const CartesianStateVariable& state_variable_type = CartesianStateVariable::POSE) const;
 
  friend std::ostream& operator<<(std::ostream& os, const CartesianPose& pose);
 
  friend CartesianPose operator*(const CartesianState& state, const CartesianPose& pose);
 
  friend CartesianPose operator*(double lambda, const CartesianPose& pose);
};
 
inline std::vector<double> CartesianPose::norms(const CartesianStateVariable& state_variable_type) const {
  return CartesianState::norms(state_variable_type);
}
 
inline CartesianPose CartesianPose::normalized(const CartesianStateVariable& state_variable_type) const {
  return CartesianState::normalized(state_variable_type);
}
}// namespace state_representation