プリメイドAIをROS対応させた話
この記事はROSアドベントカレンダー 21日目の記事です。投稿が大幅に遅れてしまいました。
さて、プリメイドAIというロボットをご存知でしょうか？ 一部の界隈で流行った2万円で買えたホビーロボットです。基本的にサーボを取るために使われるものなのですが先駆者の方々がプロトコルを解析していたのでROS対応させてみました。
作業内容はざっとこんなものです。
- URDF 作成
- ros_control 対応
- gazebo plugin 作成
- moveit! 対応
だいたいのROS化フルセットですね。趣味でやる程度ならちょうどいい負荷量です。本当は歩行やファームウェアハックまで手を伸ばしたかったのですが、年末までの作業はここまでということで。
動作環境
今回の作業環境は以下を対象とします。
- Ubuntu 18.04
- ROS melodic
- gazebo 9.0.0
環境整備
とりあえず全てのレポジトリをcloneして環境を整備します。今回は簡単に動作確認ができるようにサンプルワークスペースを用意しました。
$ source /opt/ros/melodic/setup.bash
$ mkdir -p premaidai_ws/src
$ cd premaidai_ws
$ catkin init
$ wget https://raw.githubusercontent.com/chikuta/premaidai_ws/qiita-201912/premaidai_ws.rosinstall -O .rosinstall
$ rosinstall .
$ rosdep install --from-paths src --ignore-src -y
$ catkin build
Bluetooth設定と接続方法
プリメイドAIの電源をつけたあとに以下のコマンドでBD Adressを取得します。プリメイドAIのBlueToothは RNBT-XXXX と認識されるので、このBD Addressをメモします。ここでは 00:06:66:84:XX:XX として表記します。
$ hcitool scan
Scanning ...
00:06:66:84:XX:XX RNBT-XXXX
次に接続時に使用するチャンネルを取得します。出力される RFCOMM の Channel をメモします。環境によって違いが出ると思うので以後 ${CHANNEL} と表記します。
$ sdptool records 00:06:66:84:XX:XX
Service Name: RNI-SPP
Service RecHandle: 0x10000
Service Class ID List:
"Serial Port" (0x1101)
Protocol Descriptor List:
"L2CAP" (0x0100)
"RFCOMM" (0x0003)
Channel: 1
Language Base Attr List:
code_ISO639: 0x656e
encoding: 0x6a
base_offset: 0x100
/etc/bluetooth/rfcomm.conf に以下を記述して再起動します。
rfcomm0 {
bind yes;
device 00:06:66:84:XX:XX;
channel ${CHANNEL};
comment "serial port";
}
以下のコマンドを入力してプリメイドAIを接続します。
$ sudo rfcomm release ${CHANNEL}
$ sudo rfcomm bind rfcomm0 00:06:66:84:XX:XX ${CHANNEL}
この場合、 /dev/rfcomm0 がプリメイドAIと接続がされたシリアルポートになります。
rviz上のプリメイドAIモデルを動かす
公開しているプリメイドAIのURDFモデルを使用して画面に表示されたものを動かしてみましょう。ここで使用したモデルは kuroiwasiさんが公開したモデルをROS対応させたものです。公開されているモデルを各パーツごとに分解し、パーツの重量を基にイナーシャを計算してURDFに記述しています。イナーシャの計算方法は こちらのサイトを参考にして計算しています。
$ cd premaidai_ws
$ source devel/setup.bash
$ roslaunch premaidai_description display.launch
とりあえずプリメイドAIのURDFを作ったのでおいておきますね。 関節角のリミット設定ができていないから週末までにやる予定。https://t.co/I4NecPFsG5 pic.twitter.com/7QO3DMm24n
— chikuta (@chikuta) July 24, 2019
rviz経由でプリメイドAIを動かす
次にrvizで実機を動かしてみます。通信プロトコルを基にプリメイドAI用の ros_controller を実装したので、Bluetooth設定と接続方法 に従って接続設定を行った後に以下のコマンドを実行してください。
$ cd premaidai_ws
$ source devel/setup.bash
$ roslaunch premaidai_controller rviz_control.launch serial_port:=/dev/frcomm0
プリメイドAIのC++コントローラーを追加。これぐらいが無改変ファームの性能限界かな。JointStateを取得しないなら20Hzでるぐらい。動画は10Hzで目標値設定。https://t.co/tgToOzPZ6y pic.twitter.com/YGsUsrwXmo
— chikuta (@chikuta) November 17, 2019
物理シュミレータでプリメイドAIを動かす
ROSの物理シュミレータgazeboを用いてプリメイドAIを動かしてみましょう。gazeboを使って物理シミュレーションをするにあたり、gazeboプラグインを作成する必要があります。drcsim_gazebo_pluginあたりを参考にして実装しています。gazebo自体が後方互換性を考えずにAPIを更新してくるので、適宜ソースコードを追いながら実装することがおすすめです。
$ cd premaidai_ws
$ source devel/setup.bash
$ roslaunch premaidai_gazebo rviz_control.launch
とりあえず rviz と Gazebo を使ったサンプルを作成完了。適当に公開しておきますね。 #プリメイドAI https://t.co/B6xAsFjDLL pic.twitter.com/sLMaYqYI5Y
— chikuta (@chikuta) August 28, 2019
moveit経由でプリメイドAIを動かす
ROSのプランニングフレームワークである moveit を用いてプリメイドAIを動かしてみます。MoveIt! Setup assistantを使って設定ファイルを作成していきます。
$ cd premaidai_ws
$ source devel/setup.bash
$ roslaunch premaidai_controller ros_control.launch serial_port:=/dev/frcomm0
プリメイドAIとmoveit! pic.twitter.com/TsGMy2ziDj
— chikuta (@chikuta) October 27, 2019
プリメイドAIとmoveit。レスポンス遅すぎて使い物にならんな。目標位置はランダムで生成。 pic.twitter.com/TnUktbxGoj
— chikuta (@chikuta) November 18, 2019
思ったよりも上手く動いていませんね。
今後の課題
さて、ここまでやってみてわかった問題点ですが、プリメイドAIのデフォルトファームウェアではコマンドの送受信周期が 20Hz 程度しか保証できないことです。この通信遅延はmaster-slaveで動かす場合は致命的なのでカスタムファームウェアを作る必要性が発生します。幸い、STM32-Arduino-ICSservoに搭載されたマイコンのファームウェアを書き換える方法を公開している方がいらっしゃるのでこれを参考にして作業をするとよさそうです。自分が開発したカスタムファームウェアもそのうち公開できるかと。