OpenManipulator-X(ROS2)-Humble-Chap5-控制器

章節目錄

C5. Controller 控制器

C5. Controller 控制器 #

OpenMANIPULATOR-X 控制器已經重構，利用 ros2_control 框架和 MoveIt，來增強靈活性、模組化和可用性。此更新後的控制器，可與基於 ROS2 的系統無縫整合，提供高級功能，例如軌跡規劃(trajectory planning)、即時控制(real-time control)和狀態回饋(state feedback)等。

Note 請確認相依的套件，在執行下列指令時，都已事前安裝好。
- 安裝 ROS packages。(https://emanual.robotis.com/docs/en/platform/openmanipulator_x/quick_start_guide/#install-ros-packages)

C5.1. 啟動 controller #

開新 terminal，啟動 open_manipulator_x packages。

如是使用 U2D2

結束所有 terminal，開新 terminal 輸入下列指令

				
					$ ros2 launch open_manipulator_x_bringup hardware.launch.py

如是使用 OpenCR

結束所有 terminal，開新 terminal 輸入下列指令

				
					$ ros2 launch open_manipulator_x_bringup hardware.launch.py port_name:=/dev/ttyACM0

警告請在運行 OpenMANIPULATOR-X 手臂之前，先檢查每個關節的位置。如果關節位置超出運作範圍，手臂可能會停止操作。下面的圖片向你展示了 OpenMANIPULATOR-X 機器手臂，在啟動前的建議姿勢方位。當還未啟動 DYNAMIXEL 智能馬達的扭力時，請調整每個關節的方位如下圖所示。

透過 U2D2 啟動 controller 成功後，在 terminal 顯示輸出結果會類似於

				
					$ ros2 launch open_manipulator_x_bringup hardware.launch.py

[INFO] [launch]: All log files can be found below /home/omx/.ros/log/2024-12-10-16-13-03-846807-omx-3063
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ros2_control_node-1]: process started with pid [3066]
[INFO] [robot_state_publisher-2]: process started with pid [3068]
[INFO] [spawner-3]: process started with pid [3070]
[robot_state_publisher-2] [INFO] [1733814784.140348781] [robot_state_publisher]: got segment dummy_mimic_fix
[robot_state_publisher-2] [INFO] [1733814784.140441706] [robot_state_publisher]: got segment end_effector_link
[robot_state_publisher-2] [INFO] [1733814784.140450447] [robot_state_publisher]: got segment gripper_left_link
[robot_state_publisher-2] [INFO] [1733814784.140455005] [robot_state_publisher]: got segment gripper_right_link
[robot_state_publisher-2] [INFO] [1733814784.140459339] [robot_state_publisher]: got segment link1
[robot_state_publisher-2] [INFO] [1733814784.140463495] [robot_state_publisher]: got segment link2
[robot_state_publisher-2] [INFO] [1733814784.140467530] [robot_state_publisher]: got segment link3
[robot_state_publisher-2] [INFO] [1733814784.140471673] [robot_state_publisher]: got segment link4
[robot_state_publisher-2] [INFO] [1733814784.140475662] [robot_state_publisher]: got segment link5
[robot_state_publisher-2] [INFO] [1733814784.140479440] [robot_state_publisher]: got segment world
[ros2_control_node-1] [WARN] [1733814784.150297704] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-1] [INFO] [1733814784.150762238] [resource_manager]: Loading hardware 'OpenManipulatorXSystem' 
[ros2_control_node-1] [INFO] [1733814784.163256190] [resource_manager]: Initialize hardware 'OpenManipulatorXSystem' 
[ros2_control_node-1] transmission_to_joint_matrix_ 
[ros2_control_node-1] [0][0] 1.000000, [0][1] 0.000000, [0][2] 0.000000, [0][3] 0.000000, [0][4] 0.000000, 
[ros2_control_node-1] [1][0] 0.000000, [1][1] 1.000000, [1][2] 0.000000, [1][3] 0.000000, [1][4] 0.000000, 
[ros2_control_node-1] [2][0] 0.000000, [2][1] 0.000000, [2][2] 1.000000, [2][3] 0.000000, [2][4] 0.000000, 
[ros2_control_node-1] [3][0] 0.000000, [3][1] 0.000000, [3][2] 0.000000, [3][3] 1.000000, [3][4] 0.000000, 
[ros2_control_node-1] [4][0] 0.000000, [4][1] 0.000000, [4][2] 0.000000, [4][3] 0.000000, [4][4] 1.000000, 
[ros2_control_node-1] [5][0] 0.000000, [5][1] 0.000000, [5][2] 0.000000, [5][3] 0.000000, [5][4] 0.000000, 
[ros2_control_node-1] joint_to_transmission_matrix_ 
[ros2_control_node-1] [0][0] 1.000000, [0][1] 0.000000, [0][2] 0.000000, [0][3] 0.000000, [0][4] 0.000000, [0][5] 0.000000, 
[ros2_control_node-1] [1][0] 0.000000, [1][1] 1.000000, [1][2] 0.000000, [1][3] 0.000000, [1][4] 0.000000, [1][5] 0.000000, 
[ros2_control_node-1] [2][0] 0.000000, [2][1] 0.000000, [2][2] 1.000000, [2][3] 0.000000, [2][4] 0.000000, [2][5] 0.000000, 
[ros2_control_node-1] [3][0] 0.000000, [3][1] 0.000000, [3][2] 0.000000, [3][3] 1.000000, [3][4] 0.000000, [3][5] 0.000000, 
[ros2_control_node-1] [4][0] 0.000000, [4][1] 0.000000, [4][2] 0.000000, [4][3] 0.000000, [4][4] 1.000000, [4][5] 0.000000, 
[ros2_control_node-1] [INFO] [1733814784.164000125] [dynamixel_hardware_interface]: port_name /dev/ttyUSB0 / baudrate 1000000
[ros2_control_node-1] Dynamixel Information File List.
[ros2_control_node-1] num: 1000, name: xh430_w350.model
[ros2_control_node-1] num: 1020, name: xm430_w350.model
[ros2_control_node-1] num: 1060, name: xl430_w250.model
[ros2_control_node-1] num: 1080, name: xc430_w240.model
[ros2_control_node-1] num: 1100, name: xh540_w270.model
[ros2_control_node-1] num: 1160, name: 2xc430_w250.model
[ros2_control_node-1] num: 4000, name: ym070_210_m001.model
[ros2_control_node-1] num: 4020, name: ym070_210_r051.model
[ros2_control_node-1] num: 4030, name: ym070_210_r099.model
[ros2_control_node-1] num: 4050, name: ym070_210_a099.model
[ros2_control_node-1] num: 4120, name: ym080_230_m001.model
[ros2_control_node-1] num: 4140, name: ym080_230_r051.model
[ros2_control_node-1] num: 4150, name: ym080_230_r099.model
[ros2_control_node-1] num: 4170, name: ym080_230_a099.model
[ros2_control_node-1] num: 35074, name: rh_p12_rn.model
[ros2_control_node-1] [INFO] [1733814784.165060244] [dynamixel_hardware_interface]: $$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
[ros2_control_node-1] [INFO] [1733814784.165085781] [dynamixel_hardware_interface]: $$$$$ Init Dxl Comm Port
[ros2_control_node-1] [INFO] [1733814784.165119953] [dynamixel_hardware_interface]: Revolute to Prismatic gripper conversion enabled.
[ros2_control_node-1] Succeeded to open the port!
[ros2_control_node-1] Succeeded to change the [1000000] baudrate!
[ros2_control_node-1] [ID:011] Request ping	 - Ping succeeded. Dynamixel model number : 1020
[ros2_control_node-1] [ID:012] Request ping	 - Ping succeeded. Dynamixel model number : 1020
[ros2_control_node-1] [ID:013] Request ping	 - Ping succeeded. Dynamixel model number : 1020
[ros2_control_node-1] [ID:014] Request ping	 - Ping succeeded. Dynamixel model number : 1020
[ros2_control_node-1] [ID:015] Request ping	 - Ping succeeded. Dynamixel model number : 1020
[ros2_control_node-1] [INFO] [1733814784.172855510] [dynamixel_hardware_interface]: Trying to connect to the communication port...
[ros2_control_node-1] [INFO] [1733814784.172904554] [dynamixel_hardware_interface]: $$$$$ Init Dxl Items
[ros2_control_node-1] [INFO] [1733814784.174263837] [dynamixel_hardware_interface]: [ID:11] item_name:Drive Mode	data:0
[ros2_control_node-1] [INFO] [1733814784.175202777] [dynamixel_hardware_interface]: [ID:11] item_name:Position D Gain	data:100
[ros2_control_node-1] [INFO] [1733814784.176189566] [dynamixel_hardware_interface]: [ID:11] item_name:Position I Gain	data:100
[ros2_control_node-1] [INFO] [1733814784.177218657] [dynamixel_hardware_interface]: [ID:11] item_name:Position P Gain	data:800
[ros2_control_node-1] [INFO] [1733814784.178201814] [dynamixel_hardware_interface]: [ID:12] item_name:Drive Mode	data:0
[ros2_control_node-1] [INFO] [1733814784.179188157] [dynamixel_hardware_interface]: [ID:12] item_name:Position D Gain	data:100
[ros2_control_node-1] [INFO] [1733814784.180215312] [dynamixel_hardware_interface]: [ID:12] item_name:Position I Gain	data:100
[ros2_control_node-1] [INFO] [1733814784.181209639] [dynamixel_hardware_interface]: [ID:12] item_name:Position P Gain	data:800
[ros2_control_node-1] [INFO] [1733814784.182195348] [dynamixel_hardware_interface]: [ID:13] item_name:Drive Mode	data:0
[ros2_control_node-1] [INFO] [1733814784.183204690] [dynamixel_hardware_interface]: [ID:13] item_name:Position D Gain	data:100
[ros2_control_node-1] [INFO] [1733814784.184192489] [dynamixel_hardware_interface]: [ID:13] item_name:Position I Gain	data:100
[ros2_control_node-1] [INFO] [1733814784.185186500] [dynamixel_hardware_interface]: [ID:13] item_name:Position P Gain	data:800
[ros2_control_node-1] [INFO] [1733814784.186251591] [dynamixel_hardware_interface]: [ID:14] item_name:Drive Mode	data:0
[ros2_control_node-1] [INFO] [1733814784.187186969] [dynamixel_hardware_interface]: [ID:14] item_name:Position D Gain	data:100
[ros2_control_node-1] [INFO] [1733814784.188187523] [dynamixel_hardware_interface]: [ID:14] item_name:Position I Gain	data:100
[ros2_control_node-1] [INFO] [1733814784.189248186] [dynamixel_hardware_interface]: [ID:14] item_name:Position P Gain	data:800
[ros2_control_node-1] [INFO] [1733814784.189281693] [dynamixel_hardware_interface]: $$$$$ Init Dxl Read Items
[ros2_control_node-1] Dynamixel Read Type : sync read
[ros2_control_node-1] ID : 11, 12, 13, 14, 15, 
[ros2_control_node-1] Read items : 	Present Position	Present VelocityPresent Current	Torque Enable	Present Input Voltage
[ros2_control_node-1] set sync read (indirect addr) : addr 224, size 13
[ros2_control_node-1] Success to set SyncRead handler using indirect address
[ros2_control_node-1] [INFO] [1733814784.255333410] [dynamixel_hardware_interface]: $$$$$ Init Dxl Write Items
[ros2_control_node-1] Dynamixel Write Type : sync write
[ros2_control_node-1] ID : 11, 12, 13, 14, 15, 
[ros2_control_node-1] Write items : 	Goal Position
[ros2_control_node-1] set sync write (indirect addr) : addr 634, size 4
[ros2_control_node-1] Success to set SyncWrite handler using indirect address
[ros2_control_node-1] [INFO] [1733814784.285087764] [resource_manager]: Successful initialization of hardware 'OpenManipulatorXSystem'
[ros2_control_node-1] [INFO] [1733814784.285623329] [resource_manager]: 'configure' hardware 'OpenManipulatorXSystem' 
[ros2_control_node-1] [INFO] [1733814784.285649762] [resource_manager]: Successful 'configure' of hardware 'OpenManipulatorXSystem'
[ros2_control_node-1] [INFO] [1733814784.285677277] [resource_manager]: 'activate' hardware 'OpenManipulatorXSystem' 
[ros2_control_node-1] [INFO] [1733814784.288399302] [dynamixel_hardware_interface]: Sync joint state to command (position, 3.25354 <- position, 3.25354
[ros2_control_node-1] [INFO] [1733814784.288484144] [dynamixel_hardware_interface]: Sync joint state to command (position, -22.5879 <- position, -22.5879
[ros2_control_node-1] [INFO] [1733814784.288522542] [dynamixel_hardware_interface]: Sync joint state to command (position, 48.5393 <- position, 48.5393
[ros2_control_node-1] [INFO] [1733814784.288556311] [dynamixel_hardware_interface]: Sync joint state to command (position, 59.4431 <- position, 59.4431
[ros2_control_node-1] [INFO] [1733814784.288587808] [dynamixel_hardware_interface]: Sync joint state to command (position, 1.08561 <- position, 1.08561
[ros2_control_node-1] [INFO] [1733814784.288619773] [dynamixel_hardware_interface]: Sync joint state to command (position, 0 <- position, 0
[spawner-3] [INFO] [1733814784.344387122] [spawner_joint_state_broadcaster]: waiting for service /controller_manager/list_controllers to become available...
[ros2_control_node-1] [ID:011] Torque ON
[ros2_control_node-1] [ID:012] Torque ON
[ros2_control_node-1] [ID:013] Torque ON
[ros2_control_node-1] [ID:014] Torque ON
[ros2_control_node-1] [ID:015] Torque ON
[ros2_control_node-1] [INFO] [1733814784.794363518] [dynamixel_hardware_interface]: Dynamixel Hardware Start!
[ros2_control_node-1] [INFO] [1733814784.794583503] [resource_manager]: Successful 'activate' of hardware 'OpenManipulatorXSystem'
[ros2_control_node-1] [INFO] [1733814784.844187956] [controller_manager]: update rate is 1000 Hz
[ros2_control_node-1] [INFO] [1733814784.844239126] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50
[ros2_control_node-1] [WARN] [1733814784.847602761] [controller_manager]: No real-time kernel detected on this system. See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling.
[ros2_control_node-1] [INFO] [1733814784.864748196] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-3] [INFO] [1733814784.872066996] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-1] [INFO] [1733814784.872576377] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-1] [INFO] [1733814784.872671446] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-3] [INFO] [1733814784.890559256] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[INFO] [spawner-3]: process has finished cleanly [pid 3070]
[INFO] [spawner-4]: process started with pid [3114]
[INFO] [spawner-5]: process started with pid [3116]
[ros2_control_node-1] [INFO] [1733814785.561511632] [controller_manager]: Loading controller 'arm_controller'
[ros2_control_node-1] [WARN] [1733814785.574916138] [arm_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ros2_control_node-1] [INFO] [1733814785.576633589] [controller_manager]: Loading controller 'gripper_controller'
[spawner-5] [INFO] [1733814785.598697696] [spawner_arm_controller]: Loaded arm_controller
[ros2_control_node-1] [INFO] [1733814785.599380288] [controller_manager]: Configuring controller 'arm_controller'
[ros2_control_node-1] [INFO] [1733814785.599545696] [arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-1] [INFO] [1733814785.599572347] [arm_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ros2_control_node-1] [INFO] [1733814785.599598355] [arm_controller]: Using 'splines' interpolation method.
[spawner-4] [INFO] [1733814785.600233495] [spawner_gripper_controller]: Loaded gripper_controller
[ros2_control_node-1] [INFO] [1733814785.600325312] [arm_controller]: Controller state will be published at 200.00 Hz.
[ros2_control_node-1] [INFO] [1733814785.605748626] [arm_controller]: Action status changes will be monitored at 20.00 Hz.
[ros2_control_node-1] [INFO] [1733814785.609463352] [controller_manager]: Configuring controller 'gripper_controller'
[ros2_control_node-1] [INFO] [1733814785.609542143] [gripper_controller]: Action status changes will be monitored at 20Hz.
[spawner-5] [INFO] [1733814785.619695267] [spawner_arm_controller]: Configured and activated arm_controller
[spawner-4] [INFO] [1733814785.626031382] [spawner_gripper_controller]: Configured and activated gripper_controller
[INFO] [spawner-4]: process has finished cleanly [pid 3114]
[INFO] [spawner-5]: process has finished cleanly [pid 3116]

TIPS 1. 如果無法加載 DYNAMIXEL 智能馬達，請使用 DYNAMIXEL Wizard 2.0 來檢查韌體。
(https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/#firmware-update)

NOTE OpenMANIPULATOR-X controller 與 Protocol 2.0 相容。
Protocol 1.0 不支持同時存取多個 DYNAMIXEL 的 SyncRead 指令。
Protocol 2.0 支持 Dynamixel MX 2.0，X 系列，P 系列和 Pro 系列智能馬達，但它不支持舊款 protocol 1.0 的 Dynamixel AX，RX 和 EX 系列智能馬達。

C5.2. 檢查設定 #

C5.2.1. Manipulator Description 手臂描述 #

此節不支援

C5.2.2. RViz #

在 RViz 中載入 OpenManipulator-X。

				
					$ ros2 launch open_manipulator_x_description model.launch.py

C5.3. Message List #

NOTE 如要了解更詳細，請參考 MoveIt 2 及 ros2_control 官方文件。
- MoveIt 2 官方文件 (https://moveit.picknik.ai/main/index.html)
- ros2_control 官方文件 (https://control.ros.org/humble/index.html)

C5.3.1. Topic #

此節不支援

C5.3.2. Service #

此節不支援

C5.4. Experimental 實驗性 #

C5.4.1. MoveIt 2! #

MoveIt 2! 是一組用於機器人，進行運動規劃(motion planning)、操作、逆運動學(Inverse Kinematics)、控制、3D 感知(3D Perception)，和碰撞檢查的套件包。

C5.4.1.1. 啟動 MoveIt 2! 的手臂硬體 #

啟動 MoveIt 2! 前，在另一個 terminal，請確認 Chap 5.1. Launch Controller 內的 hardware.launch.py 已經先執行。

然後下列指令啟動 MoveIt!

				
					$ ros2 launch open_manipulator_x_moveit_config moveit_core.launch.py

MoveIt 2 使用簡單說明：

1. 你可以使用 RViz 中可見的 Interactive Marker，將機器手臂移動到所需的方位。
2. 使用命令列中的 Plan 選項，來模擬機器手臂的運動。
3. 點擊 “Execute”，可根據模擬的動作來移動機器手臂。
4. 將 Planning Group 設定為 arm，並使用 Goal State 選項(例如 init 或 home)，將機器手臂移動到預先定義的姿勢。
5. 將 Planning Group 變更為 gripper，並使用 Goal State 選項(例如 close 或 open)，來開啟和關閉夾爪。

C5.4.1.2. 在 Gazebo 模擬器使用 MoveIt! #

Gazebo 是一個軟體工具，它無需實體機器人，就可以在一個虛擬環境中，模擬及測試你的機器人。

你可以用下列指令，在 Gazebo 虛擬環境中，使用 MoveIt 功能來操作虛擬機器人。

更多 Gazebo 資訊，可參考 Chap 7. Simulation 章節。

結束所有 terminal，啟動 Gazebo 模擬器及 RViz 可視化工具。

				
					$ ros2 launch open_manipulator_x_moveit_config moveit_gazebo.launch.py

C5.4.2. Gravity Compensation 重力補償 #

Note ROS2 尚未支援 Gravity Compensation 重力補償

How can we help you?

C5. Controller 控制器 #

C5.1. 啟動 controller #

C5.2. 檢查設定 #

C5.2.1. Manipulator Description 手臂描述 #

C5.2.2. RViz #

C5.3. Message List #

C5.3.1. Topic #

C5.3.2. Service #

C5.4. Experimental 實驗性 #

C5.4.1. MoveIt 2! #

C5.4.1.1. 啟動 MoveIt 2! 的手臂硬體 #

C5.4.1.2. 在 Gazebo 模擬器使用 MoveIt! #

C5.4.2. Gravity Compensation 重力補償 #