1. 概要
Sainsmart 6dof arm の URDF(Unified Robot Description Format) file と、ROS 用 Driver を作成し、MoveIt(2) で制御するまでを、ソフトウェア中心に簡単にまとめました。
2. 構成
システム構成を簡単に説明しておきます。
RPi (Raspberry Pi 4B)は、ECU (Electronic Control Unit)のような位置付けです。つまり、センサー類から情報を取得し、Wheel や Arm を動かします。また、RPi は安価な為、 Wi-Fi が遅いので、Wi-Fi が高速な OpenWrt と Gigabit Ether で接続し、PC 上の ROS と Wi-Fi で接続しています。
Arm は、物理的には、RPi に USB で接続されていますが、制御は、PC2 の ROS2 上で動作している MoveIt2 から行っています。もちろん、RPi は ROS1 ですから、ros1_bridge を経由して /joint_states を送っています。
PC1 は何をしているのかと言うと、RTAB-Map(Real Time Appearance Based Mapping) と、物体認識・トラッキング処理（タイトル画像と動画の左下 Window を参照）を行っています。
これから作成するプログラムは、ROS2 用に書きたいところですが、ROS2 の MoveIt2 (Ubuntu 22.04 + Humble) は、バグが多すぎて、ちょっと厳しいなという印象でした。
3. URDF file の作成
Sainsmart 6dof arm は、意外と有名な arm ですが、なぜか ROS 用 Driver は存在せず、ROS で動かせる環境は、整っていないようでしたので、URDF(Unified Robot Description Format) file の作成から始めました。URDF file は、直感的に分かり易い構成でもありますが、初めは、単純な Link と Joint の組み合わせから、徐々に複雑な構造に組み上げて行くのが、近道だと感じました。
取り組み易い 6dof（Six Degrees of Freedom：6軸自由度）ARM の URDF は、こちらのウェブサイトを参考にさせて頂きました。ありがとうございます。
上記ウェブサイトの sixdofarm.urdf を参考に作成した、Sainsmart 6dof arm 用 URDF file は、私の GitHub に収めてあります。
Link のサイズは、実測値で作成しています。重さは、正確に出したい場合は、組み上げる前に量っておくか、分解して量り直す必要があるので、今回は量っていません。
inertia は、同じサイズ感の物体の値を、何となく入れてあります。今回の場合は、各 Link も小さいですし、位置制御のサーボモーターでゆっくり動かすので、実用上、問題にならないかと考えていますが、より正確な値を求める場合は、測り直してください。
Elbow Joint は、実機では、リンクを介して駆動するようになっていますが、今回の URDF では、リンクを省略し、Driver 側で吸収する造りにしています。同様に、Shoulder Joint も、回転方向を Driver 側で反転しています。
ROS1、ROS2 共に、sainsmart_description のような Package を作成して、その URDF ディレクトリに格納します。後でも述べていますが、ROS1 側でも robot_state_publisher を起動する為、両方に必要になります。また、MoveIt で使用する為に、moveit_setup_assistant で、sainsmart_moveit_config を作ると簡単です。他の Node で読み込めるように、.xacro ファイルも作成します。
GitHub の readme にも書いていますが、標準仕様の Sainsmart 6dof arm では、サーボモーターに SG90 を使用しています。SG90 は、Stall Torque が 1.8[kgf/cm] で、樹脂製ギアを使用している為、Stall Torque が高く、金属製ギアの MG90D と、MG92B に置き換えています。この URDF 上では、Mesh ファイルの都合で、MG90D と MG90S を使っています。
この Sainsmart 6dof arm は、パレタイジングロボットなので、床面の物を持ち上げて移動するには、丁度いいのですが、振り回すのは厳しい造りです。
4. ROS Driver の作成
Driver は、今回、ROS1 用に組んでいますが、必要に応じて、ROS2 への移行も容易なはずです。こちらも URDF file と同様に、私の GitHub に収めてあります。
使用する際は、ROS の Package を作成して、そのディレクトリに収めます。注意点は、GitHub の readme にも記載しましたが、arduino との通信速度を上げると不安定になる点です。
また、/joint_states を Subscribe して動く仕様にしているので、Fake Controller でも動かすことができます。
ROS1 の通信は、TCP/IP なので、基本的に通信の信頼性は保証されています。MoveIt 側と物理的に離れており、無線である Wi-Fi で通信する仕様上、ROS の通信を使用しなかったとしても、遅延や欠損のリスクは、あまり変わらないはずです。
5. MoveIt との接続
今回、Arm は ROS1、MoveIt(2) は ROS2 で動かしている為、プロトコルを変換する ros1_bridge を使用しています。
ROS1 と ROS2 は、/tf や /tf_static も変更されている為、ROS2 の出す /tf や /tf_static は、そのままでは使用できません。基本的に、ROS2 の MoveIt からは、/joint_states だけ ros1_bridge で送れればいいはずです。
ROS1 側では、PC1 で別途、 robot_state_publisher を起動しています。URDF も、ROS1 側と ROS2 側に別々に用意します。
以上で、ROS2 上の MoveIt(2) から、ROS1 上の Sainsmart 6dof arm を動かす環境が整うはずです。実際に動かしている実験動画は、こちらです。
左上の Window が、実機を映しているもの。右 Window が PC2/ROS2 上の MoveIt2、左下 Window が PC1/ROS1 上の RTAB-Map と物体認識・トラッキング画面です。
MoveIt2 の軌道計画の通りに、左上の実機と、左下のモデルが動いているのが分かります。
tf も、Arm の動きに合わせて計算されている為、Arm の End Effector に搭載している Depth Camera の映し出す Point Cloud も、Arm の動きに合わせてマップ上に映し出されています。
左下 Window のシリンダーは、認識した物体の位置と大きさ、文字は、認識結果(Cloth 0.55[m]、Package Goods 0.54[m])です。
以上