data.hpp

 
#pragma once
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include "control_toolbox/pid_ros.hpp"
 
#include <random>
#include <string>
#include <vector>
 
namespace mujoco_ros2_control
{
 
enum class ActuatorType
{
  UNKNOWN,
  MOTOR,
  POSITION,
  VELOCITY,
  PASSIVE,
  CUSTOM
};
 
struct InterfaceData
{
  explicit InterfaceData(const std::string& command_interface) : command_interface_(command_interface)
  {
  }
 
  std::string command_interface_;
  double command_ = std::numeric_limits<double>::quiet_NaN();
  double state_ = std::numeric_limits<double>::quiet_NaN();
 
  // this is the "sink" that will be part of the transmission Joint/Actuator handles
  double transmission_passthrough_ = std::numeric_limits<double>::quiet_NaN();
};
 
struct MuJoCoActuatorData
{
  std::string joint_name = "";
  InterfaceData position_interface{ hardware_interface::HW_IF_POSITION };
  InterfaceData velocity_interface{ hardware_interface::HW_IF_VELOCITY };
  InterfaceData effort_interface{ hardware_interface::HW_IF_EFFORT };
  std::shared_ptr<control_toolbox::PidROS> pos_pid{ nullptr };
  std::shared_ptr<control_toolbox::PidROS> vel_pid{ nullptr };
  ActuatorType actuator_type{ ActuatorType::UNKNOWN };
  int mj_joint_type = -1;
  int mj_pos_adr = -1;
  int mj_vel_adr = -1;
  int mj_actuator_id = -1;
 
  // Booleans record whether or not we should be writing commands to these interfaces
  // based on if they have been claimed.
  bool is_position_control_enabled{ false };
  bool is_position_pid_control_enabled{ false };
  bool is_velocity_pid_control_enabled{ false };
  bool is_velocity_control_enabled{ false };
  bool is_effort_control_enabled{ false };
  bool has_pos_pid{ false };
  bool has_vel_pid{ false };
 
  void copy_state_to_transmission()
  {
    position_interface.transmission_passthrough_ = position_interface.state_;
    velocity_interface.transmission_passthrough_ = velocity_interface.state_;
    effort_interface.transmission_passthrough_ = effort_interface.state_;
  }
 
  void copy_command_from_transmission()
  {
    position_interface.command_ = position_interface.transmission_passthrough_;
    velocity_interface.command_ = velocity_interface.transmission_passthrough_;
    effort_interface.command_ = effort_interface.transmission_passthrough_;
  }
 
  void copy_command_to_state()
  {
    position_interface.state_ = position_interface.command_;
    velocity_interface.state_ = velocity_interface.command_;
    effort_interface.state_ = effort_interface.command_;
  }
};
 
struct URDFJointData
{
  std::string name = "";
  InterfaceData position_interface{ hardware_interface::HW_IF_POSITION };
  InterfaceData velocity_interface{ hardware_interface::HW_IF_VELOCITY };
  InterfaceData effort_interface{ hardware_interface::HW_IF_EFFORT };
 
  std::vector<std::string> command_interfaces = {};
 
  bool is_mimic{ false };
  long int mimicked_joint_index;
  double mimic_multiplier;
 
  bool is_position_control_enabled{ false };
  bool is_velocity_control_enabled{ false };
  bool is_effort_control_enabled{ false };
 
  void copy_state_from_transmission()
  {
    position_interface.state_ = position_interface.transmission_passthrough_;
    velocity_interface.state_ = velocity_interface.transmission_passthrough_;
    effort_interface.state_ = effort_interface.transmission_passthrough_;
  }
 
  void copy_command_to_transmission()
  {
    position_interface.transmission_passthrough_ = position_interface.command_;
    velocity_interface.transmission_passthrough_ = velocity_interface.command_;
    effort_interface.transmission_passthrough_ = effort_interface.command_;
  }
 
  void copy_state_to_command()
  {
    position_interface.command_ = position_interface.state_;
    velocity_interface.command_ = velocity_interface.state_;
    effort_interface.command_ = effort_interface.state_;
  }
};
 
template <typename T>
struct SensorData
{
  std::string name;
  T data;
  int mj_sensor_index;
};
 
enum class NoiseDistribution
{
  kGaussian,
  kUniform,
};
 
struct NoiseState
{
  NoiseDistribution distribution = NoiseDistribution::kGaussian;
  std::mt19937 rng{ std::random_device{}() };
};
 
struct FTSensorData
{
  std::string name;
  SensorData<Eigen::Vector3d> force;
  SensorData<Eigen::Vector3d> torque;
 
  double force_noise_stddev = 0.0;
  double torque_noise_stddev = 0.0;
  NoiseState noise;
};
 
struct IMUSensorData
{
  std::string name;
  SensorData<Eigen::Quaternion<double>> orientation;
  SensorData<Eigen::Vector3d> angular_velocity;
  SensorData<Eigen::Vector3d> linear_acceleration;
 
  double orientation_noise_stddev = 0.0;
  double angular_velocity_noise_stddev = 0.0;
  double linear_acceleration_noise_stddev = 0.0;
  NoiseState noise;
 
  // Diagonal-only (independent per-axis noise) covariance, derived from the *_noise_stddev fields above at
  // registration time. Left at all-zero (as before noise support existed) when noise is not configured.
  std::vector<double> orientation_covariance;
  std::vector<double> angular_velocity_covariance;
  std::vector<double> linear_acceleration_covariance;
};
 
struct SitePoseData
{
  std::string name;
  SensorData<Eigen::Vector3d> position;
  SensorData<Eigen::Quaterniond> orientation;
 
  double position_noise_stddev = 0.0;
  double orientation_noise_stddev = 0.0;
  NoiseState noise;
};
 
struct MagnetometerSensorData
{
  std::string name;
  SensorData<Eigen::Vector3d> magnetic_field;
 
  double magnetic_field_noise_stddev = 0.0;
  NoiseState noise;
};
 
}  // namespace mujoco_ros2_control