JointState.hpp

#pragma once
 
#include "state_representation/State.hpp"
#include "state_representation/exceptions/IncompatibleSizeException.hpp"
 
namespace state_representation {
 
class JointState;
 
enum class JointStateVariable {
  POSITIONS, VELOCITIES, ACCELERATIONS, TORQUES, ALL
};
 
double dist(
    const JointState& s1, const JointState& s2, const JointStateVariable& state_variable_type = JointStateVariable::ALL
);
 
class JointState : public State {
public:
  JointState();
 
  explicit JointState(const std::string& robot_name, unsigned int nb_joints = 0);
 
  JointState(const std::string& robot_name, const std::vector<std::string>& joint_names);
 
  JointState(const JointState& state);
 
  static JointState Zero(const std::string& robot_name, unsigned int nb_joints);
 
  static JointState Zero(const std::string& robot_name, const std::vector<std::string>& joint_names);
 
  static JointState Random(const std::string& robot_name, unsigned int nb_joints);
 
  static JointState Random(const std::string& robot_name, const std::vector<std::string>& joint_names);
 
  friend void swap(JointState& state1, JointState& state2);
 
  JointState& operator=(const JointState& state);
 
  unsigned int get_size() const;
 
  const std::vector<std::string>& get_names() const;
 
  unsigned int get_joint_index(const std::string& joint_name) const;
 
  const Eigen::VectorXd& get_positions() const;
 
  double get_position(const std::string& joint_name) const;
 
  double get_position(unsigned int joint_index) const;
 
  const Eigen::VectorXd& get_velocities() const;
 
  double get_velocity(const std::string& joint_name) const;
 
  double get_velocity(unsigned int joint_index) const;
 
  const Eigen::VectorXd& get_accelerations() const;
 
  double get_acceleration(const std::string& joint_name) const;
 
  double get_acceleration(unsigned int joint_index) const;
 
  const Eigen::VectorXd& get_torques() const;
 
  double get_torque(const std::string& joint_name) const;
 
  double get_torque(unsigned int joint_index) const;
 
  virtual Eigen::VectorXd data() const;
 
  Eigen::ArrayXd array() const;
 
  void set_names(unsigned int nb_joints);
 
  void set_names(const std::vector<std::string>& names);
 
  void set_positions(const Eigen::VectorXd& positions);
 
  void set_positions(const std::vector<double>& positions);
 
  void set_position(double position, const std::string& joint_name);
 
  void set_position(double position, unsigned int joint_index);
 
  void set_velocities(const Eigen::VectorXd& velocities);
 
  void set_velocities(const std::vector<double>& velocities);
 
  void set_velocity(double velocity, const std::string& joint_name);
 
  void set_velocity(double velocity, unsigned int joint_index);
 
  void set_accelerations(const Eigen::VectorXd& accelerations);
 
  void set_accelerations(const std::vector<double>& accelerations);
 
  void set_acceleration(double acceleration, const std::string& joint_name);
 
  void set_acceleration(double acceleration, unsigned int joint_index);
 
  void set_torques(const Eigen::VectorXd& torques);
 
  void set_torques(const std::vector<double>& torques);
 
  void set_torque(double torque, const std::string& joint_name);
 
  void set_torque(double torque, unsigned int joint_index);
 
  virtual void set_data(const Eigen::VectorXd& data) override;
 
  virtual void set_data(const std::vector<double>& data) override;
 
  void clamp_state_variable(
      double max_absolute_value, const JointStateVariable& state_variable_type, double noise_ratio = 0
  );
 
  void clamp_state_variable(
      const Eigen::ArrayXd& max_absolute_value_array, const JointStateVariable& state_variable_type,
      const Eigen::ArrayXd& noise_ratio_array
  );
 
  JointState copy() const;
 
  double dist(const JointState& state, const JointStateVariable& state_variable_type = JointStateVariable::ALL) const;
 
  friend double dist(const JointState& s1, const JointState& s2, const JointStateVariable& state_variable_type);
 
  void reset() override;
 
  bool is_incompatible(const State& state) const override;
 
  void set_zero();
 
  std::vector<double> to_std_vector() const;
 
  JointState& operator*=(double lambda);
 
  JointState operator*(double lambda) const;
 
  friend JointState operator*(double lambda, const JointState& state);
 
  friend JointState operator*(const Eigen::MatrixXd& lambda, const JointState& state);
 
  JointState& operator/=(double lambda);
 
  JointState operator/(double lambda) const;
 
  JointState& operator+=(const JointState& state);
 
  JointState operator+(const JointState& state) const;
 
  JointState operator-() const;
 
  JointState& operator-=(const JointState& state);
 
  JointState operator-(const JointState& state) const;
 
  friend std::ostream& operator<<(std::ostream& os, const JointState& state);
 
protected:
  void multiply_state_variable(const Eigen::MatrixXd& lambda, const JointStateVariable& state_variable_type);
 
  Eigen::VectorXd get_state_variable(const JointStateVariable& state_variable_type) const;
 
  void set_state_variable(const Eigen::VectorXd& new_value, const JointStateVariable& state_variable_type);
 
  void set_state_variable(const std::vector<double>& new_value, const JointStateVariable& state_variable_type);
 
  void set_state_variable(double new_value, unsigned int joint_index, const JointStateVariable& state_variable_type);
 
  std::string to_string() const override;
 
private:
  std::vector<std::string> names_;
  Eigen::VectorXd positions_;     
  Eigen::VectorXd velocities_;    
  Eigen::VectorXd accelerations_; 
  Eigen::VectorXd torques_;       
};
 
inline void swap(JointState& state1, JointState& state2) {
  swap(static_cast<State&>(state1), static_cast<State&>(state2));
  std::swap(state1.names_, state2.names_);
  std::swap(state1.positions_, state2.positions_);
  std::swap(state1.velocities_, state2.velocities_);
  std::swap(state1.accelerations_, state2.accelerations_);
  std::swap(state1.torques_, state2.torques_);
}
}// namespace state_representation