二足歩行ロボット研修二足歩行ロボット研修（kora編）
Dynamixel

二足歩行ロボット研修（kora編）[18] 自作基板でDYNAMIXEL Workbenchを使ってみる

二足歩行ロボット研修（kora編）[18] 自作基板でDYNAMIXEL Workbenchを使ってみる 二足歩行ロボット研修

こんにちは。koraです。

今回は、自作したRaspberry Pi用の拡張基板にXM540をつないで動かしてみます。

最初に第12回の記事で書いたとおりDYNAMIXEL SDKで動かしてみたのですが、問題ありませんでした。せっかくなので今回はDYNAMIXEL Workbenchを使うことにします。

DYNAMIXEL Workbenchについて

DYNAMIXELサーボモーターをROSやArduinoから簡単に利用可能にするライブラリです。ここではROSから動かしてみたいと思います。

DYNAMIXEL Workbenchのインストール

まずは、公式e-Manualを参考にインストールします。ROSとDYNAMIXEK SDKをインストールするように書かれていますがこれまでの記事でインストール済みなので、DYNAIXEL workbenchのみ追加します。

$ cd ~/catkin_ws/src
$ https://github.com/ROBOTIS-GIT/dynamixel-workbench.git
$ https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git

拡張基板用の変更

自作した拡張基板はROBOTIS公式のU2D2とは構成が異なるので、多少の修正が必要です。

C++ライブラリの変更

第12回の記事ではPythonモジュールに手を加えてRS485変換ICを使用できるようにしました。DYNAMIXEL WorkbenchはDYNAMIXEL SDKのC++ライブラリを使用するので、C++モジュールに手を加えます。~/catkin_ws/src/DynamixelSDK/ros/src/dynamixel_sdk/port_handler_linux.cpp b/ros/src/dynamixel_sdk/port_handler_linux.cpp というファイルを開いて、

int PortHandlerLinux::writePort(uint8_t *packet, int length)
{
  return write(socket_fd_, packet, length);
}

を次のように書き換えます。

int PortHandlerLinux::writePort(uint8_t *packet, int length)
{
  // set RTS pin high
  int buf = TIOCM_RTS;
  ioctl(socket_fd_, TIOCMBIC, &buf); // clear bit

  // write data
  int ret = write(socket_fd_, packet, length);

  // wait for TX
  int lsr;
  do
  {
    ioctl(socket_fd_, TIOCSERGETLSR, &lsr); // get line status register
  }
  while(!(lsr & TIOCSER_TEMT));

  // set RTS pin low
  ioctl(socket_fd_, TIOCMBIS, &buf); // set bit

  return ret;
}

この変更の後、catkin_makeを実行してビルドします。

launchファイルの変更

サーボと通信するデバイスファイルを指定する必要があります。U2D2を使う場合は”/dev/ttyUSB0″となりますが、ここではGPIOピンから直接シリアル通信するようにしていますので、”/dev/ttyAMA1″にします。~/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers/launch/dynamixel_controllers.launch を開いて、

<arg name="usb_port" default="/dev/ttyUSB0"></arg>

を

<arg name="usb_port" default="/dev/ttyAMA1"></arg>

と書き換えます。

動作確認

DYNAMIXELサーボを1つ繋いで動作確認をします。

動作確認

サーボをスキャンする

次のコマンドで接続されているDYNAMIXELサーボをスキャンします。

$ rosrun dynamixel_workbench_controllers find_dynamixel /dev/ttyAMA1

成功するとこのような出力が表示されます。ボーレート57600でID1のXM540を認識していることがわかります。

[ INFO] [1606356759.734734141]: Succeed to init(9600)
[ INFO] [1606356759.734850121]: Wait for scanning...
[ INFO] [1606356785.766038784]: Find 0 Dynamixels
[ INFO] [1606356785.766205245]: Succeed to init(57600)
[ INFO] [1606356785.766236429]: Wait for scanning...
[ INFO] [1606356804.485519878]: Find 1 Dynamixels
[ INFO] [1606356804.485661468]: id : 1, model name : XM540-W150
[ INFO] [1606356804.485923205]: Succeed to init(115200)
[ INFO] [1606356804.485999352]: Wait for scanning...
[ INFO] [1606356822.511591450]: Find 0 Dynamixels
[ INFO] [1606356822.511974852]: Succeed to init(1000000)
[ INFO] [1606356822.512099943]: Wait for scanning...

サーボを動かす

いよいよROSを使ってDYNAMIXELサーボを動かします。

サーボの設定ファイル

まずyamlファイルで接続するサーボを設定します。設定項目の詳細はe-Manualにあります。

basic.yamlというファイルが予めあるのですが、2つのサーボを繋がないと動かない設定なので、1つだけ繋いで動くようなファイルを作って、~/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers/config/test.yaml に保存します。

test:
  ID: 1
  Return_Delay_Time: 0

ちなみにReturn_Delay_Timeはサーボが返事を返すまでの待ち時間です。単位は2usです。

起動時にこのyamlファイルを読み込めるように、~/catkin_ws/src/dynamixel-workbench/dynamixel_workbench_controllers/launch/dynamixel_controllers.launch を編集します。

  <param name="dynamixel_info"          value="$(find dynamixel_workbench_controllers)/config/basic.yaml"/>

を

  <param name="dynamixel_info"          value="$(find dynamixel_workbench_controllers)/config/test.yaml"/>

と書き換えます。

ROSノードの起動

次のコマンドで、DYNAMIXELサーボを制御するROSノードが立ち上がります。

$ roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch

/dynamixel_workbench/dynamixel_command というサービスが使えるようになるので、リクエストを送ることでサーボを制御することができます。新たにROSノードを作ってサービスクライアントを立ち上げてもいいのですが、今回は動作確認なのでrqtからリクエストを送ります。

まずコマンドラインに

$ rqt

と入力し、rqtを立ち上げます。立ち上がったら図のようにメニューバーからService Callerを開きます。

Service Caller

次に、id、addr_name、valueをそれぞれ1、’Goal_Position’、2048と入力して、右上のCallボタンを押すと、サーボを0度の位置に動かすことができます。Goal_Positionには0 ~ 4095の値を指定でき、-180度 ~ 180度に相当します。

id、addr_name、valueを入力

サーボを0度の位置に動かす

その他

他にも、一連の動作をモーションとして指定したり、車輪のように連続回転させたり、といった使い方ができるようです。ここでは割愛しますが、詳細についてはe-Manualを参照してください。

タイトルとURLをコピーしました