ししかわです。
社員研修の一環で二足歩行ロボットを作って、競技会「Humanoid Autonomous Challenge（HAC）」に参加します。
M5Stackの画像認識モジュール「M5Stack UnitV2」をスタンドアロン（単体）で使って、ロボットを制御しよう！というマニアックな計画を進めています。
今回はUnitV2からMAX-E1を動かしてみます。shotaさんが作成されているMAX-E1制御ライブラリ（max_e1_lib）を使います。このライブラリは内部でDynamixelSDKを使っています。ソフトウェアライブラリ、ハードウェア間の依存関係を図示すると次のとおりです。本記事では図中緑枠の部分を扱います。
DynamixelSDKをクロスコンパイルする
前回、PCでクロスコンパイルしたバイナリをUnitV2に転送して動かす方法について紹介しました。今回は「MAX-E1のライブラリ」「DynamixelSDK」そして「それらを使うサンプルコードのバイナリ」の3つをクロスコンパイルして転送します。まずはDynamixelSDKです。
DynamixelSDKはROBOTISのサーボモータ「Dynamixel」と通信・制御するためのSDKです。サーボモータの他にもDYNAMIXEL Protocolを実装した制御モジュールとの通信が可能です。MAX-E1ライブラリはDynamixelSDKを使って制御モジュールCM-550と通信することでロボットを動かしています。
pythonやC/C++、javaなどの様々な言語バインディングが同梱されていますが、今回はC++を使います。
DynamixelSDKは、RaspberryPiなどのシングルボードコンピュータ向けのビルドが可能です。UnitV2もRaspberryPiと同じアーキテクチャなのでビルド設定が使い回せます。
まずDynamixelSDKを取得します
git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git
Makefileのあるディレクトリに移動してmakeを実行します。
cd DynamixelSDK/c++/build/linux_sbc/ make make install
同じディレクトリにlibdxl_sbc_cpp.soというライブラリができます。これをあとでリンクさせます。
MAX-E1ライブラリをクロスコンパイルする
続いてmax_e1_libです。MAX-E1をC++で制御できます。shotaさんが二足キット研修のために開発しています。UnitV2で動かすために少し修正が必要なのでforkしてきます。
前回少しだけ触れたCMakeをここでも使います。armv7向けのクロスコンパイル設定を付け足す必要があります。
CMakeはクロスコンパイルに対応しています。プラットフォーム毎に異なる設定をツールチェインファイルに記述して、ビルド時に指定すればOKです。
arm-toolchain.cmakeを作成し、設定を足していきます。CMAKE_C_COMPILERなどの変数を設定することで、コンパイルに使うツールを差し替えます。使い回しが聞くようにターゲットトリプル（arm-linux-gnueabihf-）を変数にしています。
set(TRIPLET "arm-linux-gnueabihf-")
SET(CMAKE_SYSTEM_NAME Linux)
SET(CMAKE_C_COMPILER ${TRIPLET}gcc)
SET(CMAKE_CXX_COMPILER ${TRIPLET}g++)
SET(CMAKE_LINKER ${TRIPLET}ld)
SET(CMAKE_AR ${TRIPLET}ar)
SET(CMAKE_RANLIB ${TRIPLET}ranlib)
SET(CMAKE_AS ${TRIPLET}as)
SET(CMAKE_NM ${TRIPLET}nm)
SET(CMAKE_OBJDUMP ${TRIPLET}objdump)
その他、依存するDynamixelSDKのライブラリ名が変わっているので修正などします（すべての変更点はコミットログを参照）。
修正したらcmakeコマンドでビルドします。このときDCMAKE_TOOLCHAIN_FILEフラグを渡してツールチェインファイルの場所を指定します。
cmake -B build -DCMAKE_TOOLCHAIN_FILE=../arm-toolchain.cmake cmake --build build
これでlibmax_e1.soというライブラリができます。
同様の手順でサンプルコード（motions_example）もコンパイルしました。1点注意として、UnitV2の環境に合わせて接続するシリアルポートの名前を/dev/ttyS0に修正しています。
最後に、コンパイルしたバイナリをそれぞれUnitV2上に転送しておきます。
成果物が増えてきたので、本体に同梱のソースと混ざらないように作業用のディレクトリをSDカード上に作りました（/media/sdcard/bipedal）。
scp ./libmax_e1.so ./libdxl_sbc_cpp.so ./motions_example m5stack@unitv2.local:/media/sdcard/bipedal
UnitV2とMAX-E1を接続する
プログラムの準備が整ったらいよいよ実験です！
UnitV2とCM-550のUARTポート同士を接続します。M5StackのコネクタはGroveと呼ばれる規格です。今回はUnitV2に付属のGroveケーブルを使いますが、下図に示すようにケーブルの加工が必要です。
- 電源線の電圧がCM-550の出力（~3.6V）とUnitV2の入力（5V）で異なるため、接続しない。
- UnitV2の電源をどこから取るかは後で決める。「スマホ用充電池を別途UnitV2に挿す」「MAX-E1からの入力電圧を昇圧する」などが考えられる。
- TXとRXをクロスさせる必要がある
なお、今回は簡単のために既存のケーブルを加工しましたが、一度挿したコンタクトを抜き差しするのは推奨しません。ちゃんと作るのであればハウジング、コンタクトピンを使って自作するのがよいです。
注意：UnitV2のTxとRxが逆だった
実験中にハマったので書いておきます。2021年9月現在のUnitV2ロットは「本体のラベルに書いてあるTX/RXの表記と、実際のピンアサインが逆」です。これに気づかず「なぜか通信できない…？」と3時間くらい試行錯誤しましたが、ロジックアナライザを使ってUnitV2の波形を確認したら発覚しました。
まあ、M5Stackではよくあることです。私は敬虔なM5Stack教徒なので、Twitterで公式アカウントにサクッと指摘したところ、1時間ほどで返信が。そのうち直るかと思います。
Ok, thanks!
— M5Stack (@M5Stack) September 4, 2021
ピンアサイン自体は合っているので、UnitV2とM5Stack CoreをGroveコネクタで繋ぐ分には問題になりませんが、ケーブルを1本ずつ接続するときは注意が必要です。
なぜ人類はTXとRXを逆に繋いでしまうのか
— ししかわ/Shinya Ishikawa (@meganetaaan) January 10, 2020
動かしてみた
UnitV2にログインしてMAX-E1のサンプルを実行します。
ところで、DynamixelSDKとMAX-E1ライブラリもサンプルと同じ場所にコピーしていました。サンプルコードの実行時に、これらのライブラリをバイナリと動的にリンクさせる必要があります。
各ライブラリをデフォルトのライブラリ探索パス（/usr/local/lib）に置いてもよいですが、環境変数LD_LIBRARY_PATHにライブラリがあるディレクトリを追加しても動かすことができます。
unitv2% sudo env LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/media/sdcard/bipedal ./motions_example
動かしてみた様子がこちらです。
M5Stack #UnitV2 をハックしてロボットを動かしました！#二足ロボ研修 pic.twitter.com/q3Zas0Tzzc
— ししかわ/Shinya Ishikawa (@meganetaaan) September 3, 2021
無事にロボットの動きが再生できました！
なお、UARTポートから通信する場合、UnitV2とCM-550間の通信はできていますが、UnitV2から直接各Dynamixelのサーボと通信することができませんでした。つまりCM-550に保存されたモーションは再生できますが、各関節の動きをUnitV2から直接変更できません。
何かしら通信をバイパスする設定がありそうですが、とりあえず元々用意されている前進、旋回などのモーションは問題なく実行できるので、このまま進めます。
全部まとめたリポジトリを作った
今回はMAX-E1のサンプルコードを動かしました。ここまでで一通りの依存ライブラリの機能が動くようになりました。これからは、前回試したUnitV2Frameworkと組み合わせて、HACのためのロボット制御アプリケーションを実装していきます。新たにリポジトリを作成しました。
名前はHAC-chan（ﾊｯｸﾁｬﾝ）（仮）です！
MAX-E1とUnitV2Frameworkはgitのサブモジュールとして取得し、リポジトリ内のバージョン管理に含めます。これはﾊｯｸﾁｬﾝの開発に合わせてライブラリ側のコードも修正する可能性が高いからです。一方、DynamixelSDKやOpenCVなどに手を入れる可能性は低いので、開発環境にあらかじめインストールしておく前提としています（Dockerコンテナを使ってインストールし、バージョンは固定します）。依存関係を図示すると次のとおりです。
以上です。長い準備期間が終わり、次回からいよいよロボットの様々な機能を作り込んでいきます。考えることは山積みですが少しずつでも進捗を出せるようがんばります！
