mujoco_system_interface.hpp

 
#pragma once
 
#include <memory>
#include <string>
#include <thread>
#include <vector>
 
#include <hardware_interface/version.h>
#include <hardware_interface/handle.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
#include <rclcpp_lifecycle/state.hpp>
#include <realtime_tools/realtime_publisher.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
 
#include <mujoco/mujoco.h>
 
#include "mujoco_ros2_control/data.hpp"
#include "mujoco_ros2_control/mujoco_simulation.hpp"
 
#include <pluginlib/class_list_macros.hpp>
#include <pluginlib/class_loader.hpp>
#include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp"
#include "transmission_interface/transmission.hpp"
#include "transmission_interface/transmission_interface_exception.hpp"
#include "transmission_interface/transmission_loader.hpp"
 
#define ROS_DISTRO_HUMBLE (HARDWARE_INTERFACE_VERSION_MAJOR < 3)
 
// defining these for Humble, because they are defined elsewhere in future versions, and we use them in this file
#if ROS_DISTRO_HUMBLE
namespace hardware_interface
{
constexpr char HW_IF_TORQUE[] = "torque";
constexpr char HW_IF_FORCE[] = "force";
}  // namespace hardware_interface
#endif
 
namespace mujoco_ros2_control
{
constexpr char MUJOCO_TYPE_PARAM[] = "mujoco_type";
constexpr char MUJOCO_SENSOR_NAME_PARAM[] = "mujoco_sensor_name";
 
constexpr char MUJOCO_TYPE_FTS[] = "fts";
constexpr char MUJOCO_TYPE_IMU[] = "imu";
constexpr char MUJOCO_TYPE_POSE[] = "pose";
constexpr char MUJOCO_TYPE_MAGNETOMETER[] = "magnetometer";
 
class MujocoSystemInterface : public hardware_interface::SystemInterface
{
public:
  MujocoSystemInterface();
  ~MujocoSystemInterface() override;
 
  hardware_interface::CallbackReturn
// Jazzy introduces a new HarwareComponentInterfaceParams object which doesn't exist in humble. This adds
// compatibility by switching to the old interface, which behaves similarly
#if ROS_DISTRO_HUMBLE
  on_init(const hardware_interface::HardwareInfo& info) override;
#else
  on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override;
#endif
// Jazzy+ handles own their value instead of aliasing a double*, so we build our own handles
// and export them via on_export_*(); read()/write() sync those handles against the doubles below.
#if ROS_DISTRO_HUMBLE
  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
#else
  std::vector<hardware_interface::StateInterface::ConstSharedPtr> on_export_state_interfaces() override;
  std::vector<hardware_interface::CommandInterface::SharedPtr> on_export_command_interfaces() override;
#endif
 
  hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
  hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
 
  hardware_interface::return_type perform_command_mode_switch(const std::vector<std::string>& start_interfaces,
                                                              const std::vector<std::string>& stop_interfaces) override;
 
  hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
  hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
// In humble this method doesn't exist, so we just add it back in with the implementation
#if ROS_DISTRO_HUMBLE
  const hardware_interface::HardwareInfo& get_hardware_info() const
  {
    return info_;
  }
#endif
  void actuator_state_to_joint_state();
 
  void joint_command_to_actuator_command();
 
  void get_model(mjModel*& dest);
 
  void get_data(mjData*& dest);
 
  void set_data(mjData* mj_data);
 
protected:
  rclcpp::Logger get_logger() const;
 
private:
  struct InterfaceBinding
  {
    std::string prefix;          
    std::string interface_name;  
    double* value;               
  };
 
  std::vector<InterfaceBinding> collect_state_interface_bindings();
 
  std::vector<InterfaceBinding> collect_command_interface_bindings();
 
#if !ROS_DISTRO_HUMBLE
  template <typename InterfaceT>
  struct HandleBinding
  {
    typename InterfaceT::SharedPtr handle;
    double* value;
  };
 
  std::vector<HandleBinding<hardware_interface::StateInterface>> state_bindings_;
  std::vector<HandleBinding<hardware_interface::CommandInterface>> command_bindings_;
 
  template <typename InterfaceT>
  static std::vector<HandleBinding<InterfaceT>> build_bindings(const std::vector<InterfaceBinding>& bindings);
 
