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). diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index f1797ae..773296b 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -767,9 +767,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); } } } @@ -915,10 +916,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); } } } @@ -1007,10 +1010,12 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vectoris_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 @@ -1025,7 +1030,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; } @@ -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); @@ -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. diff --git a/test/test_resources/test_robot.urdf b/test/test_resources/test_robot.urdf index abee6a0..ccc9f89 100644 --- a/test/test_resources/test_robot.urdf +++ b/test/test_resources/test_robot.urdf @@ -167,7 +167,10 @@ 0.0 + + + @@ -178,7 +181,10 @@ 0.0 + + +