Skip to content

Commit 20d0bfe

Browse files
mathias31415mathias31415
authored
Merge pull request #3 from urfeex/fix_moprim_controller
Fix moprim controller
2 parents 5bb0a8b + bf586b4 commit 20d0bfe

File tree

3 files changed

+123
-4
lines changed

3 files changed

+123
-4
lines changed

‎ur_robot_driver/config/ur_controllers.yaml‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -214,4 +214,3 @@ tool_contact_controller:
214214
motion_primitive_forward_controller:
215215
ros__parameters:
216216
tf_prefix: "$(var tf_prefix)"
217-

‎ur_robot_driver/src/hardware_interface.cpp‎

Lines changed: 16 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -68,48 +68,64 @@ URPositionHardwareInterface::URPositionHardwareInterface()
6868
mode_compatibility_[hardware_interface::HW_IF_POSITION][PASSTHROUGH_GPIO] = false;
6969
mode_compatibility_[hardware_interface::HW_IF_POSITION][FREEDRIVE_MODE_GPIO] = false;
7070
mode_compatibility_[hardware_interface::HW_IF_POSITION][TOOL_CONTACT_GPIO] = true;
71+
mode_compatibility_[hardware_interface::HW_IF_POSITION][HW_IF_MOTION_PRIMITIVES] = false;
7172

7273
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_POSITION] = false;
7374
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][hardware_interface::HW_IF_EFFORT] = false;
7475
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FORCE_MODE_GPIO] = false;
7576
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][PASSTHROUGH_GPIO] = false;
7677
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][FREEDRIVE_MODE_GPIO] = false;
7778
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][TOOL_CONTACT_GPIO] = true;
79+
mode_compatibility_[hardware_interface::HW_IF_VELOCITY][HW_IF_MOTION_PRIMITIVES] = false;
7880

7981
mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_POSITION] = false;
8082
mode_compatibility_[hardware_interface::HW_IF_EFFORT][hardware_interface::HW_IF_VELOCITY] = false;
8183
mode_compatibility_[hardware_interface::HW_IF_EFFORT][FORCE_MODE_GPIO] = false;
8284
mode_compatibility_[hardware_interface::HW_IF_EFFORT][PASSTHROUGH_GPIO] = false;
8385
mode_compatibility_[hardware_interface::HW_IF_EFFORT][FREEDRIVE_MODE_GPIO] = false;
8486
mode_compatibility_[hardware_interface::HW_IF_EFFORT][TOOL_CONTACT_GPIO] = true;
87+
mode_compatibility_[hardware_interface::HW_IF_EFFORT][HW_IF_MOTION_PRIMITIVES] = false;
8588

8689
mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false;
8790
mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
8891
mode_compatibility_[FORCE_MODE_GPIO][hardware_interface::HW_IF_EFFORT] = false;
8992
mode_compatibility_[FORCE_MODE_GPIO][PASSTHROUGH_GPIO] = true;
9093
mode_compatibility_[FORCE_MODE_GPIO][FREEDRIVE_MODE_GPIO] = false;
9194
mode_compatibility_[FORCE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
95+
mode_compatibility_[FORCE_MODE_GPIO][HW_IF_MOTION_PRIMITIVES] = true;
9296

9397
mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_POSITION] = false;
9498
mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
9599
mode_compatibility_[PASSTHROUGH_GPIO][hardware_interface::HW_IF_EFFORT] = false;
96100
mode_compatibility_[PASSTHROUGH_GPIO][FORCE_MODE_GPIO] = true;
97101
mode_compatibility_[PASSTHROUGH_GPIO][FREEDRIVE_MODE_GPIO] = false;
98102
mode_compatibility_[PASSTHROUGH_GPIO][TOOL_CONTACT_GPIO] = true;
103+
mode_compatibility_[PASSTHROUGH_GPIO][HW_IF_MOTION_PRIMITIVES] = false;
99104

100105
mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_POSITION] = false;
101106
mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_VELOCITY] = false;
102107
mode_compatibility_[FREEDRIVE_MODE_GPIO][hardware_interface::HW_IF_EFFORT] = false;
103108
mode_compatibility_[FREEDRIVE_MODE_GPIO][FORCE_MODE_GPIO] = false;
104109
mode_compatibility_[FREEDRIVE_MODE_GPIO][PASSTHROUGH_GPIO] = false;
105110
mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
111+
mode_compatibility_[FREEDRIVE_MODE_GPIO][TOOL_CONTACT_GPIO] = false;
112+
mode_compatibility_[FREEDRIVE_MODE_GPIO][HW_IF_MOTION_PRIMITIVES] = false;
106113

107114
mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_POSITION] = true;
108115
mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_VELOCITY] = true;
109116
mode_compatibility_[TOOL_CONTACT_GPIO][hardware_interface::HW_IF_EFFORT] = true;
110117
mode_compatibility_[TOOL_CONTACT_GPIO][FORCE_MODE_GPIO] = false;
111118
mode_compatibility_[TOOL_CONTACT_GPIO][PASSTHROUGH_GPIO] = true;
112119
mode_compatibility_[TOOL_CONTACT_GPIO][FREEDRIVE_MODE_GPIO] = false;
120+
mode_compatibility_[TOOL_CONTACT_GPIO][HW_IF_MOTION_PRIMITIVES] = true;
121+
122+
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][hardware_interface::HW_IF_POSITION] = false;
123+
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][hardware_interface::HW_IF_VELOCITY] = false;
124+
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][hardware_interface::HW_IF_EFFORT] = false;
125+
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][FORCE_MODE_GPIO] = true;
126+
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][PASSTHROUGH_GPIO] = false;
127+
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][FREEDRIVE_MODE_GPIO] = false;
128+
mode_compatibility_[HW_IF_MOTION_PRIMITIVES][TOOL_CONTACT_GPIO] = true;
113129
}
114130

