DYNAMIXELの各モードをROS 2で動かす
海洋ロボコンをやってた人です。
今回はDYNAMIXELを始めて使ったので、その備忘録として使い方を記載していきます。
この記事のゴールは、DYNAMIXEL製品で各モードを動かせることです。
動作環境は以下です。
- OS: Ubuntu 22.04
- ROS Distributions: Humble
使用するパッケージは以下に置いてあるので、cloneして利用してください。
本記事に対するコメントも積極的に募集しますので、よろしくお願いいたします。
DYNAMIXELのセットアップ・準備
この記事を見ている方は、DYANMIXELが既に手元にある/これから入手を検討している方だと思うので、想定される使用リストを記載します。
| Item | Detail:EN | 詳細：JP |
|---|---|---|
| U2D2 | USB IF | USBインターフェイス |
| U2D2 PHB | Power Hub Board | ハブ基盤 |
| XM430-WR350-R | Smart Actuator | アクチュエータ |
| 12V 5A SMPS | Power Supply | 電源供給 |
| RaspberryPi 4 Model B | OS:Ubuntu 22.04, ROS 2 Humble | マイコン |
上記SBCはラズパイ以外でも可です。
ROBOTIS OpenSource TeamよりDYNAMIXEL Quick Start Guide for ROS 2という丁寧なチュートリアル動画があるので、まずはこちらを見て進めるのも良いと思います。
DYNAMIXELはXM430-WR350-Rを使用します。
XM430-WR350にはT/RとTTLとRS485モデルがあります。
これらの違いについては、DynamixelのRS485モデルとTTLモデルの差は何ですか？ #10
に詳細があるため、気になる方は参照してください。
U2D2の組み立てはCombined with U2D2 Power Hubを参考に組み立て、配線します。
DYNAMIXEL Wizard 2.0のインストール
DYNAMIXELのファームウェア更新、ダイアグ（診断）、構成テストを行うためのツールとしてDYNAMIXEL Wizard 2.0 があります。
複数のDYNAMIXELを使用する場合に、こちらからIDを変更するのでインストールしておきます。
cd ~/Downloads
sudo chmod 775 DynamixelWizard2Setup_x64
./DynamixelWizard2Setup_x64
DYNAMIXEL用の電源をONにして、「Scan」を押すと接続されているDYNAMIXELを認識できます。
「Position > Torque」とすると、赤矢印の指定位置へサーボモータの位置を動かすこともできます。
DYNAMIXEL SDKで各モードをROS 2で動かす
最初に、使用するリポジトリをクローンします。
mkdir -p ~/robotis_ws/src && cd ~/robotis_ws/src
git clone https://github.com/tasada038/DynamixelSDK.git
cd ~/robotis_ws && colcon build
source /opt/ros/humble/setup.bash
. install/setup.bash
ls /dev/tty*
sudo usermod -aG dialout ubuntu
Position Control Mode：位置制御
ros2 topic pubで動かす
動作確認として、ros2 topic pubで適当な値を与えて動かしてみましょう。
1つ目のターミナルで以下を起動し
ros2 run dynamixel_sdk_examples read_write_position_node
2つ目のターミナルで以下を起動すると動作確認ができます。
cd ~/robotis_ws && . install/setup.bash
ros2 topic pub --once /set_position dynamixel_sdk_custom_interfaces/msg/SetPosition "{id: 1, position: 2000}"
ros2 topic pub --once /set_position dynamixel_sdk_custom_interfaces/msg/SetPosition "{id: 1, position: 0}"
複数のDYNAMIXELを動かす場合は、以下のようにコマンド入力して動かすことも可能です。
cd ~/robotis_ws && . install/setup.bash
ros2 topic pub --once /set_position dynamixel_sdk_custom_interfaces/msg/SetPosition "{id: 2, position: 2000}"
ros2 topic pub --once /set_position dynamixel_sdk_custom_interfaces/msg/SetPosition "{id: 1, position: 2000}"
ros2 topic pub --once /set_position dynamixel_sdk_custom_interfaces/msg/SetPosition "{id: 2, position: 0}"
ros2 topic pub --once /set_position dynamixel_sdk_custom_interfaces/msg/SetPosition "{id: 1, position: 0}"
動かすと以下のイメージです。
dynamixel_sdk_custom_interfacesで動かす
上記のようにtopic pubで動かす場合、汎用性がないため、dynamixel_sdk_custom_interfaces/msg/SetPositionをROS 2でpublishするサンプルも用意しました。以下各ターミナルで起動して動作確認します。
ros2 run dynamixel_sdk_examples read_write_position_node # First Shell
ros2 run dynamixel_sdk_examples pub_position_node # Second Shell
プログラムの中で重要な箇所は2点
-
dxl_comm_result = packetHandler->write4ByteTxRxで目標位置の指定する
-
ADDR_OPERATING_MODEをPOSITION_CONTROL(3)にする
ことです。
また、フォーク元のdynamixel_sdk_examples/src/read_write_node.cpp ではsubscriberにADDR_GOAL_POSITIONを使用しているため、Position Control Mode制御できることを意味しています。
なので、pub_position_nodeで0-4095の分解能で位置をpublishすれば良いと分かりますね。
モードに対しての、分解能やパラメータはControl Table Description - Goal Position(116)を参考にしてください。
以下抜粋です。
| Mode | Values | Description |
|---|---|---|
| Position Control Mode | Min Position Limit(52) ~ Max Position Limit(48) | 0.088 [°], 0 ~ 4,095(1 rotation) |
| Extended Position Control Mode | -1,048,575 ~ 1,048,575 | -256[rev] ~ 256[rev] |
| Current-based Position Control Mode | -1,048,575 ~ 1,048,575 | -256[rev] ~ 256[rev] |
Velocity Control Mode：速度制御
速度制御でも位置制御と同様に2点を主に変更します。
-
dxl_comm_result = packetHandler->write4ByteTxRxで目標速度の指定する
-
ADDR_OPERATING_MODEをVELOCITY_CONTROL(1)にする
ros2 run dynamixel_sdk_examples read_write_velocity_node # First Shell
ros2 run dynamixel_sdk_examples pub_velocity_node # Second Shell
速度制御についてのQ&Aや、cmd_velocityで左右の車輪を模擬して動作させたい場合は以下を参照してください。
cmd_velocity対応はTBD(to be determined：未定）です
位置制御、速度制御で動かすと、以下のようになります。
Current Control Mode：電流制御
Control Table Description - Goal Current(102)によると動作モード(11)がトルク制御モードの場合、目標電流(102)を使用して希望の電流で動かすことができるようです。
目標電流(102)を使用して、電流ベースの位置制御モードで電流の制限を設定することもできますが、目標電流(102)は電流制限(±38)より大きく設定できないことに注意してください。
| Unit | Value Range |
|---|---|
| about 2.69[mA] | -Current Limit(38) ~ Current Limit(38) |
電流制御でも同様に2点を主に変更します。
-
dxl_comm_result = packetHandler->write2ByteTxRxで目標速度の指定する
-
ADDR_OPERATING_MODEをTORQUE_CONTROL(0)にする
動作確認は以下で行います。
ros2 run dynamixel_sdk_examples read_write_current_node # First Shell
ros2 run dynamixel_sdk_examples pub_current_node # Second Shell
Current-based Position Control Mode：電流位置制御
恐らく、これが一番使ってみたい機能だと思うので、電流位置制御のサンプルも紹介します。
始めに、電流位置制御ですが、Position PID Gain(80, 82, 84), Feedforward 1st/2nd Gains(88, 90)にあるように、フィードフォワードおよび PID コントローラーは、目的の軌道に基づいて目的の電流値を計算しているようです。
プログラム側ではADDR_OPERATING_MODEをCURRENT_BASED_POSITION_CONTROL(5)にして電流、位置を与えることで動作確認することができます。
1つ目のread_write_current_pos_v1_nodeでは、モーターを外力により適当な角度回すと、固定位置へ戻る動作となっています。
ros2 run dynamixel_sdk_examples read_write_current_pos_v1_node # First Shell
ros2 run dynamixel_sdk_examples pub_current_pos_v1_node # Second Shell
2つ目のread_write_current_pos_v2_nodeでは、一定周期で位置制御を行いつつ、外力を検知（電流変化）があると現在位置まで戻る動作となっています。
プログラムの中身では、目標電流値を20 (2.69[mA]×20=53.8[mA])とし、目標位置をros2 topicでpublishしています。
dxl_comm_result =
packetHandler->write2ByteTxRx(
portHandler,
(uint8_t) msg->id,
ADDR_GOAL_CURRENT,
20,
&dxl_error
);
dxl_comm_result_pos =
packetHandler->write4ByteTxRx(
portHandler,
(uint8_t) msg->id,
ADDR_GOAL_POSITION,
goal_position,
&dxl_error
);
動作は以下を起動し
ros2 run dynamixel_sdk_examples read_write_current_pos_v2_node # First Shell
ros2 run dynamixel_sdk_examples pub_current_pos_v2_node # Second Shell
実際のイメージは以下です。
以上、皆さんのお力添えになれば幸いです。
この記事へのLike、Gitリポジトリへのスターいただけると大変励みになりますので、よろしくお願いいたします。
Other Reference
その他
技術的なご質問に対する回答集の記載も以下にあるので、学びたい方はこちらも参照すると良いと思います。
Discussion