Robotis社のDynamixelを動かすpkgの紹介 (ROS 2)

Last updated at 2025-01-16Posted at 2024-12-28

初めに

Robotis社のDynamixelをROSから制御するための ros pkg dynamixel_handlerを提供するリポジトリ https://github.com/ROBOTIS-JAPAN-GIT/DynamixelHandler-ros2 の紹介です．
Robotis日本支社の公式リポジトリとして取り込まれているので活用してもらえたらうれしいです．

だいたいReadmeのコピーですが，一応紹介というていで書きます．

このパッケージを作ったモチベーション

サーボモータDynamixelは非常に使いやすく良いモータです．しかし，提供されているSDKの使い勝手が微妙です．

Dyanmixelは，一般的な"かしこい"サーボモータと同様に，何らかの形でサーボ内蔵のマイコンに対して通信を行う必要があります．特にDyanmixelの場合は RS485 or TTL のどちらかになるので，USB-シリアル変換を用いて通信を行うことになります．

公式のSDKはシリアル通信と通信プロトコルについての下位のAPIを提供するのみで，角度の指令，トルクのオンオフなどの上位機能は自分で実装しなくてはなりません．
ROS対応をうたっているのに，Dynamixelと実際に通信するためのROSノードは自分で書かなければいけないということです．(RealSenseなんかはaptでpkgをインストールしたら ros2 launch ... でそのまま動くというのに...)

補足 (@Dream_Drive さんからの指摘をいただきました．)
dynamixel_workbench パッケージを用いることで，ROS 経由でそのまま動かせます．
ただ，Dynamixelの細かい機能を活用するためのインターフェースが提供されていないことや，サーボをjointかwheelのどちらかの部品として扱う必要があり事前情報を設定しなくてはならないなど，問題が多いです．

かといって，繋いで即動作確認ができる Dynamixel Wizard (公式のGUIアプリ)では複数モータを同時に動かすことはできない...

ということで「サーボを繋いでROSノードを起動したらそのまま動かせる！」を目標に開発したのがこのpkg DynamixelHandler-ros2 になります．
version 0.2.0 をリリースしたばかりなのでバグ報告などあれば助かります．

本パッケージの特徴

Dynamixel制御に特化した最小単位のパッケージ
- このパッケージが提供する dynamixel_handler_node は，サーボとの通信を担う．
- サーボの動作制御はユーザーが開発する 別の制御ノードが行い，dynamixel_handler_node は通信の仲介を行うイメージ．
- ユーザーはシリアル通信の通信プロトコルや，コントロールテーブルついて知る必要が(あまり)ない．
- Dynamixelの各種情報や機能が適切に分類され，ほぼすべての情報・機能を利用することが可能
ROSトピックのみで制御できるシンプルなインターフェース
トピック通信だけで利用できるので，このパッケージ自体のコードの編集は不要
- Publish: /dynamixel/states
  - 分類された各情報を個別の周期で自動的に read & publish．
- Subscribe: /dynamixel/commands/x (他、/dynamixel/commands/pなど)
  - シリーズごとに定義された制御コマンドを送ることで動作制御．
  - 制御コマンドの内容に合わせてサーボの制御モードが自動変更．
物理量ベースでやり取り
- current [mA], velocity [deg/s], position [deg] などの物理量を直接扱える．
- Dynamixelからシリアル通信で read されたパルス値(0 ~ 4095 など)は物理量に変換してから publish される．
- Dynamixelへの目標値は物理量で sbscribe され，内部でパルス値に変換されてからシリアル通信で write される．
※ 特に角度を rad (-π ~ π) ではなく degree (-180.0 ~ 180.0) で扱うので直感的．
比較的高速なRead/Write
- 読み書きをできるだけ一括で行うことで通信回数を削減
  - 連続アドレスの一括読み書き
  - 複数サーボの一括読み書き (SyncRead/SyncWrite)
- Fast Sync Read インストラクションによるReadの高速化
  - 通信形式についての細かい知識が無くてもros paramで設定するだけで利用可能．
- ROS Node周期に同期したRead/Write
  - Topicの受信頻度に依存しないため，安定した通信が可能．
