From d41f10665044dca87016bc4d2faab66d7563a651 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Dec 2025 00:31:34 +0100 Subject: [PATCH 1/5] Support torque and force command and state interfaces --- src/mujoco_system_interface.cpp | 27 ++++++++++++++++++--------- test/test_resources/test_robot.urdf | 2 ++ 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index cedf0df..57c9298 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -769,9 +769,10 @@ std::vector MujocoSystemInterface::export_st { new_state_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity); } - else if (state_if.name == hardware_interface::HW_IF_EFFORT) + else if (state_if.name == hardware_interface::HW_IF_EFFORT || + state_if.name == hardware_interface::HW_IF_TORQUE || state_if.name == hardware_interface::HW_IF_FORCE) { - new_state_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effort); + new_state_interfaces.emplace_back(joint.name, state_if.name, &joint.effort); } } } @@ -917,10 +918,12 @@ std::vector MujocoSystemInterface::export_ 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) + else if (command_if.name == hardware_interface::HW_IF_EFFORT || + command_if.name == hardware_interface::HW_IF_TORQUE || + command_if.name == hardware_interface::HW_IF_FORCE) { if (joint.is_effort_control_enabled) - new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effort_command); + new_command_interfaces.emplace_back(joint.name, command_if.name, &joint.effort_command); } } } @@ -1015,11 +1018,13 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_effort_control_enabled = true; RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), - "Joint %s: effort control enabled (position, velocity disabled)", joint_name.c_str()); + "Joint %s: %s control enabled (position, velocity disabled)", joint_name.c_str(), + interface_type.c_str()); } } else @@ -1034,7 +1039,8 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_velocity_control_enabled = false; joint_it->is_velocity_pid_control_enabled = false; } - else if (interface_type == hardware_interface::HW_IF_EFFORT) + else if (interface_type == hardware_interface::HW_IF_EFFORT || + interface_type == hardware_interface::HW_IF_TORQUE || interface_type == hardware_interface::HW_IF_FORCE) { joint_it->is_effort_control_enabled = false; } @@ -1284,7 +1290,8 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn last_joint_state.velocity = should_override_start_position ? mj_data_->qvel[joint_state.mj_vel_adr] : get_initial_value(state_if); } - else if (state_if.name == hardware_interface::HW_IF_EFFORT) + else if (state_if.name == hardware_interface::HW_IF_EFFORT || state_if.name == hardware_interface::HW_IF_TORQUE || + state_if.name == hardware_interface::HW_IF_FORCE) { // We never set data for effort from an initial conditions file, so just default to the initial value if it exists. last_joint_state.effort = get_initial_value(state_if); @@ -1407,7 +1414,9 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn } } } - else if (command_if.name.find(hardware_interface::HW_IF_EFFORT) != std::string::npos) + else if (command_if.name.find(hardware_interface::HW_IF_EFFORT) != std::string::npos || + command_if.name.find(hardware_interface::HW_IF_TORQUE) != std::string::npos || + command_if.name.find(hardware_interface::HW_IF_FORCE) != std::string::npos) { // Effort command interface: // Direct control for effort actuators; not supported for position or velocity actuators. diff --git a/test/test_resources/test_robot.urdf b/test/test_resources/test_robot.urdf index abee6a0..7e62f2a 100644 --- a/test/test_resources/test_robot.urdf +++ b/test/test_resources/test_robot.urdf @@ -168,6 +168,7 @@ + @@ -179,6 +180,7 @@ + From b04ab886882c13d3254b8d4a18fe3c15c578c2c4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Dec 2025 18:55:39 +0100 Subject: [PATCH 2/5] add pre-commit changes --- src/mujoco_system_interface.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index c4d2c0b..9fa2849 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -1012,8 +1012,7 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_effort_control_enabled = true; - RCLCPP_INFO(get_logger(), - "Joint %s: %s control enabled (position, velocity disabled)", joint_name.c_str(), + RCLCPP_INFO(get_logger(), "Joint %s: %s control enabled (position, velocity disabled)", joint_name.c_str(), interface_type.c_str()); } } From 011ffe0e339d05c50b6c71449e07d99d0f5ff12a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Dec 2025 19:39:36 +0100 Subject: [PATCH 3/5] udpate README --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 8bbe69e..d39d2fa 100644 --- a/README.md +++ b/README.md @@ -195,6 +195,8 @@ Could map to the following hardware interface: +> [!NOTE] +> The `torque` and `force` command/state interfaces are semantically equivalent to `effort`, and map to the same underlying data in the sim. Switching actuator/control types on the fly is an [open issue](#13). From 5222b5082facfd7fb3857ab612f17073a6c30774 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Dec 2025 20:14:24 +0100 Subject: [PATCH 4/5] Update test/test_resources/test_robot.urdf Co-authored-by: Erik Holum --- test/test_resources/test_robot.urdf | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/test_resources/test_robot.urdf b/test/test_resources/test_robot.urdf index 7e62f2a..3a6e345 100644 --- a/test/test_resources/test_robot.urdf +++ b/test/test_resources/test_robot.urdf @@ -167,6 +167,8 @@ 0.0 + + From 61ecfd28c4b645ba8e0019ea7c5b6eca7b71ff64 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Dec 2025 20:14:33 +0100 Subject: [PATCH 5/5] Update test/test_resources/test_robot.urdf Co-authored-by: Erik Holum --- test/test_resources/test_robot.urdf | 2 ++ 1 file changed, 2 insertions(+) diff --git a/test/test_resources/test_robot.urdf b/test/test_resources/test_robot.urdf index 3a6e345..ccc9f89 100644 --- a/test/test_resources/test_robot.urdf +++ b/test/test_resources/test_robot.urdf @@ -181,6 +181,8 @@ 0.0 + +