From 1694f40733e7eca19d0c5b7a800cbef20edd0569 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Wed, 5 Nov 2025 15:55:34 +0100 Subject: [PATCH 01/29] Added mujoco topic loading --- src/mujoco_system_interface.cpp | 173 +++++++++++++++++++++++--------- 1 file changed, 124 insertions(+), 49 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 6440b55..f3826dc 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -35,6 +35,8 @@ #include #include +#include + #include #include #include @@ -178,71 +180,137 @@ const char* Diverged(int disableflags, const mjData* d) return nullptr; } -mjModel* LoadModel(const char* file, mj::Simulate& sim) +mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr node) { + std::string mujoco_model_path; + std::string robot_description; + mjModel* mnew = 0; + + //Try to get the mujoco model from URDF + // this copy is needed so that the mju::strlen call below compiles char filename[mj::Simulate::kMaxFilenameLength]; mju::strcpy_arr(filename, file); // make sure filename is not empty - if (!filename[0]) + if (filename[0]) { - return nullptr; - } + // load and compile + char loadError[kErrorLength] = ""; + auto load_start = mj::Simulate::Clock::now(); + if (mju::strlen_arr(filename) > 4 && !std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb", + mju::sizeof_arr(filename) - mju::strlen_arr(filename) + 4)) + { + mnew = mj_loadModel(filename, nullptr); + if (!mnew) + { + mju::strcpy_arr(loadError, "could not load binary model"); + } + } + else + { + mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength); + + // remove trailing newline character from loadError + if (loadError[0]) + { + int error_length = mju::strlen_arr(loadError); + if (loadError[error_length - 1] == '\n') + { + loadError[error_length - 1] = '\0'; + } + } + } + auto load_interval = mj::Simulate::Clock::now() - load_start; + double load_seconds = Seconds(load_interval).count(); - // load and compile - char loadError[kErrorLength] = ""; - mjModel* mnew = 0; - auto load_start = mj::Simulate::Clock::now(); - if (mju::strlen_arr(filename) > 4 && !std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb", - mju::sizeof_arr(filename) - mju::strlen_arr(filename) + 4)) - { - mnew = mj_loadModel(filename, nullptr); if (!mnew) { - mju::strcpy_arr(loadError, "could not load binary model"); + std::printf("%s\n", loadError); + mju::strcpy_arr(sim.load_error, loadError); + return nullptr; } - } - else - { - mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength); - // remove trailing newline character from loadError + // compiler warning: print and pause if (loadError[0]) { - int error_length = mju::strlen_arr(loadError); - if (loadError[error_length - 1] == '\n') - { - loadError[error_length - 1] = '\0'; - } + // mj_forward() below will print the warning message + std::printf("Model compiled, but simulation warning (paused):\n %s\n", loadError); + sim.run = 0; + } + + // if no error and load took more than 1/4 seconds, report load time + else if (load_seconds > 0.25) + { + mju::sprintf_arr(loadError, "Model loaded in %.2g seconds", load_seconds); } - } - auto load_interval = mj::Simulate::Clock::now() - load_start; - double load_seconds = Seconds(load_interval).count(); - if (!mnew) - { - std::printf("%s\n", loadError); mju::strcpy_arr(sim.load_error, loadError); - return nullptr; + return mnew; } - // compiler warning: print and pause - if (loadError[0]) - { - // mj_forward() below will print the warning message - std::printf("Model compiled, but simulation warning (paused):\n %s\n", loadError); - sim.run = 0; - } + // // Try to geto the mujoco_model from parameter. + // if (node->has_parameter("mujoco_model_path")) + // { + // mujoco_model_path = node->get_parameter("mujoco_model_path").as_string(); + // if (!mujoco_model_path.empty()) + // { + // RCLCPP_INFO(node->get_logger(), "Using MuJoCo model from path parameter: %s", mujoco_model_path.c_str()); + // mnew=mj_loadXML(mujoco_model_path.c_str(), nullptr, nullptr, 0); + // return mnew; + // } + // } + // RCLCPP_WARN( + // node->get_logger(), + // "Failed to get robot_description from parameter. Will listen on the ~/mujoco_robot_description " + // "topic..."); + + // Try to get the mujoco model from topic + rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + qos_profile.reliable().transient_local().keep_last(1); + // Try to get mujoco_model via topic + auto mujoco_model_sub = node->create_subscription( + "/mujoco_robot_description", qos_profile, + [&](const std_msgs::msg::String::SharedPtr msg) + { + if (!msg->data.empty() && robot_description.empty()) robot_description = msg->data; + }); + + const auto start = node->now(); + const auto timeout = rclcpp::Duration::from_seconds(5.0); + auto last_print = start; + + while (robot_description.empty() && rclcpp::ok()) { + + if (node->now() - last_print >= rclcpp::Duration::from_seconds(1.0)) { + RCLCPP_INFO(node->get_logger(), "Waiting for /mujoco_robot_description..."); + last_print = node->now(); + } - // if no error and load took more than 1/4 seconds, report load time - else if (load_seconds > 0.25) - { - mju::sprintf_arr(loadError, "Model loaded in %.2g seconds", load_seconds); + if (node->now() - start > timeout) { + RCLCPP_WARN(node->get_logger(),"Timeout waiting for robot_description_mujoco topic. Falling back to URDF."); + break; + } + rclcpp::sleep_for(std::chrono::milliseconds(200)); } + if (!robot_description.empty()){ + // Load Mujoco model + char error[1000] = "Could not load XML model"; + + mjSpec * spec= nullptr; + spec = mj_parseXMLString(robot_description.c_str(), nullptr, error, 1000); + mnew = mj_compile(spec, nullptr); + mj_deleteSpec(spec); + + if (!mnew){ + const char* myerr= mjs_getError(spec); + RCLCPP_INFO(node->get_logger(), "Error %s", myerr); + RCLCPP_FATAL(node->get_logger(), "Failed to compile MuJoCo model: %s", error); + } - mju::strcpy_arr(sim.load_error, loadError); - + RCLCPP_INFO(node->get_logger(), "Model body count: %d", mnew->nbody); + RCLCPP_INFO(node->get_logger(), "Model geom count: %d", mnew->ngeom); + } return mnew; } @@ -321,12 +389,18 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf const auto model_path_maybe = get_parameter("mujoco_model"); if (!model_path_maybe.has_value()) { - RCLCPP_FATAL(rclcpp::get_logger("MujocoSystemInterface"), "Missing 'mujoco_model' in ."); - return hardware_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + rclcpp::get_logger("MujocoSystemInterface"), + "Parameter 'mujoco_model' not found in . Try to get the mujoco model from parameter or topic" + ); + model_path_.clear(); } - model_path_ = model_path_maybe.value(); - - RCLCPP_INFO_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "Loading 'mujoco_model' from: " << model_path_); + else + { + model_path_ = model_path_maybe.value(); + RCLCPP_INFO_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "Loading 'mujoco_model' from: " << model_path_); + } + // Pull the initial speed factor from the hardware parameters, if present sim_speed_factor_ = std::stod(get_parameter("sim_speed_factor").value_or("-1")); @@ -388,7 +462,8 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf // Load the model and data prior to hw registration and starting the physics thread sim_->LoadMessage(model_path_.c_str()); - mj_model_ = LoadModel(model_path_.c_str(), *sim_); + auto node = get_node(); + mj_model_ = LoadModel(model_path_.c_str(), *sim_, node); if (!mj_model_) { RCLCPP_FATAL(rclcpp::get_logger("MujocoSystemInterface"), "Mujoco failed to load '%s'", model_path_.c_str()); From d9a08825f0541a66d48ee81cdfa4e46ca5359b7c Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Wed, 5 Nov 2025 16:39:01 +0100 Subject: [PATCH 02/29] removed the model loading from parameter --- src/mujoco_system_interface.cpp | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index f3826dc..526dc6b 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -248,22 +248,6 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr mju::strcpy_arr(sim.load_error, loadError); return mnew; } - - // // Try to geto the mujoco_model from parameter. - // if (node->has_parameter("mujoco_model_path")) - // { - // mujoco_model_path = node->get_parameter("mujoco_model_path").as_string(); - // if (!mujoco_model_path.empty()) - // { - // RCLCPP_INFO(node->get_logger(), "Using MuJoCo model from path parameter: %s", mujoco_model_path.c_str()); - // mnew=mj_loadXML(mujoco_model_path.c_str(), nullptr, nullptr, 0); - // return mnew; - // } - // } - // RCLCPP_WARN( - // node->get_logger(), - // "Failed to get robot_description from parameter. Will listen on the ~/mujoco_robot_description " - // "topic..."); // Try to get the mujoco model from topic rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); From 7372b0f4fb1766cc0feccd74af99dea424682682 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Wed, 5 Nov 2025 16:48:21 +0100 Subject: [PATCH 03/29] Improved deleteSpec --- src/mujoco_system_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 526dc6b..cfeca8a 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -284,14 +284,14 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr mjSpec * spec= nullptr; spec = mj_parseXMLString(robot_description.c_str(), nullptr, error, 1000); mnew = mj_compile(spec, nullptr); - mj_deleteSpec(spec); if (!mnew){ const char* myerr= mjs_getError(spec); + mj_deleteSpec(spec); RCLCPP_INFO(node->get_logger(), "Error %s", myerr); RCLCPP_FATAL(node->get_logger(), "Failed to compile MuJoCo model: %s", error); } - + mj_deleteSpec(spec); RCLCPP_INFO(node->get_logger(), "Model body count: %d", mnew->nbody); RCLCPP_INFO(node->get_logger(), "Model geom count: %d", mnew->ngeom); } From a2991575f347c9984d63ca5652581cea62778702 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 6 Nov 2025 09:00:23 +0100 Subject: [PATCH 04/29] improved loadModel function --- src/mujoco_system_interface.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index cfeca8a..7124f42 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -182,8 +182,6 @@ const char* Diverged(int disableflags, const mjData* d) mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr node) { - std::string mujoco_model_path; - std::string robot_description; mjModel* mnew = 0; //Try to get the mujoco model from URDF @@ -249,6 +247,8 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr return mnew; } + std::string robot_description; + // Try to get the mujoco model from topic rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_profile.reliable().transient_local().keep_last(1); @@ -272,7 +272,7 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr } if (node->now() - start > timeout) { - RCLCPP_WARN(node->get_logger(),"Timeout waiting for robot_description_mujoco topic. Falling back to URDF."); + RCLCPP_WARN(node->get_logger(),"Timeout waiting for robot_description_mujoco topic."); break; } rclcpp::sleep_for(std::chrono::milliseconds(200)); @@ -373,9 +373,9 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf const auto model_path_maybe = get_parameter("mujoco_model"); if (!model_path_maybe.has_value()) { - RCLCPP_WARN( + RCLCPP_INFO( rclcpp::get_logger("MujocoSystemInterface"), - "Parameter 'mujoco_model' not found in . Try to get the mujoco model from parameter or topic" + "Parameter 'mujoco_model' not found in . Try to get the mujoco model from topic" ); model_path_.clear(); } From 844619f80748c90a2fcc55af4503ea6ddc18a967 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Fri, 7 Nov 2025 13:56:22 +0100 Subject: [PATCH 05/29] Added control dependency on mujoco actuators --- src/mujoco_system_interface.cpp | 141 ++++++++++++++++++++++++++++++-- 1 file changed, 132 insertions(+), 9 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 7124f42..ee6a85a 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -36,6 +36,8 @@ #include #include +#include +#include #include #include @@ -51,6 +53,7 @@ const double kSimRefreshFraction = 0.7; // fraction of refresh available for si const int kErrorLength = 1024; // load error string length using Seconds = std::chrono::duration; +std::unordered_map actuator_map; namespace mujoco_ros2_simulation { @@ -180,6 +183,53 @@ const char* Diverged(int disableflags, const mjData* d) return nullptr; } +std::unordered_map parse_actuators_from_xml(const std::string& xml_path_or_str, bool from_string = false) +{ + using namespace tinyxml2; + + XMLDocument doc; + XMLError err; + if (from_string) + err = doc.Parse(xml_path_or_str.c_str()); + else + err = doc.LoadFile(xml_path_or_str.c_str()); + + std::unordered_map joint_actuator_types; + if (err != XML_SUCCESS) { + return joint_actuator_types; + } + + auto root = doc.RootElement(); + + tinyxml2::XMLElement* actuators = nullptr; + for (auto elem = root->FirstChildElement(); elem != nullptr; elem = elem->NextSiblingElement()) { + if (std::string(elem->Name()) == "actuator") { + actuators = elem; + break; + } + } + if (!actuators) { + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Actuator not found"); + return joint_actuator_types; + } + + for (XMLElement* elem = actuators->FirstChildElement(); elem; elem = elem->NextSiblingElement()) + { + std::string actuator_type = elem->Name(); + const char* joint = elem->Attribute("joint"); + const char* name = elem->Attribute("name"); + + if (joint) { + joint_actuator_types[joint] = actuator_type; + } + else if (name) { + joint_actuator_types[name] = actuator_type; + } + } + + return joint_actuator_types; +} + mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr node) { mjModel* mnew = 0; @@ -199,6 +249,7 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr if (mju::strlen_arr(filename) > 4 && !std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb", mju::sizeof_arr(filename) - mju::strlen_arr(filename) + 4)) { + // Find a way mnew = mj_loadModel(filename, nullptr); if (!mnew) { @@ -207,6 +258,13 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr } else { + + actuator_map = parse_actuators_from_xml(filename); + for (const auto& [name, type] : actuator_map) + { + RCLCPP_INFO(node->get_logger(), "Actuator '%s' type '%s'", name.c_str(), type.c_str()); + } + mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength); // remove trailing newline character from loadError @@ -281,6 +339,12 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr // Load Mujoco model char error[1000] = "Could not load XML model"; + actuator_map = parse_actuators_from_xml(robot_description, true); + for (const auto& [name, type] : actuator_map) + { + RCLCPP_INFO(node->get_logger(), "Actuator '%s' type '%s'", name.c_str(), type.c_str()); + } + mjSpec * spec= nullptr; spec = mj_parseXMLString(robot_description.c_str(), nullptr, error, 1000); mnew = mj_compile(spec, nullptr); @@ -660,12 +724,21 @@ std::vector MujocoSystemInterface::export_ { std::vector new_command_interfaces; - // Joint command interfaces + // // Joint command interfaces for (auto& joint : joint_states_) { // Add command interfaces for joint hardware. if (auto it = joint_hw_info_.find(joint.name); it != joint_hw_info_.end()) { + int actuator_id = joint.mj_actuator_id; + // + if (actuator_id < 0) + continue; + std::string actuator_name = mj_id2name(mj_model_, mjOBJ_ACTUATOR, actuator_id); + bool is_position_actuator = actuator_map[actuator_name].find("position")!= std::string::npos; + bool is_velocity_actuator = actuator_map[actuator_name].find("velocity")!= std::string::npos; + bool is_motor_actuator = actuator_map[actuator_name].find("motor")!= std::string::npos; + for (const auto& command_if : it->second.command_interfaces) { if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) @@ -674,11 +747,13 @@ std::vector MujocoSystemInterface::export_ } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity_command); + if(is_velocity_actuator || is_motor_actuator) + new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity_command); } else if (command_if.name == hardware_interface::HW_IF_EFFORT) { - new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effort_command); + if(is_motor_actuator) + new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effort_command); } } } @@ -995,23 +1070,71 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn // overwrite joint limit with min/max value for (const auto& command_if : joint.command_interfaces) { + int actuator_id = last_joint_state.mj_actuator_id; + std::string actuator_name = mj_id2name(mj_model_, mjOBJ_ACTUATOR, actuator_id); + + bool is_position_actuator = actuator_map[actuator_name].find("position")!= std::string::npos; + bool is_velocity_actuator = actuator_map[actuator_name].find("velocity")!= std::string::npos; + bool is_motor_actuator = actuator_map[actuator_name].find("motor")!= std::string::npos; + bool pid_exist=false; + // If available, always default to position control at the start if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) { - last_joint_state.is_position_control_enabled = true; - last_joint_state.position_command = last_joint_state.position; + if(is_velocity_actuator || is_motor_actuator) + { + if(!pid_exist) + { + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Position command interface for the joint : %s is not supported with velocity or motor actuator", joint.name.c_str()); + continue; + } + else if(pid_exist && is_velocity_actuator) + { + last_joint_state.is_velocity_control_enabled = true; + } + else if(pid_exist && is_motor_actuator) + { + last_joint_state.is_effort_control_enabled = true; + } + } + if(is_position_actuator) + { + last_joint_state.is_position_control_enabled = true; + last_joint_state.position_command = last_joint_state.position; + } } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - last_joint_state.is_velocity_control_enabled = true; - last_joint_state.velocity_command = last_joint_state.velocity; + if(is_position_actuator || (is_motor_actuator && !pid_exist)) + { + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Velocity command interface for the joint : %s is not supported with position or motor actuator", joint.name.c_str()); + continue; + } + if(is_motor_actuator && pid_exist) + { + last_joint_state.is_effort_control_enabled = true; + } + if(is_velocity_actuator) + { + last_joint_state.is_velocity_control_enabled = true; + last_joint_state.velocity_command = last_joint_state.velocity; + } } else if (command_if.name.find(hardware_interface::HW_IF_EFFORT) != std::string::npos) { - last_joint_state.is_effort_control_enabled = true; - last_joint_state.effort_command = last_joint_state.effort; + if(is_position_actuator || is_velocity_actuator) + { + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Effort command interface for the joint : %s is not supported with position or velocity actuator", joint.name.c_str()); + continue; + } + else{ + last_joint_state.is_effort_control_enabled = true; + last_joint_state.effort_command = last_joint_state.effort; + } } } + if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled) + throw std::runtime_error("Joint " + joint.name + " has unsupported actuator configuration"); } } From a590405f34de566f7f7dcc41c4e1d0812fc30ca0 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Mon, 10 Nov 2025 12:34:20 +0100 Subject: [PATCH 06/29] Added control_toolbox dependence --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 461a9b6..0235da1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED) find_package(controller_manager REQUIRED) find_package(Eigen3 REQUIRED) find_package(glfw3 REQUIRED) +find_package(control_toolbox REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -158,6 +159,7 @@ target_link_libraries(mujoco_ros2_simulation pluginlib::pluginlib rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle + control_toolbox::control_toolbox sensor_msgs::sensor_msgs_library Eigen3::Eigen Threads::Threads From e50a3d61acb0945d424ff13e93763a74ff26d58d Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Mon, 10 Nov 2025 12:36:34 +0100 Subject: [PATCH 07/29] Added pid control --- src/mujoco_system_interface.cpp | 148 ++++++++++++++++++++++++++------ 1 file changed, 124 insertions(+), 24 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index ee6a85a..3b48c78 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -43,6 +43,8 @@ #include #include +#include "control_toolbox/pid.hpp" + #define MUJOCO_PLUGIN_DIR "mujoco_plugin" using namespace std::chrono_literals; @@ -53,6 +55,8 @@ const double kSimRefreshFraction = 0.7; // fraction of refresh available for si const int kErrorLength = 1024; // load error string length using Seconds = std::chrono::duration; + +/// \brief vector with the current actuator for each joint std::unordered_map actuator_map; namespace mujoco_ros2_simulation @@ -249,7 +253,6 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr if (mju::strlen_arr(filename) > 4 && !std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb", mju::sizeof_arr(filename) - mju::strlen_arr(filename) + 4)) { - // Find a way mnew = mj_loadModel(filename, nullptr); if (!mnew) { @@ -262,7 +265,7 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr actuator_map = parse_actuators_from_xml(filename); for (const auto& [name, type] : actuator_map) { - RCLCPP_INFO(node->get_logger(), "Actuator '%s' type '%s'", name.c_str(), type.c_str()); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Actuator '%s' type '%s'", name.c_str(), type.c_str()); } mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength); @@ -574,6 +577,64 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf return hardware_interface::CallbackReturn::SUCCESS; } +bool extractPIDFromParameters( + const std::string & control_mode, const std::string & joint_name, + control_toolbox::Pid & pid, rclcpp::Node::SharedPtr node) +{ + const std::string parameter_prefix = "pid_gains." + control_mode + "." + joint_name; + auto get_pid_entry = [node, parameter_prefix](const std::string & entry, double & value) -> bool { + try { + // Check if the parameter is declared, if not, declare the default value NaN + if (!node->has_parameter(parameter_prefix + "." + entry)) { + node->declare_parameter( + parameter_prefix + "." + entry, + std::numeric_limits::quiet_NaN()); + } + value = node->get_parameter(parameter_prefix + "." + entry).as_double(); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + node->get_logger(), + "Parameter '%s' not declared, with error %s", entry.c_str(), ex.what()); + return false; + } catch (rclcpp::exceptions::InvalidParameterTypeException & ex) { + RCLCPP_ERROR( + node->get_logger(), + "Parameter '%s' has wrong type: %s", entry.c_str(), ex.what()); + return false; + } + return std::isfinite(value); + }; + bool are_pids_set = true; + double kp, ki, kd, max_integral_error, min_integral_error; + are_pids_set &= get_pid_entry("kp", kp); + are_pids_set &= get_pid_entry("ki", ki); + are_pids_set &= get_pid_entry("kd", kd); + if (are_pids_set) { + // RCLCPP_INFO( + // node->get_logger(), + // "Foundamental params set"); + get_pid_entry("max_integral_error", max_integral_error); + get_pid_entry("min_integral_error", min_integral_error); + RCLCPP_INFO_STREAM( + node->get_logger(), + "Setting kp = " << kp << "\t" + << " ki = " << ki << "\t" + << " kd = " << kd << "\t" + << " max_integral_error = " << max_integral_error << "\t" + << " min_integral_error = " << min_integral_error << " from node parameters"); + control_toolbox::AntiWindupStrategy antiwindup_strat; + antiwindup_strat.set_type("none"); + antiwindup_strat.i_max = max_integral_error; + antiwindup_strat.i_min = min_integral_error; + pid.initialize( + kp, ki, kd, + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), antiwindup_strat); + } + + return are_pids_set; +} + std::vector MujocoSystemInterface::export_state_interfaces() { std::vector new_state_interfaces; @@ -724,7 +785,7 @@ std::vector MujocoSystemInterface::export_ { std::vector new_command_interfaces; - // // Joint command interfaces + // Joint command interfaces for (auto& joint : joint_states_) { // Add command interfaces for joint hardware. @@ -819,16 +880,32 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_position_control_enabled = false; joint_it->is_velocity_control_enabled = false; joint_it->is_effort_control_enabled = false; + joint_it->is_position_pid_control_enabled = false; + joint_it->is_velocity_pid_control_enabled = false; if (interface_type == hardware_interface::HW_IF_POSITION) { - joint_it->is_position_control_enabled = true; - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), - "Joint %s: position control enabled (velocity, effort disabled)", joint_name.c_str()); + if(joint_it->is_pos_pid) + { + joint_it->is_position_pid_control_enabled = true; + } + else + { + joint_it->is_position_control_enabled = true; + } + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), + "Joint %s: position control enabled (velocity, effort disabled)", joint_name.c_str()); } else if (interface_type == hardware_interface::HW_IF_VELOCITY) { - joint_it->is_velocity_control_enabled = true; + if(joint_it->is_vel_pid) + { + joint_it->is_velocity_pid_control_enabled = true; + } + else + { + joint_it->is_velocity_control_enabled = true; + } RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Joint %s: velocity control enabled (position, effort disabled)", joint_name.c_str()); } @@ -844,10 +921,12 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_position_control_enabled = false; + joint_it->is_position_pid_control_enabled = false; } else if (interface_type == hardware_interface::HW_IF_VELOCITY) { joint_it->is_velocity_control_enabled = false; + joint_it->is_velocity_pid_control_enabled = false; } else if (interface_type == hardware_interface::HW_IF_EFFORT) { @@ -917,7 +996,7 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& } hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& /*time*/, - const rclcpp::Duration& /*period*/) + const rclcpp::Duration &period) { // Update mimic joints for (auto& joint_state : joint_states_) @@ -947,10 +1026,22 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& { mj_data_control_->ctrl[joint_state.mj_actuator_id] = joint_state.position_command; } + else if(joint_state.is_position_pid_control_enabled) + { + double error = joint_state.position_command - mj_data_->qpos[joint_state.mj_pos_adr]; + mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = + joint_state.pos_pid.computeCommand(error, period.nanoseconds()); + } else if (joint_state.is_velocity_control_enabled) { mj_data_control_->ctrl[joint_state.mj_actuator_id] = joint_state.velocity_command; } + else if(joint_state.is_velocity_pid_control_enabled) + { + double error = joint_state.velocity_command - mj_data_->qvel[joint_state.mj_vel_adr]; + mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = + joint_state.vel_pid.computeCommand(error, period.nanoseconds()); + } else if (joint_state.is_effort_control_enabled) { mj_data_control_->ctrl[joint_state.mj_actuator_id] = joint_state.effort_command; @@ -963,6 +1054,7 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& void MujocoSystemInterface::register_joints(const hardware_interface::HardwareInfo& hardware_info) { joint_states_.resize(hardware_info.joints.size()); + auto node = get_node(); for (size_t joint_index = 0; joint_index < hardware_info.joints.size(); joint_index++) { @@ -1076,25 +1168,27 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn bool is_position_actuator = actuator_map[actuator_name].find("position")!= std::string::npos; bool is_velocity_actuator = actuator_map[actuator_name].find("velocity")!= std::string::npos; bool is_motor_actuator = actuator_map[actuator_name].find("motor")!= std::string::npos; - bool pid_exist=false; // If available, always default to position control at the start if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) { if(is_velocity_actuator || is_motor_actuator) { - if(!pid_exist) + if(extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node) && is_velocity_actuator) { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Position command interface for the joint : %s is not supported with velocity or motor actuator", joint.name.c_str()); - continue; + last_joint_state.is_position_pid_control_enabled = true; + last_joint_state.is_pos_pid=true; } - else if(pid_exist && is_velocity_actuator) + else if(extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node) && is_motor_actuator) { - last_joint_state.is_velocity_control_enabled = true; + last_joint_state.is_position_pid_control_enabled = true; + last_joint_state.is_pos_pid=true; + } - else if(pid_exist && is_motor_actuator) + else { - last_joint_state.is_effort_control_enabled = true; + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Position command interface for the joint : %s is not supported with velocity or motor actuator", joint.name.c_str()); + continue; } } if(is_position_actuator) @@ -1105,14 +1199,18 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - if(is_position_actuator || (is_motor_actuator && !pid_exist)) + if(is_position_actuator || is_motor_actuator) { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Velocity command interface for the joint : %s is not supported with position or motor actuator", joint.name.c_str()); - continue; - } - if(is_motor_actuator && pid_exist) - { - last_joint_state.is_effort_control_enabled = true; + if(is_motor_actuator && extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node)) + { + last_joint_state.is_velocity_pid_control_enabled = true; + last_joint_state.is_vel_pid=true; + } + else + { + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Velocity command interface for the joint : %s is not supported with position or motor actuator", joint.name.c_str()); + continue; + } } if(is_velocity_actuator) { @@ -1133,7 +1231,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } } } - if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled) + if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled && !!last_joint_state.is_position_pid_control_enabled && !!last_joint_state.is_velocity_pid_control_enabled) throw std::runtime_error("Joint " + joint.name + " has unsupported actuator configuration"); } } @@ -1306,6 +1404,7 @@ void MujocoSystemInterface::PhysicsLoop() // Copy data to the control mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, mj_model_->nu); + mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, mj_model_->nu); // run single step, let next iteration deal with timing mj_step(mj_model_, mj_data_); @@ -1344,6 +1443,7 @@ void MujocoSystemInterface::PhysicsLoop() // Copy data to the control mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, mj_model_->nu); + mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, mj_model_->nu); // call mj_step mj_step(mj_model_, mj_data_); From b9e82ace732155cb2295dc27c23f745aaf4ad63e Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Mon, 10 Nov 2025 17:17:02 +0100 Subject: [PATCH 08/29] Improved structure --- include/mujoco_ros2_simulation/data.hpp | 7 ++ src/mujoco_system_interface.cpp | 150 ++++++++++++------------ 2 files changed, 80 insertions(+), 77 deletions(-) diff --git a/include/mujoco_ros2_simulation/data.hpp b/include/mujoco_ros2_simulation/data.hpp index 02fa65a..8d919c0 100644 --- a/include/mujoco_ros2_simulation/data.hpp +++ b/include/mujoco_ros2_simulation/data.hpp @@ -19,6 +19,7 @@ #pragma once +#include "control_toolbox/pid.hpp" #include #include @@ -37,6 +38,8 @@ struct JointState double position; double velocity; double effort; + control_toolbox::Pid pos_pid; + control_toolbox::Pid vel_pid; double position_command; double velocity_command; double effort_command; @@ -51,8 +54,12 @@ struct JointState // 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 }; }; template diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 3b48c78..bfd0136 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -187,6 +187,64 @@ const char* Diverged(int disableflags, const mjData* d) return nullptr; } +bool extractPIDFromParameters( + const std::string & control_mode, const std::string & joint_name, + control_toolbox::Pid & pid, rclcpp::Node::SharedPtr node) +{ + const std::string parameter_prefix = "pid_gains." + control_mode + "." + joint_name; + auto get_pid_entry = [node, parameter_prefix](const std::string & entry, double & value) -> bool { + try { + // Check if the parameter is declared, if not, declare the default value NaN + if (!node->has_parameter(parameter_prefix + "." + entry)) { + node->declare_parameter( + parameter_prefix + "." + entry, + std::numeric_limits::quiet_NaN()); + } + value = node->get_parameter(parameter_prefix + "." + entry).as_double(); + } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { + RCLCPP_ERROR( + node->get_logger(), + "Parameter '%s' not declared, with error %s", entry.c_str(), ex.what()); + return false; + } catch (rclcpp::exceptions::InvalidParameterTypeException & ex) { + RCLCPP_ERROR( + node->get_logger(), + "Parameter '%s' has wrong type: %s", entry.c_str(), ex.what()); + return false; + } + return std::isfinite(value); + }; + bool are_pids_set = true; + double kp, ki, kd, max_integral_error, min_integral_error; + are_pids_set &= get_pid_entry("kp", kp); + are_pids_set &= get_pid_entry("ki", ki); + are_pids_set &= get_pid_entry("kd", kd); + if (are_pids_set) { + // RCLCPP_INFO( + // node->get_logger(), + // "Foundamental params set"); + get_pid_entry("max_integral_error", max_integral_error); + get_pid_entry("min_integral_error", min_integral_error); + RCLCPP_INFO_STREAM( + node->get_logger(), + "Setting kp = " << kp << "\t" + << " ki = " << ki << "\t" + << " kd = " << kd << "\t" + << " max_integral_error = " << max_integral_error << "\t" + << " min_integral_error = " << min_integral_error << " from node parameters"); + control_toolbox::AntiWindupStrategy antiwindup_strat; + antiwindup_strat.set_type("none"); + antiwindup_strat.i_max = max_integral_error; + antiwindup_strat.i_min = min_integral_error; + pid.initialize( + kp, ki, kd, + std::numeric_limits::infinity(), + -std::numeric_limits::infinity(), antiwindup_strat); + } + + return are_pids_set; +} + std::unordered_map parse_actuators_from_xml(const std::string& xml_path_or_str, bool from_string = false) { using namespace tinyxml2; @@ -200,6 +258,7 @@ std::unordered_map parse_actuators_from_xml(const std: std::unordered_map joint_actuator_types; if (err != XML_SUCCESS) { + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Error parsing XML model"); return joint_actuator_types; } @@ -213,7 +272,7 @@ std::unordered_map parse_actuators_from_xml(const std: } } if (!actuators) { - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Actuator not found"); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Actuator tag not found in the MuJoCo model"); return joint_actuator_types; } @@ -223,12 +282,12 @@ std::unordered_map parse_actuators_from_xml(const std: const char* joint = elem->Attribute("joint"); const char* name = elem->Attribute("name"); - if (joint) { - joint_actuator_types[joint] = actuator_type; - } - else if (name) { + if (name) { joint_actuator_types[name] = actuator_type; } + else { + RCLCPP_WARN_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "MuJoCo actuator missing name"); + } } return joint_actuator_types; @@ -261,11 +320,10 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr } else { - actuator_map = parse_actuators_from_xml(filename); for (const auto& [name, type] : actuator_map) { - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Actuator '%s' type '%s'", name.c_str(), type.c_str()); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Found mujoco actuator '%s' of type '%s'", name.c_str(), type.c_str()); } mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength); @@ -345,7 +403,7 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr actuator_map = parse_actuators_from_xml(robot_description, true); for (const auto& [name, type] : actuator_map) { - RCLCPP_INFO(node->get_logger(), "Actuator '%s' type '%s'", name.c_str(), type.c_str()); + RCLCPP_INFO(node->get_logger(), "Found mujoco actuator '%s' of type '%s'", name.c_str(), type.c_str()); } mjSpec * spec= nullptr; @@ -442,7 +500,7 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf { RCLCPP_INFO( rclcpp::get_logger("MujocoSystemInterface"), - "Parameter 'mujoco_model' not found in . Try to get the mujoco model from topic" + "Parameter 'mujoco_model' not found in URDF." ); model_path_.clear(); } @@ -577,64 +635,6 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf return hardware_interface::CallbackReturn::SUCCESS; } -bool extractPIDFromParameters( - const std::string & control_mode, const std::string & joint_name, - control_toolbox::Pid & pid, rclcpp::Node::SharedPtr node) -{ - const std::string parameter_prefix = "pid_gains." + control_mode + "." + joint_name; - auto get_pid_entry = [node, parameter_prefix](const std::string & entry, double & value) -> bool { - try { - // Check if the parameter is declared, if not, declare the default value NaN - if (!node->has_parameter(parameter_prefix + "." + entry)) { - node->declare_parameter( - parameter_prefix + "." + entry, - std::numeric_limits::quiet_NaN()); - } - value = node->get_parameter(parameter_prefix + "." + entry).as_double(); - } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { - RCLCPP_ERROR( - node->get_logger(), - "Parameter '%s' not declared, with error %s", entry.c_str(), ex.what()); - return false; - } catch (rclcpp::exceptions::InvalidParameterTypeException & ex) { - RCLCPP_ERROR( - node->get_logger(), - "Parameter '%s' has wrong type: %s", entry.c_str(), ex.what()); - return false; - } - return std::isfinite(value); - }; - bool are_pids_set = true; - double kp, ki, kd, max_integral_error, min_integral_error; - are_pids_set &= get_pid_entry("kp", kp); - are_pids_set &= get_pid_entry("ki", ki); - are_pids_set &= get_pid_entry("kd", kd); - if (are_pids_set) { - // RCLCPP_INFO( - // node->get_logger(), - // "Foundamental params set"); - get_pid_entry("max_integral_error", max_integral_error); - get_pid_entry("min_integral_error", min_integral_error); - RCLCPP_INFO_STREAM( - node->get_logger(), - "Setting kp = " << kp << "\t" - << " ki = " << ki << "\t" - << " kd = " << kd << "\t" - << " max_integral_error = " << max_integral_error << "\t" - << " min_integral_error = " << min_integral_error << " from node parameters"); - control_toolbox::AntiWindupStrategy antiwindup_strat; - antiwindup_strat.set_type("none"); - antiwindup_strat.i_max = max_integral_error; - antiwindup_strat.i_min = min_integral_error; - pid.initialize( - kp, ki, kd, - std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), antiwindup_strat); - } - - return are_pids_set; -} - std::vector MujocoSystemInterface::export_state_interfaces() { std::vector new_state_interfaces; @@ -885,7 +885,7 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_pos_pid) + if(joint_it->has_pos_pid) { joint_it->is_position_pid_control_enabled = true; } @@ -898,7 +898,7 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_vel_pid) + if(joint_it->has_vel_pid) { joint_it->is_velocity_pid_control_enabled = true; } @@ -1177,13 +1177,8 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn if(extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node) && is_velocity_actuator) { last_joint_state.is_position_pid_control_enabled = true; - last_joint_state.is_pos_pid=true; - } - else if(extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node) && is_motor_actuator) - { - last_joint_state.is_position_pid_control_enabled = true; - last_joint_state.is_pos_pid=true; - + last_joint_state.position_command = last_joint_state.position; + last_joint_state.has_pos_pid=true; } else { @@ -1194,6 +1189,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn if(is_position_actuator) { last_joint_state.is_position_control_enabled = true; + last_joint_state.velocity_command = last_joint_state.velocity; last_joint_state.position_command = last_joint_state.position; } } @@ -1204,7 +1200,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn if(is_motor_actuator && extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node)) { last_joint_state.is_velocity_pid_control_enabled = true; - last_joint_state.is_vel_pid=true; + last_joint_state.has_vel_pid=true; } else { From 85143cfed3f7ad42c034020f90f754008348919b Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Tue, 11 Nov 2025 12:01:28 +0100 Subject: [PATCH 09/29] Added joint mujoco actuator type --- include/mujoco_ros2_simulation/data.hpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/include/mujoco_ros2_simulation/data.hpp b/include/mujoco_ros2_simulation/data.hpp index 8d919c0..5c5d87b 100644 --- a/include/mujoco_ros2_simulation/data.hpp +++ b/include/mujoco_ros2_simulation/data.hpp @@ -29,6 +29,15 @@ namespace mujoco_ros2_simulation { +enum class ActuatorType +{ + UNKNOWN, + MOTOR, + POSITION, + VELOCITY, + GENERAL +}; + /** * Wrapper for mujoco actuators and relevant ROS HW interface data. */ @@ -40,6 +49,7 @@ struct JointState double effort; control_toolbox::Pid pos_pid; control_toolbox::Pid vel_pid; + ActuatorType actuator_type; double position_command; double velocity_command; double effort_command; From d8d8645b44d307baf828f4d8b428425c940e2181 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Tue, 11 Nov 2025 12:02:18 +0100 Subject: [PATCH 10/29] Improved logic --- src/mujoco_system_interface.cpp | 125 ++++++++------------------------ 1 file changed, 32 insertions(+), 93 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index bfd0136..c0509de 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -220,9 +220,6 @@ bool extractPIDFromParameters( are_pids_set &= get_pid_entry("ki", ki); are_pids_set &= get_pid_entry("kd", kd); if (are_pids_set) { - // RCLCPP_INFO( - // node->get_logger(), - // "Foundamental params set"); get_pid_entry("max_integral_error", max_integral_error); get_pid_entry("min_integral_error", min_integral_error); RCLCPP_INFO_STREAM( @@ -245,54 +242,6 @@ bool extractPIDFromParameters( return are_pids_set; } -std::unordered_map parse_actuators_from_xml(const std::string& xml_path_or_str, bool from_string = false) -{ - using namespace tinyxml2; - - XMLDocument doc; - XMLError err; - if (from_string) - err = doc.Parse(xml_path_or_str.c_str()); - else - err = doc.LoadFile(xml_path_or_str.c_str()); - - std::unordered_map joint_actuator_types; - if (err != XML_SUCCESS) { - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Error parsing XML model"); - return joint_actuator_types; - } - - auto root = doc.RootElement(); - - tinyxml2::XMLElement* actuators = nullptr; - for (auto elem = root->FirstChildElement(); elem != nullptr; elem = elem->NextSiblingElement()) { - if (std::string(elem->Name()) == "actuator") { - actuators = elem; - break; - } - } - if (!actuators) { - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Actuator tag not found in the MuJoCo model"); - return joint_actuator_types; - } - - for (XMLElement* elem = actuators->FirstChildElement(); elem; elem = elem->NextSiblingElement()) - { - std::string actuator_type = elem->Name(); - const char* joint = elem->Attribute("joint"); - const char* name = elem->Attribute("name"); - - if (name) { - joint_actuator_types[name] = actuator_type; - } - else { - RCLCPP_WARN_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "MuJoCo actuator missing name"); - } - } - - return joint_actuator_types; -} - mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr node) { mjModel* mnew = 0; @@ -320,12 +269,6 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr } else { - actuator_map = parse_actuators_from_xml(filename); - for (const auto& [name, type] : actuator_map) - { - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Found mujoco actuator '%s' of type '%s'", name.c_str(), type.c_str()); - } - mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength); // remove trailing newline character from loadError @@ -400,12 +343,6 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr // Load Mujoco model char error[1000] = "Could not load XML model"; - actuator_map = parse_actuators_from_xml(robot_description, true); - for (const auto& [name, type] : actuator_map) - { - RCLCPP_INFO(node->get_logger(), "Found mujoco actuator '%s' of type '%s'", name.c_str(), type.c_str()); - } - mjSpec * spec= nullptr; spec = mj_parseXMLString(robot_description.c_str(), nullptr, error, 1000); mnew = mj_compile(spec, nullptr); @@ -791,15 +728,6 @@ std::vector MujocoSystemInterface::export_ // Add command interfaces for joint hardware. if (auto it = joint_hw_info_.find(joint.name); it != joint_hw_info_.end()) { - int actuator_id = joint.mj_actuator_id; - // - if (actuator_id < 0) - continue; - std::string actuator_name = mj_id2name(mj_model_, mjOBJ_ACTUATOR, actuator_id); - bool is_position_actuator = actuator_map[actuator_name].find("position")!= std::string::npos; - bool is_velocity_actuator = actuator_map[actuator_name].find("velocity")!= std::string::npos; - bool is_motor_actuator = actuator_map[actuator_name].find("motor")!= std::string::npos; - for (const auto& command_if : it->second.command_interfaces) { if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) @@ -808,12 +736,12 @@ std::vector MujocoSystemInterface::export_ } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - if(is_velocity_actuator || is_motor_actuator) + if(joint.actuator_type == ActuatorType::VELOCITY || joint.actuator_type == ActuatorType::MOTOR) new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity_command); } else if (command_if.name == hardware_interface::HW_IF_EFFORT) { - if(is_motor_actuator) + if(joint.actuator_type == ActuatorType::MOTOR) new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effort_command); } } @@ -1030,7 +958,7 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& { double error = joint_state.position_command - mj_data_->qpos[joint_state.mj_pos_adr]; mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = - joint_state.pos_pid.computeCommand(error, period.nanoseconds()); + joint_state.pos_pid.compute_command(error, period.nanoseconds()); } else if (joint_state.is_velocity_control_enabled) { @@ -1040,7 +968,7 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& { double error = joint_state.velocity_command - mj_data_->qvel[joint_state.mj_vel_adr]; mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = - joint_state.vel_pid.computeCommand(error, period.nanoseconds()); + joint_state.vel_pid.compute_command(error, period.nanoseconds()); } else if (joint_state.is_effort_control_enabled) { @@ -1157,24 +1085,36 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn last_joint_state.effort = get_initial_value(state_if); } } + // + if (mujoco_actuator_id < 0) + continue; + + int biastype = mj_model_->actuator_biastype[mujoco_actuator_id]; + int gaintype = mj_model_->actuator_gaintype[mujoco_actuator_id]; + const int NBias = 10; + const mjtNum* biasprm = mj_model_->actuator_biasprm + mujoco_actuator_id * NBias; + + if (biastype == mjBIAS_NONE) + last_joint_state.actuator_type = ActuatorType::MOTOR; + else if (biastype == mjBIAS_AFFINE && biasprm[1] != 0 && biasprm[2] != 0) + last_joint_state.actuator_type = ActuatorType::POSITION; + else if (biastype == mjBIAS_AFFINE && biasprm[1] == 0 && biasprm[2] != 0) + last_joint_state.actuator_type = ActuatorType::VELOCITY; + else if (biastype == mjBIAS_USER || gaintype == mjGAIN_USER) + last_joint_state.actuator_type = ActuatorType::GENERAL; + else + last_joint_state.actuator_type = ActuatorType::UNKNOWN; // command interfaces // overwrite joint limit with min/max value for (const auto& command_if : joint.command_interfaces) { - int actuator_id = last_joint_state.mj_actuator_id; - std::string actuator_name = mj_id2name(mj_model_, mjOBJ_ACTUATOR, actuator_id); - - bool is_position_actuator = actuator_map[actuator_name].find("position")!= std::string::npos; - bool is_velocity_actuator = actuator_map[actuator_name].find("velocity")!= std::string::npos; - bool is_motor_actuator = actuator_map[actuator_name].find("motor")!= std::string::npos; - // If available, always default to position control at the start if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) { - if(is_velocity_actuator || is_motor_actuator) + if(last_joint_state.actuator_type == ActuatorType::VELOCITY || last_joint_state.actuator_type == ActuatorType::MOTOR) { - if(extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node) && is_velocity_actuator) + if(extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node)) { last_joint_state.is_position_pid_control_enabled = true; last_joint_state.position_command = last_joint_state.position; @@ -1186,18 +1126,17 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn continue; } } - if(is_position_actuator) + if(last_joint_state.actuator_type == ActuatorType::POSITION) { last_joint_state.is_position_control_enabled = true; - last_joint_state.velocity_command = last_joint_state.velocity; last_joint_state.position_command = last_joint_state.position; } } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - if(is_position_actuator || is_motor_actuator) + if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::MOTOR) { - if(is_motor_actuator && extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node)) + if(last_joint_state.actuator_type == ActuatorType::MOTOR && extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node)) { last_joint_state.is_velocity_pid_control_enabled = true; last_joint_state.has_vel_pid=true; @@ -1208,7 +1147,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn continue; } } - if(is_velocity_actuator) + if(last_joint_state.actuator_type == ActuatorType::VELOCITY) { last_joint_state.is_velocity_control_enabled = true; last_joint_state.velocity_command = last_joint_state.velocity; @@ -1216,7 +1155,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } else if (command_if.name.find(hardware_interface::HW_IF_EFFORT) != std::string::npos) { - if(is_position_actuator || is_velocity_actuator) + if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::VELOCITY) { RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Effort command interface for the joint : %s is not supported with position or velocity actuator", joint.name.c_str()); continue; @@ -1227,8 +1166,8 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } } } - if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled && !!last_joint_state.is_position_pid_control_enabled && !!last_joint_state.is_velocity_pid_control_enabled) - throw std::runtime_error("Joint " + joint.name + " has unsupported actuator configuration"); + // if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled && !last_joint_state.is_position_pid_control_enabled && !last_joint_state.is_velocity_pid_control_enabled) + // throw std::runtime_error("Joint " + joint.name + " has unsupported actuator configuration"); } } From 3846eb43713512197c9e09c60c1fdfafaa3d4dd7 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Tue, 11 Nov 2025 16:44:44 +0100 Subject: [PATCH 11/29] Improved variable management --- src/mujoco_system_interface.cpp | 35 +++++++++++++++++++-------------- 1 file changed, 20 insertions(+), 15 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index c0509de..3cd8de6 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -813,26 +813,26 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectorhas_pos_pid) + if(joint_it->actuator_type == ActuatorType::POSITION) { - joint_it->is_position_pid_control_enabled = true; + joint_it->is_position_control_enabled = true; } - else + else if (joint_it->has_pos_pid) { - joint_it->is_position_control_enabled = true; + joint_it->is_position_pid_control_enabled = true; } RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Joint %s: position control enabled (velocity, effort disabled)", joint_name.c_str()); } else if (interface_type == hardware_interface::HW_IF_VELOCITY) { - if(joint_it->has_vel_pid) + if(joint_it->actuator_type == ActuatorType::VELOCITY) { - joint_it->is_velocity_pid_control_enabled = true; + joint_it->is_velocity_control_enabled = true; } - else + else if(joint_it->has_vel_pid) { - joint_it->is_velocity_control_enabled = true; + joint_it->is_velocity_pid_control_enabled = true; } RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Joint %s: velocity control enabled (position, effort disabled)", joint_name.c_str()); @@ -1104,6 +1104,10 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn last_joint_state.actuator_type = ActuatorType::GENERAL; else last_joint_state.actuator_type = ActuatorType::UNKNOWN; + + last_joint_state.has_pos_pid=extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node); + last_joint_state.has_vel_pid=extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node); + // command interfaces // overwrite joint limit with min/max value @@ -1114,11 +1118,9 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn { if(last_joint_state.actuator_type == ActuatorType::VELOCITY || last_joint_state.actuator_type == ActuatorType::MOTOR) { - if(extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node)) + if(last_joint_state.has_pos_pid) { last_joint_state.is_position_pid_control_enabled = true; - last_joint_state.position_command = last_joint_state.position; - last_joint_state.has_pos_pid=true; } else { @@ -1136,10 +1138,9 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn { if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::MOTOR) { - if(last_joint_state.actuator_type == ActuatorType::MOTOR && extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node)) + if(last_joint_state.actuator_type == ActuatorType::MOTOR && last_joint_state.has_vel_pid) { last_joint_state.is_velocity_pid_control_enabled = true; - last_joint_state.has_vel_pid=true; } else { @@ -1166,8 +1167,12 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } } } - // if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled && !last_joint_state.is_position_pid_control_enabled && !last_joint_state.is_velocity_pid_control_enabled) - // throw std::runtime_error("Joint " + joint.name + " has unsupported actuator configuration"); + if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled && !last_joint_state.is_position_pid_control_enabled && !last_joint_state.is_velocity_pid_control_enabled) + { + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), + "Joint '%s' has an unsupported actuator configuration", joint.name.c_str()); + // return hardware_interface::return_type::ERROR; + } } } From 62f0f4f895443ea7529de718cc9be0a764d69ea9 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Wed, 12 Nov 2025 10:10:29 +0100 Subject: [PATCH 12/29] Fixed export_command_interfaces --- src/mujoco_system_interface.cpp | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 3cd8de6..8d0aaee 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -222,13 +222,6 @@ bool extractPIDFromParameters( if (are_pids_set) { get_pid_entry("max_integral_error", max_integral_error); get_pid_entry("min_integral_error", min_integral_error); - RCLCPP_INFO_STREAM( - node->get_logger(), - "Setting kp = " << kp << "\t" - << " ki = " << ki << "\t" - << " kd = " << kd << "\t" - << " max_integral_error = " << max_integral_error << "\t" - << " min_integral_error = " << min_integral_error << " from node parameters"); control_toolbox::AntiWindupStrategy antiwindup_strat; antiwindup_strat.set_type("none"); antiwindup_strat.i_max = max_integral_error; @@ -732,16 +725,17 @@ std::vector MujocoSystemInterface::export_ { if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) { - new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, &joint.position_command); + if(joint.is_position_control_enabled || joint.is_position_pid_control_enabled) + new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, &joint.position_command); } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - if(joint.actuator_type == ActuatorType::VELOCITY || joint.actuator_type == ActuatorType::MOTOR) + if(joint.is_velocity_control_enabled || joint.is_velocity_pid_control_enabled) new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity_command); } else if (command_if.name == hardware_interface::HW_IF_EFFORT) { - if(joint.actuator_type == ActuatorType::MOTOR) + if(joint.is_effort_control_enabled) new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effort_command); } } @@ -1121,6 +1115,11 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn if(last_joint_state.has_pos_pid) { last_joint_state.is_position_pid_control_enabled = true; + double p, i, d, i_max, i_min; + last_joint_state.pos_pid.getGains(p, i, d, i_max, i_min); + + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Position control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f", joint.name.c_str(), p, i, d, i_max, i_min); + } else { @@ -1141,6 +1140,10 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn if(last_joint_state.actuator_type == ActuatorType::MOTOR && last_joint_state.has_vel_pid) { last_joint_state.is_velocity_pid_control_enabled = true; + double p, i, d, i_max, i_min; + last_joint_state.vel_pid.getGains(p, i, d, i_max, i_min); + + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f", joint.name.c_str(), p, i, d, i_max, i_min); } else { @@ -1169,9 +1172,8 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled && !last_joint_state.is_position_pid_control_enabled && !last_joint_state.is_velocity_pid_control_enabled) { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), - "Joint '%s' has an unsupported actuator configuration", joint.name.c_str()); - // return hardware_interface::return_type::ERROR; + throw std::runtime_error( + std::string("Joint '") + joint.name + "' has an unsupported command interface for the specified MuJoCo actuator"); } } } From 8138c52ce408d863698d3f56eef30d4f5747d25b Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Wed, 12 Nov 2025 11:10:23 +0100 Subject: [PATCH 13/29] managed unknown mujoco actuator --- src/mujoco_system_interface.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 8d0aaee..3961890 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1095,10 +1095,17 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn else if (biastype == mjBIAS_AFFINE && biasprm[1] == 0 && biasprm[2] != 0) last_joint_state.actuator_type = ActuatorType::VELOCITY; else if (biastype == mjBIAS_USER || gaintype == mjGAIN_USER) + { last_joint_state.actuator_type = ActuatorType::GENERAL; - else + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "General MuJoCo actuator for the joint : %s , using effort command interface", joint.name.c_str()); + } + else + { last_joint_state.actuator_type = ActuatorType::UNKNOWN; - + throw std::runtime_error( + std::string("Joint '") + joint.name + "' has an unknown MuJoCo actuator"); + } + last_joint_state.has_pos_pid=extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node); last_joint_state.has_vel_pid=extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node); From 9b02d28b53e5ac5ed1ebb37784dcea0800df573d Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Wed, 12 Nov 2025 15:53:21 +0100 Subject: [PATCH 14/29] Fixed mujoco general actuator --- src/mujoco_system_interface.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 3961890..05b61ee 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1094,17 +1094,17 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn last_joint_state.actuator_type = ActuatorType::POSITION; else if (biastype == mjBIAS_AFFINE && biasprm[1] == 0 && biasprm[2] != 0) last_joint_state.actuator_type = ActuatorType::VELOCITY; - else if (biastype == mjBIAS_USER || gaintype == mjGAIN_USER) + else { last_joint_state.actuator_type = ActuatorType::GENERAL; RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "General MuJoCo actuator for the joint : %s , using effort command interface", joint.name.c_str()); } - else - { - last_joint_state.actuator_type = ActuatorType::UNKNOWN; - throw std::runtime_error( - std::string("Joint '") + joint.name + "' has an unknown MuJoCo actuator"); - } + // else + // { + // last_joint_state.actuator_type = ActuatorType::UNKNOWN; + // throw std::runtime_error( + // std::string("Joint '") + joint.name + "' has an unknown MuJoCo actuator"); + // } last_joint_state.has_pos_pid=extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node); last_joint_state.has_vel_pid=extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node); From bde1912452bf4d8c90948e9fc019e1a4a2cbed9e Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Wed, 12 Nov 2025 16:20:34 +0100 Subject: [PATCH 15/29] Managed custom mujococ actuator --- include/mujoco_ros2_simulation/data.hpp | 3 +-- src/mujoco_system_interface.cpp | 14 ++++---------- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/include/mujoco_ros2_simulation/data.hpp b/include/mujoco_ros2_simulation/data.hpp index 5c5d87b..a2633c9 100644 --- a/include/mujoco_ros2_simulation/data.hpp +++ b/include/mujoco_ros2_simulation/data.hpp @@ -31,11 +31,10 @@ namespace mujoco_ros2_simulation enum class ActuatorType { - UNKNOWN, MOTOR, POSITION, VELOCITY, - GENERAL + CUSTOM }; /** diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 05b61ee..c82298c 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1096,15 +1096,9 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn last_joint_state.actuator_type = ActuatorType::VELOCITY; else { - last_joint_state.actuator_type = ActuatorType::GENERAL; - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "General MuJoCo actuator for the joint : %s , using effort command interface", joint.name.c_str()); + last_joint_state.actuator_type = ActuatorType::CUSTOM; + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Custom MuJoCo actuator for the joint : %s , using all command interfaces", joint.name.c_str()); } - // else - // { - // last_joint_state.actuator_type = ActuatorType::UNKNOWN; - // throw std::runtime_error( - // std::string("Joint '") + joint.name + "' has an unknown MuJoCo actuator"); - // } last_joint_state.has_pos_pid=extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node); last_joint_state.has_vel_pid=extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node); @@ -1134,7 +1128,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn continue; } } - if(last_joint_state.actuator_type == ActuatorType::POSITION) + if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::CUSTOM) { last_joint_state.is_position_control_enabled = true; last_joint_state.position_command = last_joint_state.position; @@ -1158,7 +1152,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn continue; } } - if(last_joint_state.actuator_type == ActuatorType::VELOCITY) + if(last_joint_state.actuator_type == ActuatorType::VELOCITY || last_joint_state.actuator_type == ActuatorType::CUSTOM ) { last_joint_state.is_velocity_control_enabled = true; last_joint_state.velocity_command = last_joint_state.velocity; From 8c7f05f15355abb328ee79dfcad6435e89ed993a Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 13 Nov 2025 09:06:11 +0100 Subject: [PATCH 16/29] Improved logic --- src/mujoco_system_interface.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index c82298c..fad6414 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1005,12 +1005,6 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn mujoco_actuator_id = mujoco_actuator_id == -1 ? mj_name2id(mj_model_, mjtObj::mjOBJ_ACTUATOR, joint.name.c_str()) : mujoco_actuator_id; - if (mujoco_actuator_id == -1) - { - // This isn't a failure the joint just won't be controllable - RCLCPP_WARN_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "No actuator found for joint: " << joint.name); - } - // Add to the joint hw information map joint_hw_info_.insert(std::make_pair(joint.name, joint)); @@ -1079,18 +1073,21 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn last_joint_state.effort = get_initial_value(state_if); } } - // - if (mujoco_actuator_id < 0) + + if (mujoco_actuator_id == -1) + { + // This isn't a failure the joint just won't be controllable + RCLCPP_WARN_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "No actuator found for joint: " << joint.name); continue; + } int biastype = mj_model_->actuator_biastype[mujoco_actuator_id]; - int gaintype = mj_model_->actuator_gaintype[mujoco_actuator_id]; - const int NBias = 10; + const int NBias = 10; const mjtNum* biasprm = mj_model_->actuator_biasprm + mujoco_actuator_id * NBias; if (biastype == mjBIAS_NONE) last_joint_state.actuator_type = ActuatorType::MOTOR; - else if (biastype == mjBIAS_AFFINE && biasprm[1] != 0 && biasprm[2] != 0) + else if (biastype == mjBIAS_AFFINE && biasprm[1] != 0) last_joint_state.actuator_type = ActuatorType::POSITION; else if (biastype == mjBIAS_AFFINE && biasprm[1] == 0 && biasprm[2] != 0) last_joint_state.actuator_type = ActuatorType::VELOCITY; From 9389e582621fd9b91686fa5c4e6a982c1eab2a57 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 13 Nov 2025 09:40:38 +0100 Subject: [PATCH 17/29] added commands for pid control --- src/mujoco_system_interface.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index fad6414..cf3c8a4 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1113,6 +1113,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn if(last_joint_state.has_pos_pid) { last_joint_state.is_position_pid_control_enabled = true; + last_joint_state.position_command = last_joint_state.position; double p, i, d, i_max, i_min; last_joint_state.pos_pid.getGains(p, i, d, i_max, i_min); @@ -1140,7 +1141,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn last_joint_state.is_velocity_pid_control_enabled = true; double p, i, d, i_max, i_min; last_joint_state.vel_pid.getGains(p, i, d, i_max, i_min); - + last_joint_state.velocity_command = last_joint_state.velocity; RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f", joint.name.c_str(), p, i, d, i_max, i_min); } else From 87ff505ec4fbe9c3f75fd5e0ede103eeeffdc873 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 13 Nov 2025 16:25:19 +0100 Subject: [PATCH 18/29] fix if logic --- src/mujoco_system_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index cf3c8a4..d714ada 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1126,7 +1126,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn continue; } } - if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::CUSTOM) + else if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::CUSTOM) { last_joint_state.is_position_control_enabled = true; last_joint_state.position_command = last_joint_state.position; @@ -1150,7 +1150,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn continue; } } - if(last_joint_state.actuator_type == ActuatorType::VELOCITY || last_joint_state.actuator_type == ActuatorType::CUSTOM ) + else if(last_joint_state.actuator_type == ActuatorType::VELOCITY || last_joint_state.actuator_type == ActuatorType::CUSTOM ) { last_joint_state.is_velocity_control_enabled = true; last_joint_state.velocity_command = last_joint_state.velocity; From dba9c01b870ab5d1bc971d5e9b076b64d35f088b Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Mon, 17 Nov 2025 08:56:20 +0100 Subject: [PATCH 19/29] support of pid control for custom actuators --- src/mujoco_system_interface.cpp | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index d714ada..83963a6 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1103,12 +1103,14 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn // command interfaces // overwrite joint limit with min/max value + bool has_command_interfaces = false; for (const auto& command_if : joint.command_interfaces) { + has_command_interfaces = true; // If available, always default to position control at the start if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) { - if(last_joint_state.actuator_type == ActuatorType::VELOCITY || last_joint_state.actuator_type == ActuatorType::MOTOR) + if(last_joint_state.actuator_type == ActuatorType::VELOCITY || last_joint_state.actuator_type == ActuatorType::MOTOR || last_joint_state.actuator_type == ActuatorType::CUSTOM) { if(last_joint_state.has_pos_pid) { @@ -1126,7 +1128,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn continue; } } - else if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::CUSTOM) + else if(last_joint_state.actuator_type == ActuatorType::POSITION) { last_joint_state.is_position_control_enabled = true; last_joint_state.position_command = last_joint_state.position; @@ -1134,7 +1136,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::MOTOR) + if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::MOTOR || last_joint_state.actuator_type == ActuatorType::CUSTOM) { if(last_joint_state.actuator_type == ActuatorType::MOTOR && last_joint_state.has_vel_pid) { @@ -1150,7 +1152,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn continue; } } - else if(last_joint_state.actuator_type == ActuatorType::VELOCITY || last_joint_state.actuator_type == ActuatorType::CUSTOM ) + else if(last_joint_state.actuator_type == ActuatorType::VELOCITY) { last_joint_state.is_velocity_control_enabled = true; last_joint_state.velocity_command = last_joint_state.velocity; @@ -1169,7 +1171,11 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } } } - if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled && !last_joint_state.is_position_pid_control_enabled && !last_joint_state.is_velocity_pid_control_enabled) + if(!has_command_interfaces) + { + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Joint : %s is a passive joint", joint.name.c_str()); + } + else if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled && !last_joint_state.is_position_pid_control_enabled && !last_joint_state.is_velocity_pid_control_enabled) { throw std::runtime_error( std::string("Joint '") + joint.name + "' has an unsupported command interface for the specified MuJoCo actuator"); From b066fd5a546718a6d5b6bd7454c1d7312e58aea7 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Tue, 18 Nov 2025 10:20:27 +0100 Subject: [PATCH 20/29] Improved register joints for the velocity case --- src/mujoco_system_interface.cpp | 41 ++++++++++++++++++--------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 83963a6..f7fe639 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -245,7 +245,7 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr char filename[mj::Simulate::kMaxFilenameLength]; mju::strcpy_arr(filename, file); - // make sure filename is not empty + // load model from path if the filename is not empty if (filename[0]) { // load and compile @@ -302,9 +302,10 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr return mnew; } + //Try to get the mujoco model from topic + std::string robot_description; - // Try to get the mujoco model from topic rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_profile.reliable().transient_local().keep_last(1); // Try to get mujoco_model via topic @@ -1124,7 +1125,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } else { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Position command interface for the joint : %s is not supported with velocity or motor actuator", joint.name.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Position command interface for the joint : %s is not supported with velocity or motor actuator without defining the PIDs", joint.name.c_str()); continue; } } @@ -1136,33 +1137,35 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::MOTOR || last_joint_state.actuator_type == ActuatorType::CUSTOM) + if(last_joint_state.actuator_type == ActuatorType::POSITION) { - if(last_joint_state.actuator_type == ActuatorType::MOTOR && last_joint_state.has_vel_pid) - { - last_joint_state.is_velocity_pid_control_enabled = true; - double p, i, d, i_max, i_min; - last_joint_state.vel_pid.getGains(p, i, d, i_max, i_min); - last_joint_state.velocity_command = last_joint_state.velocity; - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f", joint.name.c_str(), p, i, d, i_max, i_min); - } - else - { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Velocity command interface for the joint : %s is not supported with position or motor actuator", joint.name.c_str()); - continue; - } + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Velocity command interface for the joint : %s is not supported with position actuator", joint.name.c_str()); + continue; } - else if(last_joint_state.actuator_type == ActuatorType::VELOCITY) + if(last_joint_state.actuator_type == ActuatorType::VELOCITY) { last_joint_state.is_velocity_control_enabled = true; last_joint_state.velocity_command = last_joint_state.velocity; } + else if((last_joint_state.actuator_type == ActuatorType::MOTOR || last_joint_state.actuator_type == ActuatorType::CUSTOM) && last_joint_state.has_vel_pid) + { + last_joint_state.is_velocity_pid_control_enabled = true; + double p, i, d, i_max, i_min; + last_joint_state.vel_pid.getGains(p, i, d, i_max, i_min); + last_joint_state.velocity_command = last_joint_state.velocity; + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f", joint.name.c_str(), p, i, d, i_max, i_min); + } + else + { + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Velocity command interface for the joint : %s is not supported with motor or custom actuator without defining the PIDs", joint.name.c_str()); + continue; + } } else if (command_if.name.find(hardware_interface::HW_IF_EFFORT) != std::string::npos) { if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::VELOCITY) { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Effort command interface for the joint : %s is not supported with position or velocity actuator", joint.name.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Effort command interface for the joint : %s is not supported with position or velocity actuator. Skipping it.", joint.name.c_str()); continue; } else{ From 4bd88340288c5ae5e235bdc61e4548d40381eb71 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 18 Nov 2025 18:47:12 +0100 Subject: [PATCH 21/29] Use PidROS instead of PID to reuse elements --- include/mujoco_ros2_simulation/data.hpp | 6 +- package.xml | 1 + src/mujoco_system_interface.cpp | 76 ++++++------------------- 3 files changed, 20 insertions(+), 63 deletions(-) diff --git a/include/mujoco_ros2_simulation/data.hpp b/include/mujoco_ros2_simulation/data.hpp index a2633c9..510d49c 100644 --- a/include/mujoco_ros2_simulation/data.hpp +++ b/include/mujoco_ros2_simulation/data.hpp @@ -19,7 +19,7 @@ #pragma once -#include "control_toolbox/pid.hpp" +#include "control_toolbox/pid_ros.hpp" #include #include @@ -46,8 +46,8 @@ struct JointState double position; double velocity; double effort; - control_toolbox::Pid pos_pid; - control_toolbox::Pid vel_pid; + std::shared_ptr pos_pid; + std::shared_ptr vel_pid; ActuatorType actuator_type; double position_command; double velocity_command; diff --git a/package.xml b/package.xml index 6ad03ab..1d70d7c 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,7 @@ rclcpp rclcpp_lifecycle sensor_msgs + control_toolbox joint_state_broadcaster position_controllers diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index f7fe639..c17fa05 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -187,54 +187,6 @@ const char* Diverged(int disableflags, const mjData* d) return nullptr; } -bool extractPIDFromParameters( - const std::string & control_mode, const std::string & joint_name, - control_toolbox::Pid & pid, rclcpp::Node::SharedPtr node) -{ - const std::string parameter_prefix = "pid_gains." + control_mode + "." + joint_name; - auto get_pid_entry = [node, parameter_prefix](const std::string & entry, double & value) -> bool { - try { - // Check if the parameter is declared, if not, declare the default value NaN - if (!node->has_parameter(parameter_prefix + "." + entry)) { - node->declare_parameter( - parameter_prefix + "." + entry, - std::numeric_limits::quiet_NaN()); - } - value = node->get_parameter(parameter_prefix + "." + entry).as_double(); - } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) { - RCLCPP_ERROR( - node->get_logger(), - "Parameter '%s' not declared, with error %s", entry.c_str(), ex.what()); - return false; - } catch (rclcpp::exceptions::InvalidParameterTypeException & ex) { - RCLCPP_ERROR( - node->get_logger(), - "Parameter '%s' has wrong type: %s", entry.c_str(), ex.what()); - return false; - } - return std::isfinite(value); - }; - bool are_pids_set = true; - double kp, ki, kd, max_integral_error, min_integral_error; - are_pids_set &= get_pid_entry("kp", kp); - are_pids_set &= get_pid_entry("ki", ki); - are_pids_set &= get_pid_entry("kd", kd); - if (are_pids_set) { - get_pid_entry("max_integral_error", max_integral_error); - get_pid_entry("min_integral_error", min_integral_error); - control_toolbox::AntiWindupStrategy antiwindup_strat; - antiwindup_strat.set_type("none"); - antiwindup_strat.i_max = max_integral_error; - antiwindup_strat.i_min = min_integral_error; - pid.initialize( - kp, ki, kd, - std::numeric_limits::infinity(), - -std::numeric_limits::infinity(), antiwindup_strat); - } - - return are_pids_set; -} - mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr node) { mjModel* mnew = 0; @@ -953,7 +905,7 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& { double error = joint_state.position_command - mj_data_->qpos[joint_state.mj_pos_adr]; mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = - joint_state.pos_pid.compute_command(error, period.nanoseconds()); + joint_state.pos_pid->compute_command(error, period); } else if (joint_state.is_velocity_control_enabled) { @@ -963,7 +915,7 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& { double error = joint_state.velocity_command - mj_data_->qvel[joint_state.mj_vel_adr]; mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = - joint_state.vel_pid.compute_command(error, period.nanoseconds()); + joint_state.vel_pid->compute_command(error, period); } else if (joint_state.is_effort_control_enabled) { @@ -1087,20 +1039,26 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn const mjtNum* biasprm = mj_model_->actuator_biasprm + mujoco_actuator_id * NBias; if (biastype == mjBIAS_NONE) + { last_joint_state.actuator_type = ActuatorType::MOTOR; + } else if (biastype == mjBIAS_AFFINE && biasprm[1] != 0) + { last_joint_state.actuator_type = ActuatorType::POSITION; + last_joint_state.pos_pid = std::make_shared(get_node(), "pid_gains.position." + joint.name, ""); + last_joint_state.has_pos_pid = last_joint_state.pos_pid->initialize_from_ros_parameters(); + } else if (biastype == mjBIAS_AFFINE && biasprm[1] == 0 && biasprm[2] != 0) + { last_joint_state.actuator_type = ActuatorType::VELOCITY; + last_joint_state.vel_pid = std::make_shared(get_node(), "pid_gains.velocity." + joint.name, ""); + last_joint_state.has_vel_pid = last_joint_state.vel_pid->initialize_from_ros_parameters(); + } else { last_joint_state.actuator_type = ActuatorType::CUSTOM; RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Custom MuJoCo actuator for the joint : %s , using all command interfaces", joint.name.c_str()); } - - last_joint_state.has_pos_pid=extractPIDFromParameters ("position", joint.name, last_joint_state.pos_pid, node); - last_joint_state.has_vel_pid=extractPIDFromParameters ("velocity", joint.name, last_joint_state.vel_pid, node); - // command interfaces // overwrite joint limit with min/max value @@ -1117,10 +1075,9 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn { last_joint_state.is_position_pid_control_enabled = true; last_joint_state.position_command = last_joint_state.position; - double p, i, d, i_max, i_min; - last_joint_state.pos_pid.getGains(p, i, d, i_max, i_min); + const auto gains = last_joint_state.pos_pid->get_gains(); - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Position control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f", joint.name.c_str(), p, i, d, i_max, i_min); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Position control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.i_max_, gains.i_min_, gains.u_min_, gains.u_max_, gains.antiwindup_strat_.to_string().c_str()); } else @@ -1150,10 +1107,9 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn else if((last_joint_state.actuator_type == ActuatorType::MOTOR || last_joint_state.actuator_type == ActuatorType::CUSTOM) && last_joint_state.has_vel_pid) { last_joint_state.is_velocity_pid_control_enabled = true; - double p, i, d, i_max, i_min; - last_joint_state.vel_pid.getGains(p, i, d, i_max, i_min); + const auto gains = last_joint_state.vel_pid->get_gains(); last_joint_state.velocity_command = last_joint_state.velocity; - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f", joint.name.c_str(), p, i, d, i_max, i_min); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.i_max_, gains.i_min_, gains.u_min_, gains.u_max_, gains.antiwindup_strat_.to_string().c_str()); } else { From 631a6a9a7485d3a3b7ec17d81a1e783756b197a2 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Wed, 19 Nov 2025 17:22:57 +0100 Subject: [PATCH 22/29] added test --- src/mujoco_system_interface.cpp | 11 +- test/config/controllers_pid.yaml | 62 ++++++ test/launch/test_robot_pid.launch.py | 100 +++++++++ test/test_resources/test_pid/scene_pid.xml | 22 ++ .../test_pid/test_robot_pid.urdf | 192 ++++++++++++++++++ .../test_pid/test_robot_pid.xml | 56 +++++ 6 files changed, 439 insertions(+), 4 deletions(-) create mode 100644 test/config/controllers_pid.yaml create mode 100644 test/launch/test_robot_pid.launch.py create mode 100644 test/test_resources/test_pid/scene_pid.xml create mode 100644 test/test_resources/test_pid/test_robot_pid.urdf create mode 100644 test/test_resources/test_pid/test_robot_pid.xml diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index c17fa05..32b4095 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1045,14 +1045,10 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn else if (biastype == mjBIAS_AFFINE && biasprm[1] != 0) { last_joint_state.actuator_type = ActuatorType::POSITION; - last_joint_state.pos_pid = std::make_shared(get_node(), "pid_gains.position." + joint.name, ""); - last_joint_state.has_pos_pid = last_joint_state.pos_pid->initialize_from_ros_parameters(); } else if (biastype == mjBIAS_AFFINE && biasprm[1] == 0 && biasprm[2] != 0) { last_joint_state.actuator_type = ActuatorType::VELOCITY; - last_joint_state.vel_pid = std::make_shared(get_node(), "pid_gains.velocity." + joint.name, ""); - last_joint_state.has_vel_pid = last_joint_state.vel_pid->initialize_from_ros_parameters(); } else { @@ -1060,6 +1056,13 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Custom MuJoCo actuator for the joint : %s , using all command interfaces", joint.name.c_str()); } + last_joint_state.pos_pid = std::make_shared(get_node(), "pid_gains.position." + joint.name, ""); + last_joint_state.has_pos_pid = last_joint_state.pos_pid->initialize_from_ros_parameters(); + + last_joint_state.vel_pid = std::make_shared(get_node(), "pid_gains.velocity." + joint.name, ""); + last_joint_state.has_vel_pid = last_joint_state.vel_pid->initialize_from_ros_parameters(); + + // command interfaces // overwrite joint limit with min/max value bool has_command_interfaces = false; diff --git a/test/config/controllers_pid.yaml b/test/config/controllers_pid.yaml new file mode 100644 index 0000000..9be8723 --- /dev/null +++ b/test/config/controllers_pid.yaml @@ -0,0 +1,62 @@ +controller_manager: + ros__parameters: + update_rate: 50 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + position_controller: + type: position_controllers/JointGroupPositionController + +position_controller: + ros__parameters: + joints: + - joint1 + - joint2 + +mujocosystem: + ros__parameters: + pid_gains: + position: + joint1: + p: 1.0 + i: 0.5 + d: 0.05 + i_clamp_max: 10.0 + i_clamp_min: -10.0 + u_clamp_max: 100.0 + u_clamp_min: -100.0 + tracking_time_constant: 0.02 + error_deadband: 0.0001 + joint2: + p: 1.0 + i: 0.5 + d: 0.05 + i_clamp_max: 10.0 + i_clamp_min: -10.0 + u_clamp_max: 100.0 + u_clamp_min: -100.0 + tracking_time_constant: 0.02 + error_deadband: 0.0001 + pid_gains: + velocity: + joint1: + p: 1.0 + i: 0.5 + d: 0.05 + i_clamp_max: 10.0 + i_clamp_min: -10.0 + u_clamp_max: 100.0 + u_clamp_min: -100.0 + tracking_time_constant: 0.02 + error_deadband: 0.0001 + joint2: + p: 1.0 + i: 0.5 + d: 0.05 + i_clamp_max: 10.0 + i_clamp_min: -10.0 + u_clamp_max: 100.0 + u_clamp_min: -100.0 + tracking_time_constant: 0.02 + error_deadband: 0.0001 diff --git a/test/launch/test_robot_pid.launch.py b/test/launch/test_robot_pid.launch.py new file mode 100644 index 0000000..424899c --- /dev/null +++ b/test/launch/test_robot_pid.launch.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2025, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# This software is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + +from launch import LaunchDescription +from launch.substitutions import ( + Command, + FindExecutable, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue, ParameterFile +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("mujoco_ros2_simulation"), + "test_resources", + "test_pid", + "test_robot_pid.urdf", + ] + ), + ] + ) + + robot_description = {"robot_description": ParameterValue(value=robot_description_content, value_type=str)} + + controller_parameters_pid = ParameterFile( + PathJoinSubstitution([FindPackageShare("mujoco_ros2_simulation"), "config", "controllers_pid.yaml"]), + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[ + robot_description, + {"use_sim_time": True}, + ], + ) + + control_node = Node( + package="mujoco_ros2_simulation", + executable="ros2_control_node", + output="both", + parameters=[ + {"use_sim_time": True}, + controller_parameters_pid, + ], + ) + + spawn_joint_state_broadcaster = Node( + package="controller_manager", + executable="spawner", + name="spawn_joint_state_broadcaster", + arguments=[ + "joint_state_broadcaster", + ], + output="both", + ) + + spawn_position_controller = Node( + package="controller_manager", + executable="spawner", + name="spawn_position_controller", + arguments=[ + "position_controller", + ], + output="both", + ) + + return LaunchDescription( + [ + robot_state_publisher_node, + control_node, + spawn_joint_state_broadcaster, + spawn_position_controller, + ] + ) diff --git a/test/test_resources/test_pid/scene_pid.xml b/test/test_resources/test_pid/scene_pid.xml new file mode 100644 index 0000000..69ea3bd --- /dev/null +++ b/test/test_resources/test_pid/scene_pid.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/test/test_resources/test_pid/test_robot_pid.urdf b/test/test_resources/test_pid/test_robot_pid.urdf new file mode 100644 index 0000000..e50d1f9 --- /dev/null +++ b/test/test_resources/test_pid/test_robot_pid.urdf @@ -0,0 +1,192 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + mujoco_ros2_simulation/MujocoSystemInterface + $(find mujoco_ros2_simulation)/test_resources/test_pid/scene_pid.xml + + + 3.0 + + + 6.0 + + + 10.0 + + + + + + + + 0.0 + + + + + + + + + + + 0.0 + + + + + + + + + camera_color_mujoco_frame + /camera/color/camera_info + /camera/color/image_raw + /camera/aligned_depth_to_color/image_raw + + + + + + + + lidar_sensor_frame + 0.025 + -0.3 + 0.3 + 0.05 + 10 + /scan + + + + diff --git a/test/test_resources/test_pid/test_robot_pid.xml b/test/test_resources/test_pid/test_robot_pid.xml new file mode 100644 index 0000000..2f4e22d --- /dev/null +++ b/test/test_resources/test_pid/test_robot_pid.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From bb54e2eb83e7d2259abf4ca975b709b1890498ec Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 19 Nov 2025 22:05:27 +0100 Subject: [PATCH 23/29] fix the prints --- src/mujoco_system_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 32b4095..a5722e7 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1080,7 +1080,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn last_joint_state.position_command = last_joint_state.position; const auto gains = last_joint_state.pos_pid->get_gains(); - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Position control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.i_max_, gains.i_min_, gains.u_min_, gains.u_max_, gains.antiwindup_strat_.to_string().c_str()); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Position control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.antiwindup_strat_.i_max, gains.antiwindup_strat_.i_min, gains.u_min_, gains.u_max_, gains.antiwindup_strat_.to_string().c_str()); } else @@ -1112,7 +1112,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn last_joint_state.is_velocity_pid_control_enabled = true; const auto gains = last_joint_state.vel_pid->get_gains(); last_joint_state.velocity_command = last_joint_state.velocity; - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.i_max_, gains.i_min_, gains.u_min_, gains.u_max_, gains.antiwindup_strat_.to_string().c_str()); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.antiwindup_strat_.i_max, gains.antiwindup_strat_.i_min, gains.u_min_, gains.u_max_, gains.antiwindup_strat_.to_string().c_str()); } else { From eb41ef5405c8c2defd6a7fa9e10e59df40836b08 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 19 Nov 2025 22:15:44 +0100 Subject: [PATCH 24/29] Cleanup the repo and apply pre-commit formatting changes --- include/mujoco_ros2_simulation/data.hpp | 2 +- src/mujoco_system_interface.cpp | 163 ++++++++++-------- test/config/controllers_pid.yaml | 16 -- test/launch/test_robot_pid.launch.py | 5 +- test/test_resources/test_pid/scene_pid.xml | 2 +- .../test_pid/test_robot_pid.urdf | 2 - .../test_pid/test_robot_pid.xml | 9 +- 7 files changed, 101 insertions(+), 98 deletions(-) diff --git a/include/mujoco_ros2_simulation/data.hpp b/include/mujoco_ros2_simulation/data.hpp index 510d49c..075a3e8 100644 --- a/include/mujoco_ros2_simulation/data.hpp +++ b/include/mujoco_ros2_simulation/data.hpp @@ -19,9 +19,9 @@ #pragma once -#include "control_toolbox/pid_ros.hpp" #include #include +#include "control_toolbox/pid_ros.hpp" #include #include diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index a5722e7..c1f7bf4 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -35,8 +35,8 @@ #include #include -#include #include +#include #include #include @@ -191,8 +191,8 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr { mjModel* mnew = 0; - //Try to get the mujoco model from URDF - + // Try to get the mujoco model from URDF + // this copy is needed so that the mju::strlen call below compiles char filename[mj::Simulate::kMaxFilenameLength]; mju::strcpy_arr(filename, file); @@ -204,7 +204,7 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr char loadError[kErrorLength] = ""; auto load_start = mj::Simulate::Clock::now(); if (mju::strlen_arr(filename) > 4 && !std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb", - mju::sizeof_arr(filename) - mju::strlen_arr(filename) + 4)) + mju::sizeof_arr(filename) - mju::strlen_arr(filename) + 4)) { mnew = mj_loadModel(filename, nullptr); if (!mnew) @@ -253,8 +253,8 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr mju::strcpy_arr(sim.load_error, loadError); return mnew; } - - //Try to get the mujoco model from topic + + // Try to get the mujoco model from topic std::string robot_description; @@ -262,39 +262,42 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr qos_profile.reliable().transient_local().keep_last(1); // Try to get mujoco_model via topic auto mujoco_model_sub = node->create_subscription( - "/mujoco_robot_description", qos_profile, - [&](const std_msgs::msg::String::SharedPtr msg) - { - if (!msg->data.empty() && robot_description.empty()) robot_description = msg->data; - }); + "/mujoco_robot_description", qos_profile, [&](const std_msgs::msg::String::SharedPtr msg) { + if (!msg->data.empty() && robot_description.empty()) + robot_description = msg->data; + }); const auto start = node->now(); const auto timeout = rclcpp::Duration::from_seconds(5.0); auto last_print = start; - while (robot_description.empty() && rclcpp::ok()) { - - if (node->now() - last_print >= rclcpp::Duration::from_seconds(1.0)) { + while (robot_description.empty() && rclcpp::ok()) + { + if (node->now() - last_print >= rclcpp::Duration::from_seconds(1.0)) + { RCLCPP_INFO(node->get_logger(), "Waiting for /mujoco_robot_description..."); last_print = node->now(); } - if (node->now() - start > timeout) { - RCLCPP_WARN(node->get_logger(),"Timeout waiting for robot_description_mujoco topic."); + if (node->now() - start > timeout) + { + RCLCPP_WARN(node->get_logger(), "Timeout waiting for robot_description_mujoco topic."); break; } rclcpp::sleep_for(std::chrono::milliseconds(200)); } - if (!robot_description.empty()){ + if (!robot_description.empty()) + { // Load Mujoco model char error[1000] = "Could not load XML model"; - mjSpec * spec= nullptr; + mjSpec* spec = nullptr; spec = mj_parseXMLString(robot_description.c_str(), nullptr, error, 1000); mnew = mj_compile(spec, nullptr); - if (!mnew){ - const char* myerr= mjs_getError(spec); + if (!mnew) + { + const char* myerr = mjs_getError(spec); mj_deleteSpec(spec); RCLCPP_INFO(node->get_logger(), "Error %s", myerr); RCLCPP_FATAL(node->get_logger(), "Failed to compile MuJoCo model: %s", error); @@ -381,10 +384,7 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf const auto model_path_maybe = get_parameter("mujoco_model"); if (!model_path_maybe.has_value()) { - RCLCPP_INFO( - rclcpp::get_logger("MujocoSystemInterface"), - "Parameter 'mujoco_model' not found in URDF." - ); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Parameter 'mujoco_model' not found in URDF."); model_path_.clear(); } else @@ -392,7 +392,6 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf model_path_ = model_path_maybe.value(); RCLCPP_INFO_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "Loading 'mujoco_model' from: " << model_path_); } - // Pull the initial speed factor from the hardware parameters, if present sim_speed_factor_ = std::stod(get_parameter("sim_speed_factor").value_or("-1")); @@ -455,7 +454,7 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf // Load the model and data prior to hw registration and starting the physics thread sim_->LoadMessage(model_path_.c_str()); auto node = get_node(); - mj_model_ = LoadModel(model_path_.c_str(), *sim_, node); + mj_model_ = LoadModel(model_path_.c_str(), *sim_, node); if (!mj_model_) { RCLCPP_FATAL(rclcpp::get_logger("MujocoSystemInterface"), "Mujoco failed to load '%s'", model_path_.c_str()); @@ -678,17 +677,17 @@ std::vector MujocoSystemInterface::export_ { if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) { - if(joint.is_position_control_enabled || joint.is_position_pid_control_enabled) + if (joint.is_position_control_enabled || joint.is_position_pid_control_enabled) new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, &joint.position_command); } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - if(joint.is_velocity_control_enabled || joint.is_velocity_pid_control_enabled) + if (joint.is_velocity_control_enabled || joint.is_velocity_pid_control_enabled) new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity_command); } else if (command_if.name == hardware_interface::HW_IF_EFFORT) { - if(joint.is_effort_control_enabled) + if (joint.is_effort_control_enabled) new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effort_command); } } @@ -760,7 +759,7 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoractuator_type == ActuatorType::POSITION) + if (joint_it->actuator_type == ActuatorType::POSITION) { joint_it->is_position_control_enabled = true; } @@ -768,16 +767,16 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_position_pid_control_enabled = true; } - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), - "Joint %s: position control enabled (velocity, effort disabled)", joint_name.c_str()); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), + "Joint %s: position control enabled (velocity, effort disabled)", joint_name.c_str()); } else if (interface_type == hardware_interface::HW_IF_VELOCITY) { - if(joint_it->actuator_type == ActuatorType::VELOCITY) + if (joint_it->actuator_type == ActuatorType::VELOCITY) { joint_it->is_velocity_control_enabled = true; } - else if(joint_it->has_vel_pid) + else if (joint_it->has_vel_pid) { joint_it->is_velocity_pid_control_enabled = true; } @@ -871,7 +870,7 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& } hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& /*time*/, - const rclcpp::Duration &period) + const rclcpp::Duration& period) { // Update mimic joints for (auto& joint_state : joint_states_) @@ -901,21 +900,19 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& { mj_data_control_->ctrl[joint_state.mj_actuator_id] = joint_state.position_command; } - else if(joint_state.is_position_pid_control_enabled) + else if (joint_state.is_position_pid_control_enabled) { double error = joint_state.position_command - mj_data_->qpos[joint_state.mj_pos_adr]; - mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = - joint_state.pos_pid->compute_command(error, period); + mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = joint_state.pos_pid->compute_command(error, period); } else if (joint_state.is_velocity_control_enabled) { mj_data_control_->ctrl[joint_state.mj_actuator_id] = joint_state.velocity_command; } - else if(joint_state.is_velocity_pid_control_enabled) + else if (joint_state.is_velocity_pid_control_enabled) { double error = joint_state.velocity_command - mj_data_->qvel[joint_state.mj_vel_adr]; - mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = - joint_state.vel_pid->compute_command(error, period); + mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = joint_state.vel_pid->compute_command(error, period); } else if (joint_state.is_effort_control_enabled) { @@ -1035,7 +1032,7 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } int biastype = mj_model_->actuator_biastype[mujoco_actuator_id]; - const int NBias = 10; + const int NBias = 10; const mjtNum* biasprm = mj_model_->actuator_biasprm + mujoco_actuator_id * NBias; if (biastype == mjBIAS_NONE) @@ -1050,19 +1047,21 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn { last_joint_state.actuator_type = ActuatorType::VELOCITY; } - else + else { last_joint_state.actuator_type = ActuatorType::CUSTOM; - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Custom MuJoCo actuator for the joint : %s , using all command interfaces", joint.name.c_str()); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), + "Custom MuJoCo actuator for the joint : %s , using all command interfaces", joint.name.c_str()); } - last_joint_state.pos_pid = std::make_shared(get_node(), "pid_gains.position." + joint.name, ""); + last_joint_state.pos_pid = + std::make_shared(get_node(), "pid_gains.position." + joint.name, ""); last_joint_state.has_pos_pid = last_joint_state.pos_pid->initialize_from_ros_parameters(); - last_joint_state.vel_pid = std::make_shared(get_node(), "pid_gains.velocity." + joint.name, ""); + last_joint_state.vel_pid = + std::make_shared(get_node(), "pid_gains.velocity." + joint.name, ""); last_joint_state.has_vel_pid = last_joint_state.vel_pid->initialize_from_ros_parameters(); - // command interfaces // overwrite joint limit with min/max value bool has_command_interfaces = false; @@ -1072,24 +1071,33 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn // If available, always default to position control at the start if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) { - if(last_joint_state.actuator_type == ActuatorType::VELOCITY || last_joint_state.actuator_type == ActuatorType::MOTOR || last_joint_state.actuator_type == ActuatorType::CUSTOM) + if (last_joint_state.actuator_type == ActuatorType::VELOCITY || + last_joint_state.actuator_type == ActuatorType::MOTOR || + last_joint_state.actuator_type == ActuatorType::CUSTOM) { - if(last_joint_state.has_pos_pid) + if (last_joint_state.has_pos_pid) { last_joint_state.is_position_pid_control_enabled = true; last_joint_state.position_command = last_joint_state.position; const auto gains = last_joint_state.pos_pid->get_gains(); - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Position control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.antiwindup_strat_.i_max, gains.antiwindup_strat_.i_min, gains.u_min_, gains.u_max_, gains.antiwindup_strat_.to_string().c_str()); - + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), + "Position control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, " + "Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", + joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.antiwindup_strat_.i_max, + gains.antiwindup_strat_.i_min, gains.u_min_, gains.u_max_, + gains.antiwindup_strat_.to_string().c_str()); } else { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Position command interface for the joint : %s is not supported with velocity or motor actuator without defining the PIDs", joint.name.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), + "Position command interface for the joint : %s is not supported with velocity or motor " + "actuator without defining the PIDs", + joint.name.c_str()); continue; } } - else if(last_joint_state.actuator_type == ActuatorType::POSITION) + else if (last_joint_state.actuator_type == ActuatorType::POSITION) { last_joint_state.is_position_control_enabled = true; last_joint_state.position_command = last_joint_state.position; @@ -1097,50 +1105,69 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) { - if(last_joint_state.actuator_type == ActuatorType::POSITION) + if (last_joint_state.actuator_type == ActuatorType::POSITION) { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Velocity command interface for the joint : %s is not supported with position actuator", joint.name.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), + "Velocity command interface for the joint : %s is not supported with position actuator", + joint.name.c_str()); continue; } - if(last_joint_state.actuator_type == ActuatorType::VELOCITY) + if (last_joint_state.actuator_type == ActuatorType::VELOCITY) { last_joint_state.is_velocity_control_enabled = true; last_joint_state.velocity_command = last_joint_state.velocity; } - else if((last_joint_state.actuator_type == ActuatorType::MOTOR || last_joint_state.actuator_type == ActuatorType::CUSTOM) && last_joint_state.has_vel_pid) + else if ((last_joint_state.actuator_type == ActuatorType::MOTOR || + last_joint_state.actuator_type == ActuatorType::CUSTOM) && + last_joint_state.has_vel_pid) { last_joint_state.is_velocity_pid_control_enabled = true; const auto gains = last_joint_state.vel_pid->get_gains(); last_joint_state.velocity_command = last_joint_state.velocity; - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.antiwindup_strat_.i_max, gains.antiwindup_strat_.i_min, gains.u_min_, gains.u_max_, gains.antiwindup_strat_.to_string().c_str()); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), + "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, " + "Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s", + joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.antiwindup_strat_.i_max, + gains.antiwindup_strat_.i_min, gains.u_min_, gains.u_max_, + gains.antiwindup_strat_.to_string().c_str()); } - else + else { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Velocity command interface for the joint : %s is not supported with motor or custom actuator without defining the PIDs", joint.name.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), + "Velocity command interface for the joint : %s is not supported with motor or custom actuator " + "without defining the PIDs", + joint.name.c_str()); continue; } } else if (command_if.name.find(hardware_interface::HW_IF_EFFORT) != std::string::npos) { - if(last_joint_state.actuator_type == ActuatorType::POSITION || last_joint_state.actuator_type == ActuatorType::VELOCITY) + if (last_joint_state.actuator_type == ActuatorType::POSITION || + last_joint_state.actuator_type == ActuatorType::VELOCITY) { - RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), "Effort command interface for the joint : %s is not supported with position or velocity actuator. Skipping it.", joint.name.c_str()); + RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"), + "Effort command interface for the joint : %s is not supported with position or velocity " + "actuator. Skipping it.", + joint.name.c_str()); continue; } - else{ + else + { last_joint_state.is_effort_control_enabled = true; last_joint_state.effort_command = last_joint_state.effort; } } } - if(!has_command_interfaces) + if (!has_command_interfaces) { - RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Joint : %s is a passive joint", joint.name.c_str()); + RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Joint : %s is a passive joint", joint.name.c_str()); } - else if(!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && !last_joint_state.is_effort_control_enabled && !last_joint_state.is_position_pid_control_enabled && !last_joint_state.is_velocity_pid_control_enabled) + else if (!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled && + !last_joint_state.is_effort_control_enabled && !last_joint_state.is_position_pid_control_enabled && + !last_joint_state.is_velocity_pid_control_enabled) { - throw std::runtime_error( - std::string("Joint '") + joint.name + "' has an unsupported command interface for the specified MuJoCo actuator"); + throw std::runtime_error(std::string("Joint '") + joint.name + + "' has an unsupported command interface for the specified MuJoCo actuator"); } } } diff --git a/test/config/controllers_pid.yaml b/test/config/controllers_pid.yaml index 9be8723..02292b9 100644 --- a/test/config/controllers_pid.yaml +++ b/test/config/controllers_pid.yaml @@ -22,41 +22,25 @@ mujocosystem: p: 1.0 i: 0.5 d: 0.05 - i_clamp_max: 10.0 - i_clamp_min: -10.0 u_clamp_max: 100.0 u_clamp_min: -100.0 - tracking_time_constant: 0.02 - error_deadband: 0.0001 joint2: p: 1.0 i: 0.5 d: 0.05 - i_clamp_max: 10.0 - i_clamp_min: -10.0 u_clamp_max: 100.0 u_clamp_min: -100.0 - tracking_time_constant: 0.02 - error_deadband: 0.0001 pid_gains: velocity: joint1: p: 1.0 i: 0.5 d: 0.05 - i_clamp_max: 10.0 - i_clamp_min: -10.0 u_clamp_max: 100.0 u_clamp_min: -100.0 - tracking_time_constant: 0.02 - error_deadband: 0.0001 joint2: p: 1.0 i: 0.5 d: 0.05 - i_clamp_max: 10.0 - i_clamp_min: -10.0 u_clamp_max: 100.0 u_clamp_min: -100.0 - tracking_time_constant: 0.02 - error_deadband: 0.0001 diff --git a/test/launch/test_robot_pid.launch.py b/test/launch/test_robot_pid.launch.py index 424899c..6b0ca2b 100644 --- a/test/launch/test_robot_pid.launch.py +++ b/test/launch/test_robot_pid.launch.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 # -# Copyright (c) 2025, United States Government, as represented by the -# Administrator of the National Aeronautics and Space Administration. +# Copyright (c) 2025, PAL Robotics S.L.. # # All rights reserved. # @@ -48,7 +47,7 @@ def generate_launch_description(): controller_parameters_pid = ParameterFile( PathJoinSubstitution([FindPackageShare("mujoco_ros2_simulation"), "config", "controllers_pid.yaml"]), - ) + ) robot_state_publisher_node = Node( package="robot_state_publisher", diff --git a/test/test_resources/test_pid/scene_pid.xml b/test/test_resources/test_pid/scene_pid.xml index 69ea3bd..2f62145 100644 --- a/test/test_resources/test_pid/scene_pid.xml +++ b/test/test_resources/test_pid/scene_pid.xml @@ -1,5 +1,5 @@ - + diff --git a/test/test_resources/test_pid/test_robot_pid.urdf b/test/test_resources/test_pid/test_robot_pid.urdf index e50d1f9..53e8805 100644 --- a/test/test_resources/test_pid/test_robot_pid.urdf +++ b/test/test_resources/test_pid/test_robot_pid.urdf @@ -146,7 +146,6 @@ - 0.0 @@ -157,7 +156,6 @@ - 0.0 diff --git a/test/test_resources/test_pid/test_robot_pid.xml b/test/test_resources/test_pid/test_robot_pid.xml index 2f4e22d..52c9063 100644 --- a/test/test_resources/test_pid/test_robot_pid.xml +++ b/test/test_resources/test_pid/test_robot_pid.xml @@ -42,13 +42,8 @@ - - - - - + + From ba83b32b03f3d25458cb3c1175fcc64f1a1df11d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 19 Nov 2025 23:18:24 +0100 Subject: [PATCH 25/29] Update to use the pids_config_file argument --- src/mujoco_system_interface.cpp | 46 ++++++++++++++----- .../{controllers_pid.yaml => mujoco_pid.yaml} | 18 +------- test/launch/test_robot_pid.launch.py | 3 +- .../test_pid/test_robot_pid.urdf | 2 + 4 files changed, 40 insertions(+), 29 deletions(-) rename test/config/{controllers_pid.yaml => mujoco_pid.yaml} (63%) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index c1f7bf4..b5838a6 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -453,8 +454,38 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf // Load the model and data prior to hw registration and starting the physics thread sim_->LoadMessage(model_path_.c_str()); - auto node = get_node(); - mj_model_ = LoadModel(model_path_.c_str(), *sim_, node); + + // Construct and start the ROS node spinning + /// The PIDs config file + const auto pids_config_file = get_parameter("pids_config_file"); + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("use_sim_time", rclcpp::ParameterValue(true)); + node_options.allow_undeclared_parameters(true); + node_options.automatically_declare_parameters_from_overrides(true); + if (pids_config_file.has_value()) + { + // Check if the file exists + const std::filesystem::path path_to_file(pids_config_file.value()); + if (!std::filesystem::exists(path_to_file)) + { + RCLCPP_FATAL(rclcpp::get_logger("MujocoSystemInterface"), "PID config file '%s' does not exist!", + pids_config_file->c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + RCLCPP_INFO_STREAM(rclcpp::get_logger("MujocoSystemInterface"), + "Loading PID config from file: " << pids_config_file.value()); + auto node_options_arguments = node_options.arguments(); + node_options_arguments.push_back(RCL_ROS_ARGS_FLAG); + node_options_arguments.push_back(RCL_PARAM_FILE_FLAG); + node_options_arguments.push_back(pids_config_file.value()); + node_options.arguments(node_options_arguments); + } + executor_ = std::make_unique(); + mujoco_node_ = std::make_shared("mujoco_node", node_options); + executor_->add_node(mujoco_node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + + mj_model_ = LoadModel(model_path_.c_str(), *sim_, mujoco_node_); if (!mj_model_) { RCLCPP_FATAL(rclcpp::get_logger("MujocoSystemInterface"), "Mujoco failed to load '%s'", model_path_.c_str()); @@ -477,12 +508,6 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf register_sensors(get_hardware_info()); set_initial_pose(); - // Construct and start the ROS node spinning - executor_ = std::make_unique(); - mujoco_node_ = std::make_shared("mujoco_node"); - executor_->add_node(mujoco_node_); - executor_thread_ = std::thread([this]() { executor_->spin(); }); - // Time publisher will be pushed from the physics_thread_ clock_publisher_ = mujoco_node_->create_publisher("/clock", 1); @@ -926,7 +951,6 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& void MujocoSystemInterface::register_joints(const hardware_interface::HardwareInfo& hardware_info) { joint_states_.resize(hardware_info.joints.size()); - auto node = get_node(); for (size_t joint_index = 0; joint_index < hardware_info.joints.size(); joint_index++) { @@ -1055,11 +1079,11 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } last_joint_state.pos_pid = - std::make_shared(get_node(), "pid_gains.position." + joint.name, ""); + std::make_shared(mujoco_node_, "pid_gains.position." + joint.name, ""); last_joint_state.has_pos_pid = last_joint_state.pos_pid->initialize_from_ros_parameters(); last_joint_state.vel_pid = - std::make_shared(get_node(), "pid_gains.velocity." + joint.name, ""); + std::make_shared(mujoco_node_, "pid_gains.velocity." + joint.name, ""); last_joint_state.has_vel_pid = last_joint_state.vel_pid->initialize_from_ros_parameters(); // command interfaces diff --git a/test/config/controllers_pid.yaml b/test/config/mujoco_pid.yaml similarity index 63% rename from test/config/controllers_pid.yaml rename to test/config/mujoco_pid.yaml index 02292b9..d89a8c4 100644 --- a/test/config/controllers_pid.yaml +++ b/test/config/mujoco_pid.yaml @@ -1,20 +1,4 @@ -controller_manager: - ros__parameters: - update_rate: 50 - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - position_controller: - type: position_controllers/JointGroupPositionController - -position_controller: - ros__parameters: - joints: - - joint1 - - joint2 - -mujocosystem: +/**: ros__parameters: pid_gains: position: diff --git a/test/launch/test_robot_pid.launch.py b/test/launch/test_robot_pid.launch.py index 6b0ca2b..42398b2 100644 --- a/test/launch/test_robot_pid.launch.py +++ b/test/launch/test_robot_pid.launch.py @@ -46,7 +46,7 @@ def generate_launch_description(): robot_description = {"robot_description": ParameterValue(value=robot_description_content, value_type=str)} controller_parameters_pid = ParameterFile( - PathJoinSubstitution([FindPackageShare("mujoco_ros2_simulation"), "config", "controllers_pid.yaml"]), + PathJoinSubstitution([FindPackageShare("mujoco_ros2_simulation"), "config", "controllers.yaml"]), ) robot_state_publisher_node = Node( @@ -67,6 +67,7 @@ def generate_launch_description(): {"use_sim_time": True}, controller_parameters_pid, ], + emulate_tty=True, ) spawn_joint_state_broadcaster = Node( diff --git a/test/test_resources/test_pid/test_robot_pid.urdf b/test/test_resources/test_pid/test_robot_pid.urdf index 53e8805..be78798 100644 --- a/test/test_resources/test_pid/test_robot_pid.urdf +++ b/test/test_resources/test_pid/test_robot_pid.urdf @@ -133,6 +133,8 @@ mujoco_ros2_simulation/MujocoSystemInterface $(find mujoco_ros2_simulation)/test_resources/test_pid/scene_pid.xml + $(find mujoco_ros2_simulation)/config/mujoco_pid.yaml + 3.0 From b49b65f47bbffd90c048f9879b4127a3df1ed8f5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 19 Nov 2025 23:22:54 +0100 Subject: [PATCH 26/29] update readme --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index 151a1b0..354a691 100644 --- a/README.md +++ b/README.md @@ -51,6 +51,11 @@ Just specify the plugin and point to a valid MJCF on launch: mujoco_ros2_simulation/MujocoSystemInterface $(find my_description)/description/scene.xml + + $(find my_description)/config/pids.yaml +