※ 最速設定で700~800HzでのRead/Writeが可能．遅めの設定でも12サーボ同時Read/Writeでも150Hz以上で通信が可能．
※※ 高速通信には, latency_time (1~4ms) と baudrate (1,000,000bps 以上) の設定が必要
開発の手間を減らす便利機能
- 初期化時の動作
  - 連結したDynamixelを自動で認識
  - 連結されるDynamixel数の指定 (Optional)
    - ロボットのサーボ数を事前に入力することで，断線などの異常を検知可能
  - エラーを自動でクリア (Optional)
  - トルクを自動でON (Optional)
- 終了時の動作
  - node を kill したタイミングで動作を停止
  - node を kill したタイミングでトルクをOFF (Optional)
- Dummy Servo 機能 (Optional)
  - 未接続のサーボのIDを与えることで，そのIDのサーボの挙動を簡易シミュレート
  - 未接続のサーボを考慮したい時や実機を動かしたくない場合でも動作確認が可能
- ハードウェアエラーのクリア
  - Rebootすると回転数が消失する問題を homing offset により自動補正
- Baudrateの一括変更 (別ノードで提供)
  - パッケージに同梱している dynamixel_unify_baudrate_node で一括で変更可能
ROSパラメータによる各種ログ表示制御
- CallBackした内容
- Read/Write にかかる平均時間とSerial通信の成功率
- Read/Write されるパルス値
- Readに失敗したID
- etc...

特にROSトピックのPub/Subのみでモータからの情報取得と制御を行えるため，
$ ros2 topic pub ~ と合わせることで，コマンドラインからでもDynamixelのほぼすべての機能を利用することが可能です．

(Dynamixelの通信プロトコル用語がわかる人向けの説明)
コントロールテーブルの複数アドレスからの読み取りや Fast Sync read インストラクションの利用など，自分が持っている通信高速化に必要なノウハウを詰め込みました．(この辺は自分で毎回実装すると非常に面倒なので，そういう意味でも独立したパッケージ, ノードとして再利用可能な形で作っておきたかった．)
もっとこうしたら早くなるよっていう知識を持ってる人がいたら教えてくれるとうれしいです．

補足 (こちらも @Dream_Drive さん情報をいただきました．)
return delay time がデフォルトで 0.5ms もあるということで，それを 0ms にするように設定しました．
あまり必要性はわかりせんが，ROSパラメータから好きな値に設定することもできます．

How to install

パッケージをgit clone

お好みで．

cd ~/ros2_ws/src
# sshの場合
git clone --recursive git@github.com:ROBOTIS-JAPAN-GIT/DynamixelHandler-ros2.git dynamixel_handler
# httpsの場合
git clone --recursive https://github.com/ROBOTIS-JAPAN-GIT/DynamixelHandler-ros2.git dynamixel_handler
# 旧バージョンを使いたい場合
git clone --recursive https://github.com/ROBOTIS-JAPAN-GIT/DynamixelHandler-ros2.git dynamixel_handler -b ver0.1.0

ビルド

--symlink-install と --packages-up-to　のオプションを忘れずに．

cd ~/ros2_ws
colcon build --symlink-install --packages-up-to dynamixel_handler
source ~/.bashrc # 初回 build 時のみ

How to use

1. Dynamixelの接続

DynaimixelをディジーチェーンにしてU2D2経由でUSB接続されていること．
idに重複がないように事前に設定されていること.
baudrateが全て統一されていること．

つまり，Dynamixel Wizardでモータの動作確認ができる程度の状態を想定している．

baudrateを一括で変更するための dynamixel_unify_baudrate node も用意してあります，詳しくはリポジトリのReadmeを参照してください．

2. dynamixel_handler nodeの起動

2-1. 自分の環境とconfigの設定を合わせる

config/config_dynamixel_handler.ymlの該当部分を編集し，保存．
以下は baudrate: $57600$ かつ device name: /dec/ttyUSB0 かつ latency timer: $16$ ms の場合