  void build_interface_handles();
 
  template <typename InterfaceT>
  static void push_bindings_to_interfaces(const std::vector<HandleBinding<InterfaceT>>& bindings, bool wait_for_lock);
 
  void pull_commands_from_interfaces();
#endif
 
  bool register_mujoco_actuators();
 
  void register_urdf_joints(const hardware_interface::HardwareInfo& info);
 
  bool register_transmissions(const hardware_interface::HardwareInfo& info);
 
  bool initialize_initial_positions(const hardware_interface::HardwareInfo& info);
 
  void register_sensors(const hardware_interface::HardwareInfo& info);
 
  bool set_override_start_positions(const std::string& override_start_position_file);
 
  void set_initial_pose();
 
  void reset_simulation_state(bool fill_initial_state);
 
 
  rclcpp::Node::SharedPtr get_node() const;
 
 
  void load_mujoco_plugins();
 
  bool auto_register_plugin_if_needed(const std::string& plugin_type, const std::string& plugin_ns,
                                      const std::vector<std::string>& loaded_plugins);
  void load_legacy_cameras(const std::vector<std::string>& plugins_ns);
  void load_legacy_lidar(const std::vector<std::string>& plugins_ns);
 
  // Logger
  rclcpp::Logger logger_ = rclcpp::get_logger("MujocoSystemInterface");
 
  // The simulation host: owns the Simulate app, model/data, physics & UI threads,
  // clock publisher, and reset/pause/step services.
  std::unique_ptr<MujocoSimulation> simulation_;
 
  // Provides access to ROS interfaces for elements that require it
  std::shared_ptr<rclcpp::Node> mujoco_node_;
  std::unique_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
  std::thread executor_thread_;
 
  // Actuators state publisher
  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> actuator_state_publisher_ = nullptr;
  realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>::SharedPtr actuator_state_realtime_publisher_ =
      nullptr;
  sensor_msgs::msg::JointState actuator_state_msg_;
 
  // Floating base state publisher
  std::shared_ptr<rclcpp::Publisher<nav_msgs::msg::Odometry>> floating_base_publisher_ = nullptr;
  realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>::SharedPtr floating_base_realtime_publisher_ = nullptr;
  nav_msgs::msg::Odometry floating_base_msg_;
 
  // Free joint data
  int free_joint_id_ = -1;
  int free_joint_qpos_adr_ = -1;
  int free_joint_qvel_adr_ = -1;
 
  // Data containers for the HW interface
  std::unordered_map<std::string, hardware_interface::ComponentInfo> joint_hw_info_;
  std::unordered_map<std::string, std::vector<hardware_interface::ComponentInfo>> sensors_hw_info_;
 
  // Container for interacting with the underlying physics sim's data.
  // Handed to plugins during `write` and used to stage control inputs.
  // Its full-data refresh is skipped when no plugins are loaded.
  mjData* mj_data_control_{ nullptr };
 
  // Per-step robot state snapshot used by `read` and `write`.
  MujocoSimulation::ControlState control_state_;
 
  // Data containers for the MuJoCo Actuators
  std::vector<MuJoCoActuatorData> mujoco_actuator_data_;
 
  // Data containers for the URDF joints
  std::vector<URDFJointData> urdf_joint_data_;
 
  // Transmission instances
  std::unique_ptr<pluginlib::ClassLoader<transmission_interface::TransmissionLoader>> transmission_loader_ = nullptr;
  std::vector<std::shared_ptr<transmission_interface::Transmission>> transmission_instances_;
 
  // Plugin loader and instances
  std::unique_ptr<pluginlib::ClassLoader<mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase>> plugin_loader_ =
      nullptr;
  std::vector<std::shared_ptr<mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase>> plugin_instances_;
 
  std::vector<FTSensorData> ft_sensor_data_;
  std::vector<IMUSensorData> imu_sensor_data_;
  std::vector<SitePoseData> pose_sensor_data_;
  std::vector<MagnetometerSensorData> magnetometer_sensor_data_;
 
  bool override_mujoco_actuator_positions_{ false };
  bool override_urdf_joint_positions_{ false };
 
  std::string initial_keyframe_ = "";
};
 
}  // namespace mujoco_ros2_control