Add interface for accessing the mujoco data and model

Signed-off-by: Paul Gesel <[email protected]>

Author: pgesel-rai

include/mujoco_ros2_simulation/mujoco_system_interface.hpp

@@ -73,6 +73,30 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
   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;
 
+  /**
+   * @brief Returns a copy of the MuJoCo model.
+   *
+   * This method locks the simulation mutex to ensure thread safety.
+   * @param dest Pointer to an mjModel structure where the copy will be stored. The pointer will be allocated if it is nullptr.
+   */
+  void get_model(mjModel*& dest);
+
+  /**
+   * @brief Returns a copy of the current MuJoCo data.
+   *
+   * This method locks the simulation mutex to ensure thread safety.
+   * @param dest Pointer to an mjData structure where the copy will be stored. The pointer will be allocated if it is nullptr.
+   */
+  void get_data(mjData*& dest);
+
+  /**
+   * @brief Sets the MuJoCo data to the provided value.
+   *
+   * This method locks the simulation mutex to ensure thread safety.
+   * @param mj_data Pointer to an mjData structure containing the new data.
+   */
+  void set_data(mjData* mj_data);
+
 private:
   /**
    * @brief Loads actuator information into the HW interface.

src/mujoco_system_interface.cpp

@@ -1212,6 +1212,28 @@ void MujocoSystemInterface::publish_clock()
   clock_publisher_->publish(sim_time_msg);
 }
 
+void MujocoSystemInterface::get_model(mjModel*& dest)
+{
+  const std::unique_lock<std::recursive_mutex> lock(*sim_mutex_);
+  mj_copyModel(dest, mj_model_);
+}
+
+void MujocoSystemInterface::get_data(mjData*& dest)
+{
+  const std::unique_lock<std::recursive_mutex> lock(*sim_mutex_);
+  if (dest == nullptr)
+  {
+    dest = mj_makeData(mj_model_);
+  }
+  mj_copyData(dest, mj_model_, mj_data_);
+}
+
+void MujocoSystemInterface::set_data(mjData* mj_data)
+{
+  const std::unique_lock<std::recursive_mutex> lock(*sim_mutex_);
+  mj_copyData(mj_data_, mj_model_, mj_data);
+}
+
 }  // namespace mujoco_ros2_simulation
 
 #include "pluginlib/class_list_macros.hpp"