Skip to content

Fix moprim controller #3

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
merged 3 commits into from
Nov 4, 2025
Merged

Fix moprim controller #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3c86dd6
Fix EOL in ur_controllers.yaml
urfeex Nov 3, 2025
29fb9e9
Remove logging statements from perform_switch method
urfeex Nov 3, 2025
bf586b4
Add cross-controller compatibility checks
urfeex Nov 3, 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
1 change: 0 additions & 1 deletion ur_robot_driver/config/ur_controllers.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -214,4 +214,3 @@ tool_contact_controller:
motion_primitive_forward_controller:
ros__parameters:
tf_prefix: "$(var tf_prefix)"

19 changes: 16 additions & 3 deletions ur_robot_driver/src/hardware_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,48 +68,64 @@ URPositionHardwareInterface::URPositionHardwareInterface()
mode_compatibility_[hardware_interface::HW_IF_POSITION][PASSTHROUGH_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_POSITION][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_POSITION][TOOL_CONTACT_GPIO] = true;
mode_compatibility_[hardware_interface::HW_IF_POSITION][HW_IF_MOTION_PRIMITIVES] = false;

mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_EFFORT] = false;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FORCE_MODE_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][PASSTHROUGH_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TOOL_CONTACT_GPIO] = true;
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][HW_IF_MOTION_PRIMITIVES] = false;

mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_VELOCITY] = false;
mode_compatibility_[hardware_interface::HW_IF_EFFORT][FORCE_MODE_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_EFFORT][PASSTHROUGH_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_EFFORT][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[hardware_interface::HW_IF_EFFORT][TOOL_CONTACT_GPIO] = true;
mode_compatibility_[hardware_interface::HW_IF_EFFORT][HW_IF_MOTION_PRIMITIVES] = false;

mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_EFFORT] = false;
mode_compatibility_[FORCE_MODE_GPIO][PASSTHROUGH_GPIO] = true;
mode_compatibility_[FORCE_MODE_GPIO][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[FORCE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
mode_compatibility_[FORCE_MODE_GPIO][HW_IF_MOTION_PRIMITIVES] = true;

mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_EFFORT] = false;
mode_compatibility_[PASSTHROUGH_GPIO][FORCE_MODE_GPIO] = true;
mode_compatibility_[PASSTHROUGH_GPIO][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[PASSTHROUGH_GPIO][TOOL_CONTACT_GPIO] = true;
mode_compatibility_[PASSTHROUGH_GPIO][HW_IF_MOTION_PRIMITIVES] = false;

mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_EFFORT] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][FORCE_MODE_GPIO] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][PASSTHROUGH_GPIO] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
mode_compatibility_[FREEDRIVE_MODE_GPIO][HW_IF_MOTION_PRIMITIVES] = false;

mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true;
mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true;
mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_EFFORT] = true;
mode_compatibility_[TOOL_CONTACT_GPIO][FORCE_MODE_GPIO] = false;
mode_compatibility_[TOOL_CONTACT_GPIO][PASSTHROUGH_GPIO] = true;
mode_compatibility_[TOOL_CONTACT_GPIO][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[TOOL_CONTACT_GPIO][HW_IF_MOTION_PRIMITIVES] = true;

mode_compatibility_[HW_IF_MOTION_PRIMITIVES][hardware_interface::HW_IF_POSITION] = false;
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][hardware_interface::HW_IF_VELOCITY] = false;
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][hardware_interface::HW_IF_EFFORT] = false;
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][FORCE_MODE_GPIO] = true;
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][PASSTHROUGH_GPIO] = false;
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][FREEDRIVE_MODE_GPIO] = false;
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][TOOL_CONTACT_GPIO] = true;
}

URPositionHardwareInterface::~URPositionHardwareInterface()
Expand Down Expand Up @@ -1398,7 +1414,6 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
}
if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(),
StoppingInterface::STOP_PASSTHROUGH) != stop_modes_[0].end()) {
RCLCPP_WARN(get_logger(), "Stopping passthrough trajectory controller.");
passthrough_trajectory_controller_running_ = false;
passthrough_trajectory_abort_ = 1.0;
trajectory_joint_positions_.clear();
Expand All @@ -1417,8 +1432,6 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
resetMoprimCmdInterfaces();
current_moprim_execution_status_ = MoprimExecutionState::IDLE;
ready_for_new_moprim_ = false;

RCLCPP_INFO(get_logger(), "Motion primitives mode stopped.");
}
if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(),
StoppingInterface::STOP_TOOL_CONTACT) != stop_modes_[0].end()) {
Expand Down
107 changes: 107 additions & 0 deletions ur_robot_driver/test/integration_test_controller_switch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@
"passthrough_trajectory_controller",
"force_mode_controller",
"freedrive_mode_controller",
"motion_primitive_forward_controller",
]


Expand Down Expand Up @@ -483,3 +484,109 @@ def test_tool_contact_compatibility(self):
],
).ok
)

self.assertTrue(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
activate_controllers=[
"motion_primitive_forward_controller",
"tool_contact_controller",
],
).ok
)
self.assertTrue(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
deactivate_controllers=[
"motion_primitive_forward_controller",
"tool_contact_controller",
],
).ok
)

def test_moprim_compatibility(self):
# Deactivate all writing controllers
self.assertTrue(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.BEST_EFFORT,
deactivate_controllers=ALL_CONTROLLERS,
).ok
)

time.sleep(3)

# moprim controller should not start with any other joint controller
self.assertFalse(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
activate_controllers=[
"motion_primitive_forward_controller",
"joint_trajectory_controller",
],
).ok
)
self.assertFalse(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
activate_controllers=[
"motion_primitive_forward_controller",
"forward_effort_controller",
],
).ok
)
self.assertFalse(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
activate_controllers=[
"motion_primitive_forward_controller",
"forward_velocity_controller",
],
).ok
)
self.assertFalse(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
activate_controllers=[
"motion_primitive_forward_controller",
"forward_position_controller",
],
).ok
)
self.assertFalse(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
activate_controllers=[
"motion_primitive_forward_controller",
"passthrough_trajectory_controller",
],
).ok
)
self.assertFalse(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
activate_controllers=[
"motion_primitive_forward_controller",
"freedrive_mode_controller",
],
).ok
)

# MoPrim controller and force_mode should be possible to combine
self.assertTrue(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
activate_controllers=[
"motion_primitive_forward_controller",
"force_mode_controller",
],
).ok
)
self.assertTrue(
self._controller_manager_interface.switch_controller(
strictness=SwitchController.Request.STRICT,
deactivate_controllers=[
"motion_primitive_forward_controller",
"force_mode_controller",
],
).ok
)