JointState.hpp

#pragma once
 
#include "state_representation/State.hpp"
#include "state_representation/exceptions/IncompatibleSizeException.hpp"
 
using namespace state_representation::exceptions;
 
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 {
private:
  std::vector<std::string> names_;
  Eigen::VectorXd positions_;     
  Eigen::VectorXd velocities_;    
  Eigen::VectorXd accelerations_; 
  Eigen::VectorXd torques_;       
 
  Eigen::VectorXd get_all_state_variables() const;
 
  void set_state_variable(Eigen::VectorXd& state_variable, const Eigen::VectorXd& new_value);
 
  void set_state_variable(Eigen::VectorXd& state_variable, const std::vector<double>& new_value);
 
  void set_all_state_variables(const Eigen::VectorXd& new_values);
 
protected:
  void multiply_state_variable(const Eigen::ArrayXd& lambda, const JointStateVariable& state_variable_type);
 
  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);
 
public:
  explicit JointState();
 
  explicit JointState(const std::string& robot_name, unsigned int nb_joints = 0);
 
  explicit JointState(const std::string& robot_name, const std::vector<std::string>& joint_names);
 
  JointState(const JointState& state) = default;
 
  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;
 
  void set_names(unsigned int nb_joints);
 
  void set_names(const std::vector<std::string>& names);
 
  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;
 
  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);
 
  const Eigen::VectorXd& get_velocities() const;
 
  double get_velocity(const std::string& joint_name) const;
 
  double get_velocity(unsigned int joint_index) const;
 
  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);
 
  const Eigen::VectorXd& get_accelerations() const;
 
  double get_acceleration(const std::string& joint_name) const;
 
  double get_acceleration(unsigned int joint_index) const;
 
  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);
 
  const Eigen::VectorXd& get_torques() const;
 
  double get_torque(const std::string& joint_name) const;
 
  double get_torque(unsigned int joint_index) const;
 
  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);
 
  bool is_compatible(const State& state) const;
 
  void initialize();
 
  void set_zero();
 
  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;
 
  virtual Eigen::VectorXd data() const;
 
  virtual void set_data(const Eigen::VectorXd& data) override;
 
  virtual void set_data(const std::vector<double>& data) override;
 
  Eigen::ArrayXd array() const;
 
  JointState& operator+=(const JointState& state);
 
  JointState operator+(const JointState& state) const;
 
  JointState& operator-=(const JointState& state);
 
  JointState operator-(const JointState& state) const;
 
  JointState& operator*=(double lambda);
 
  JointState operator*(double lambda) const;
 
  JointState& operator*=(const Eigen::ArrayXd& lambda);
 
  JointState operator*(const Eigen::ArrayXd& lambda) const;
 
  JointState& operator*=(const Eigen::MatrixXd& lambda);
 
  JointState operator*(const Eigen::MatrixXd& lambda) const;
 
  JointState& operator/=(double lambda);
 
  JointState operator/(double lambda) const;
 
  double dist(const JointState& state, const JointStateVariable& state_variable_type = JointStateVariable::ALL) const;
 
  friend std::ostream& operator<<(std::ostream& os, const JointState& state);
 
  friend JointState operator*(double lambda, const JointState& state);
 
  friend JointState operator*(const Eigen::ArrayXd& lambda, const JointState& state);
 
  friend JointState operator*(const Eigen::MatrixXd& lambda, const JointState& state);
 
  std::vector<double> to_std_vector() const;
};
 
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_);
}
 
inline JointState& JointState::operator=(const JointState& state) {
  JointState tmp(state);
  swap(*this, tmp);
  return *this;
}
 
inline Eigen::VectorXd JointState::get_all_state_variables() const {
  Eigen::VectorXd all_fields(this->get_size() * 4);
  all_fields << this->get_positions(), this->get_velocities(), this->get_accelerations(), this->get_torques();
  return all_fields;
}
 