115131
URPositionHardwareInterface::~URPositionHardwareInterface()
@@ -1398,7 +1414,6 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
13981414
}
13991415
if (stop_modes_[0].size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(),
14001416
StoppingInterface::STOP_PASSTHROUGH) != stop_modes_[0].end()) {
1401-
RCLCPP_WARN(get_logger(), "Stopping passthrough trajectory controller.");
14021417
passthrough_trajectory_controller_running_ = false;
14031418
passthrough_trajectory_abort_ = 1.0;
14041419
trajectory_joint_positions_.clear();
@@ -1417,8 +1432,6 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod
14171432
resetMoprimCmdInterfaces();
14181433
current_moprim_execution_status_ = MoprimExecutionState::IDLE;
14191434
ready_for_new_moprim_ = false;
1420-
1421-
RCLCPP_INFO(get_logger(), "Motion primitives mode stopped.");
14221435
}
14231436
if (stop_modes_.size() != 0 && std::find(stop_modes_[0].begin(), stop_modes_[0].end(),
14241437
StoppingInterface::STOP_TOOL_CONTACT) != stop_modes_[0].end()) {

‎ur_robot_driver/test/integration_test_controller_switch.py‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,7 @@
5656
"passthrough_trajectory_controller",
5757
"force_mode_controller",
5858
"freedrive_mode_controller",
59+
"motion_primitive_forward_controller",
5960
]
6061

6162

@@ -483,3 +484,109 @@ def test_tool_contact_compatibility(self):
483484
],
484485
).ok
485486
)
487+
488+
self.assertTrue(
489+
self._controller_manager_interface.switch_controller(
490+
strictness=SwitchController.Request.STRICT,
491+
activate_controllers=[
492+
"motion_primitive_forward_controller",
493+
"tool_contact_controller",
494+
],
495+
).ok
496+
)
497+
self.assertTrue(
498+
self._controller_manager_interface.switch_controller(
499+
strictness=SwitchController.Request.STRICT,
500+
deactivate_controllers=[
501+
"motion_primitive_forward_controller",
502+
"tool_contact_controller",
503+
],
504+
).ok
505+
)
506+
507+
def test_moprim_compatibility(self):
508+
# Deactivate all writing controllers
509+
self.assertTrue(
510+
self._controller_manager_interface.switch_controller(
511+
strictness=SwitchController.Request.BEST_EFFORT,
512+
deactivate_controllers=ALL_CONTROLLERS,
513+
).ok
514+
)
515+
516+
time.sleep(3)
517+
518+
# moprim controller should not start with any other joint controller
519+
self.assertFalse(
520+
self._controller_manager_interface.switch_controller(
521+
strictness=SwitchController.Request.STRICT,
522+
activate_controllers=[
523+
"motion_primitive_forward_controller",
524+
"joint_trajectory_controller",
525+
],
526+
).ok
527+
)
528+
self.assertFalse(
529+
self._controller_manager_interface.switch_controller(
530+
strictness=SwitchController.Request.STRICT,
531+
activate_controllers=[
532+
"motion_primitive_forward_controller",
533+
"forward_effort_controller",
534+
],
535+
).ok
536+
)
537+
self.assertFalse(
538+
self._controller_manager_interface.switch_controller(
539+
strictness=SwitchController.Request.STRICT,
540+
activate_controllers=[
541+
"motion_primitive_forward_controller",
542+
"forward_velocity_controller",
543+
],
544+
).ok
545+
)
546+
self.assertFalse(
547+
self._controller_manager_interface.switch_controller(
548+
strictness=SwitchController.Request.STRICT,
549+
activate_controllers=[
550+
"motion_primitive_forward_controller",
551+
"forward_position_controller",
552+
],
553+
).ok
554+
)
555+
self.assertFalse(
556+
self._controller_manager_interface.switch_controller(
557+
strictness=SwitchController.Request.STRICT,
558+
activate_controllers=[
559+
"motion_primitive_forward_controller",
560+
"passthrough_trajectory_controller",
561+
],
562+
).ok
563+
)
564+
self.assertFalse(
565+
self._controller_manager_interface.switch_controller(
566+
strictness=SwitchController.Request.STRICT,
567+
activate_controllers=[
568+
"motion_primitive_forward_controller",
569+
"freedrive_mode_controller",
570+
],
571+
).ok
572+
)
573+
574+
# MoPrim controller and force_mode should be possible to combine
575+
self.assertTrue(
576+
self._controller_manager_interface.switch_controller(
577+
strictness=SwitchController.Request.STRICT,
578+
activate_controllers=[
579+
"motion_primitive_forward_controller",
580+
"force_mode_controller",
581+
],
582+
).ok
583+
)
584+
self.assertTrue(
585+
self._controller_manager_interface.switch_controller(
586+
strictness=SwitchController.Request.STRICT,
587+
deactivate_controllers=[
588+
"motion_primitive_forward_controller",
589+
"force_mode_controller",
590+
],
591+
).ok
592+
)

0 commit comments

Comments
 (0)