# config/config_dynamixel_handler.launch
/**:
    ros__parameters:
        # 通信機器の設定
        device_name: /dev/ttyUSB0 # 通信するデバイス名,
        baudrate: 57600    # 通信速度, Dynamixelのデフォルトは 57600 bps (で遅い) 
        latency_timer: 16 # 通信のインターバル, 多くの環境でデフォルトは 16 ms (で遅い)

通信環境の設定についてはReadMeのParametersの章を参照.

2-2. ターミナルから実行

ros2 launch dynamixel_handler dynamixel_handler_launch.xml

# 出力例
# ... 略 ...
[dynamixel_handler_node-1] 00000.00000: Initializing DynamixelHandler ..... 
[dynamixel_handler_node-1] 00000.00000:  Succeeded to open device [/dev/ttyUSB0]     
[dynamixel_handler_node-1] 00000.00000:   -------------- baudrate '4000000' 
[dynamixel_handler_node-1] 00000.00000:   --------- latency_timer '1' 
[dynamixel_handler_node-1] 00000.00000: Expected number of Dynamixel is not set. 
[dynamixel_handler_node-1] 00000.00000: Free number of Dynamixel is allowed.
[dynamixel_handler_node-1] 00000.00000:  Auto scanning Dynamixel (id range [0] to [30]) ...
[dynamixel_handler_node-1] 00000.00000:    * P series servo id [1] is found 
[dynamixel_handler_node-1] 00000.00000:    ID [1] is enabled torque 
[dynamixel_handler_node-1] 00000.00000:    * X series servo id [6] is found 
[dynamixel_handler_node-1] 00000.00000:    ID [6] is enabled torque 
[dynamixel_handler_node-1] 00000.00000:    * X series servo id [7] is found 
[dynamixel_handler_node-1] 00000.00000:    ID [7] is enabled torque 
[dynamixel_handler_node-1] 00000.00000:  ... Finish scanning Dynamixel 
[dynamixel_handler_node-1] 00000.00000: ..... DynamixelHandler is initialized 
[dynamixel_handler_node-1] 00000.00000: Loop [0]: write=0.00ms read=11.72ms(p/f=100%/100%) 
[dynamixel_handler_node-1] 00000.00000: Loop [300]: write=0.01ms read=5.55ms(p/f=100%/100%) 
[dynamixel_handler_node-1] 00000.00000: Loop [600]: write=0.01ms read=5.47ms(p/f=100%/100%) 
[dynamixel_handler_node-1] 00000.00000: Loop [900]: write=0.01ms read=5.30ms(p/f=100%/100%)

連結したDynamixelが自動で探索され，見つかったDynamixelの初期設定が行われる．
初期化時の動作設定についてはReadMeのParametersの章を参照.

3. Dynamixelを制御

コマンドラインから指令する用の topic として /dyanmixel/command/... と /dyanmixel/shortcut が，プログラムから指令する用の topic として /dynamixel/commands/x が用意されている．

以下ではコマンドラインから指令を送る場合の例を示す．

例：ID:5のDynamixel Xシリーズのサーボを位置制御モードで角度を90degにする場合

/dynamixel/command/x/position_control topicにIDと角度を設定してpublish．

ros2 topic pub /dynamixel/command/x/position \
 dynamixel_handler/msg/DynamixelControlXPosition \
 "{id_list: [5], position_deg: [90], profile_vel_deg_s: [], profile_acc_deg_ss: []}" -1

[!note]
ID:5のDynamixelの制御モードは自動的に位置制御に変換される．

もしトルクが入っていなかった場合，/dynamixel/shortcut topicに "torque_on" コマンドとIDを設定してpublish. (ただし，デフォルトでは初期化時に自動でトルクONになっているため不要のはず)

ros2 topic pub /dynamixel/shortcut \
 dynamixel_handler/msg/DynamixelShortcut \
 "{command: 'torque_on', id_list: [5]}"

/dynamixel/shortcut topic command fieldに指定できる文字列．

torque_on / TON : 安全にトルクをenableにする．目標姿勢を現在姿勢へ一致させ，速度を0にする．
torque_off / TOFF: トルクをdisableにする．
clear_error / CE : ハードウェアエラー(ex. overload)をrebootによって解除する．
回転数の情報が喪失する問題を解消するために，homing offset用いて自動で補正する．
remove_id / RMID : 指定したIDのサーボを認識リストから削除する．
add_id / ADID : 指定したIDのサーボを認識リストに追加する．

4. Dynamixelの情報を取得

コマンドラインから確認する用の topic として /dynamixel/state/... と /dynamixel/debug topic が，プログラムから利用する用の topic として /dynamxiel/states が一定周期で pub され続けている．

以下ではコマンドラインから確認する例を示す．

例: ID:5とID:6のモータが接続している場合の現在値の確認

ros2 topic echo --flow-style /dynamixel/state/present # status, goal, gain, limit, error... など．

--- # 出力例
id_list: [5, 6] # 認識されているサーボのID
pwm_percent: [0.0, 0.0] # 現在のPWM値
current_ma: [0.0, -2.69] # 現在の電流値
velocity_deg_s: [0.1, 0.1] # 現在の各速度
position_deg: [89.91210937499999, -0.2636718750000023] # 現在の角度
vel_trajectory_deg_s: [0.0, 0.0] # 目標速度 みたいなもの
pos_trajectory_deg: [0.0, 0.0] # 目標角度 みたいなもの
temperature_degc: [0.0, 0.0] # 現在の温度
input_voltage_v: [0.0, 0.0] # 現在の入力電圧
---

上記は電流，速度，位置を読み込むように設定した場合なのでそれ以外の要素は初期値の0になっている．
read & pub される情報の選択についてはReadMeのParametersの章を参照.

例: ID:1, 6, 7, 8, 9のモータが接続している場合のデバック用情報の確認

ros2 topic echo --flow-style /dynamixel/debug

--- # 出力例
status: # /dynamixel/state/status と同じ
  id_list: [1, 6, 7, 8, 9] # 認識されているサーボのID
  torque: [true, true, true, true, true] # トルクがONかOFFか
  error: [false, false, false, false, false] # エラーが発生しているか
  ping: [true, true, true, true, true] # pingが通っているか
  mode: [velocity, cur_position, cur_position, cur_position, cur_position] # 制御モード
current_ma: # 現在の電流値と目標電流値
  present: [0.0, 3.0, 2.0, 9.0, 4.0] 
  goal: [0.0, 900.0, 910.0, 910.0, 910.0]
velocity_deg_s: # 現在の速度と目標速度
  present: [0.0, 0.0, 0.0, 1.374, 0.0]
  goal: [0.0, 439.68, 439.68, 439.68, 439.68]
position_deg: # 現在の角度と目標角度
  present: [1007.5781, -24.7852, 44.1211, -33.8379, -87.8906]
  goal: [1007.666, -24.7852, 44.1211, -33.8379, -87.8906]
---

トルクのオンオフ，制御モード，目標電流(実質的な最大電流)など，動作状況を確認するための情報が含まれる．だいたいこのトピックを監視してればOK．

[!NOTE]
dynamixelからのread方式は Sync Read または Fast Sync Read であり，すべてのIDから一斉にreadするようになっている．
ros param use/fast_read が falseの場合は Sync Read が，true の場合は Fast Sync Read が用いられる．
それぞれの違いは公式の動画を参照されたし．

[!NOTE]
複数のアドレスの情報を一括でreadするか，分割でreadするかは，ros param use/split_read によって変更できる．
分割でreadする場合は，読み込む情報の数分だけreadに時間がかかるので注意．

まとめ

以下が開発したパッケージの特徴のまとめです．

Dynamixel制御に特化した最小単位のパッケージである
ROSトピックのみで制御できるシンプルなインターフェースを持つ
物理量ベースでの直観的なやり取りが可能
高速で安定したRead/Writeが可能
開発の手間を減らす便利機能を多数実装

Dynamixel搭載の実機ロボットと上位の制御用ROSノードとの仲介を担うノードとして作成しました．
公式SDKなどを使って自分で実装すると結構面倒な機能も，このパッケージを使うと簡単に利用できると思います．具体的には電流制限付きの位置制御とか，ゲインの調整とかハードウェアエラーの解消とか．

今後，もっと具体的な使用法について別の記事にまとめていこうと思います．

追記

後半の Topic やら Parameter やらはだらだらと長いだけなので削除しました．
ReadMeに乗ってるのでそちらを参照ください．

ROS 2Advent Calendar 2024

Day 24

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up