From ab949b7c7ddf01ada918bee2fcc5084e847f2dd4 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 20 Nov 2025 14:40:20 -0500 Subject: [PATCH 1/2] Add interface for accessing the mujoco data and model Signed-off-by: Paul Gesel --- .../mujoco_system_interface.hpp | 24 +++++++++++++++++++ src/mujoco_system_interface.cpp | 22 +++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/include/mujoco_ros2_simulation/mujoco_system_interface.hpp b/include/mujoco_ros2_simulation/mujoco_system_interface.hpp index 96c5e9c..777b859 100644 --- a/include/mujoco_ros2_simulation/mujoco_system_interface.hpp +++ b/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. diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 6b6087b..875c540 100644 --- a/src/mujoco_system_interface.cpp +++ b/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 lock(*sim_mutex_); + mj_copyModel(dest, mj_model_); +} + +void MujocoSystemInterface::get_data(mjData*& dest) +{ + const std::unique_lock 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 lock(*sim_mutex_); + mj_copyData(mj_data_, mj_model_, mj_data); +} + } // namespace mujoco_ros2_simulation #include "pluginlib/class_list_macros.hpp" From c8144c31ee1d3b224769c23448501aa5af5c910b Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 20 Nov 2025 17:49:00 -0500 Subject: [PATCH 2/2] fix pointer Signed-off-by: Paul Gesel --- src/mujoco_system_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 875c540..a6deafa 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1215,7 +1215,7 @@ void MujocoSystemInterface::publish_clock() void MujocoSystemInterface::get_model(mjModel*& dest) { const std::unique_lock lock(*sim_mutex_); - mj_copyModel(dest, mj_model_); + dest = mj_copyModel(dest, mj_model_); } void MujocoSystemInterface::get_data(mjData*& dest)