Skip to content

Support torque and force command and state interfaces #29

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
saikishor merged 7 commits into from
Dec 15, 2025
Merged

Support torque and force command and state interfaces #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
d41f106
Support torque and force command and state interfaces
saikishor Dec 10, 2025
37caaf1
Merge branch 'main' into support_torque_force_interfaces
saikishor Dec 15, 2025
b04ab88
add pre-commit changes
saikishor Dec 15, 2025
099db09
Merge branch 'main' into support_torque_force_interfaces
saikishor Dec 15, 2025
011ffe0
udpate README
saikishor Dec 15, 2025
5222b50
Update test/test_resources/test_robot.urdf
saikishor Dec 15, 2025
61ecfd2
Update test/test_resources/test_robot.urdf
saikishor Dec 15, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -195,6 +195,8 @@ Could map to the following hardware interface:
</tbody>
</table>

> [!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).

Expand Down
27 changes: 18 additions & 9 deletions src/mujoco_system_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -767,9 +767,10 @@ std::vector<hardware_interface::StateInterface> 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);
}
}
}
Expand Down Expand Up @@ -915,10 +916,12 @@ std::vector<hardware_interface::CommandInterface> 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);
}
}
}
Expand Down Expand Up @@ -1007,10 +1010,12 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vector<std::string
}
RCLCPP_INFO(get_logger(), "Joint %s: velocity control enabled (position, effort disabled)", joint_name.c_str());
}
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 = true;
RCLCPP_INFO(get_logger(), "Joint %s: effort 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());
}
}
else
Expand All @@ -1025,7 +1030,8 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vector<std::string
joint_it->is_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;
}
Expand Down Expand Up @@ -1272,7 +1278,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);
Expand Down Expand Up @@ -1392,7 +1399,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.
Expand Down
6 changes: 6 additions & 0 deletions test/test_resources/test_robot.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,7 +167,10 @@
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>

<!-- Effort and torque are semantically different but map to the same underlying data. -->
<state_interface name="effort"/>
<state_interface name="torque"/>
</joint>
<joint name="joint2">
<command_interface name="position"/>
Expand All @@ -178,7 +181,10 @@
<param name="initial_value">0.0</param>
</state_interface>
<state_interface name="velocity"/>

<!-- Effort and torque are semantically different but map to the same underlying data. -->
<state_interface name="effort"/>
<state_interface name="torque"/>
</joint>

<!-- For cameras, the sensor name _must_ match the camera name in the MJCF -->
Expand Down
Loading