inline void JointState::set_state_variable(Eigen::VectorXd& state_variable, const Eigen::VectorXd& new_value) {
  if (new_value.size() != this->get_size()) {
    throw IncompatibleSizeException(
        "Input vector is of incorrect size: expected " + std::to_string(this->get_size()) + ", given "
            + std::to_string(new_value.size()));
  }
  this->set_filled();
  state_variable = new_value;
}
 
inline void JointState::set_state_variable(Eigen::VectorXd& state_variable, const std::vector<double>& new_value) {
  this->set_state_variable(state_variable, Eigen::VectorXd::Map(new_value.data(), new_value.size()));
}
 
inline void JointState::set_all_state_variables(const Eigen::VectorXd& new_values) {
  if (new_values.size() != 4 * this->get_size()) {
    throw IncompatibleSizeException(
        "Input is of incorrect size: expected " + std::to_string(this->get_size()) + ", given "
            + std::to_string(new_values.size()));
  }
  this->set_positions(new_values.segment(0, this->get_size()));
  this->set_velocities(new_values.segment(this->get_size(), this->get_size()));
  this->set_accelerations(new_values.segment(2 * this->get_size(), this->get_size()));
  this->set_torques(new_values.segment(3 * this->get_size(), this->get_size()));
}
 
inline bool JointState::is_compatible(const State& state) const {
  bool compatible = this->State::is_compatible(state);
  compatible = compatible && (this->names_.size() == dynamic_cast<const JointState&>(state).names_.size());
  if (compatible) {
    for (unsigned int i = 0; i < this->names_.size(); ++i) {
      compatible = (compatible && this->names_[i] == dynamic_cast<const JointState&>(state).names_[i]);
    }
  }
  return compatible;
}
 
inline unsigned int JointState::get_size() const {
  return this->names_.size();
}
 
inline const std::vector<std::string>& JointState::get_names() const {
  return this->names_;
}
 
inline void JointState::set_names(unsigned int nb_joints) {
  if (this->get_size() != nb_joints) {
    throw exceptions::IncompatibleSizeException(
        "Input number of joints is of incorrect size, expected " + std::to_string(this->get_size()) + " got "
            + std::to_string(nb_joints));
  }
  this->names_.resize(nb_joints);
  for (unsigned int i = 0; i < nb_joints; ++i) {
    this->names_[i] = "joint" + std::to_string(i);
  }
}
 
inline void JointState::set_names(const std::vector<std::string>& names) {
  if (this->get_size() != names.size()) {
    throw exceptions::IncompatibleSizeException(
        "Input number of joints is of incorrect size, expected " + std::to_string(this->get_size()) + " got "
            + std::to_string(names.size()));
  }
  this->names_ = names;
}
 
inline Eigen::VectorXd JointState::get_state_variable(const JointStateVariable& state_variable_type) const {
  switch (state_variable_type) {
    case JointStateVariable::POSITIONS:
      return this->get_positions();
 
    case JointStateVariable::VELOCITIES:
      return this->get_velocities();
 
    case JointStateVariable::ACCELERATIONS:
      return this->get_accelerations();
 
    case JointStateVariable::TORQUES:
      return this->get_torques();
 
    case JointStateVariable::ALL:
      return this->get_all_state_variables();
  }
  // this never goes here but is compulsory to avoid a warning
  return Eigen::Vector3d::Zero();
}
 
inline void JointState::set_state_variable(
    const Eigen::VectorXd& new_value, const JointStateVariable& state_variable_type
) {
  switch (state_variable_type) {
    case JointStateVariable::POSITIONS:
      this->set_positions(new_value);
      break;
 
    case JointStateVariable::VELOCITIES:
      this->set_velocities(new_value);
      break;
 
    case JointStateVariable::ACCELERATIONS:
      this->set_accelerations(new_value);
      break;
 
    case JointStateVariable::TORQUES:
      this->set_torques(new_value);
      break;
 
    case JointStateVariable::ALL:
      this->set_all_state_variables(new_value);
      break;
  }
}
 
inline std::vector<double> JointState::to_std_vector() const {
  Eigen::VectorXd data = this->data();
  return std::vector<double>(data.data(), data.data() + data.size());
}
}// namespace state_representation