Hololens2 でロボットアームを動かしてみました.
はじめに
こんにちは。昨年はコロナウイルス対策をテーマに Mixed Reality を使って非接触で遊ぶ記事を書かせていただきました飯田です。
在宅勤務が長くなるとたまに外に出るのが怖いですね。まだまだ油断できない状況ではありますが、今後ワクチンの接種が進むにつれ街に出て働く機会も増えてくると思われます。
とはいえ世の中は変わってしまいました。マスクや手指の消毒はもはや人類の義務です。コロナ禍以前と同じような生活様式が戻ってくる日はもう来ません。たぶん。というわけで AR/MR を使い非接触ひきこもり生活を続けることを私はまだあきらめてはいません。
ロボを動かしたい
今回のテーマはロボです。結局のところ私たちはガ○ダムを動かすことを目標に技術者になったわけですから、そのあたりに転がってるロボット如きは鼻歌まじりに動かせないとプログラマやってる意味ないじゃないですか。今回はそういう話です。
ロボットの操縦と言えば、おじさん世代が大好きな昭和ガ○ダムはダサい操縦桿みたいなので操縦してました。しかしいま時代は令和。コロナ対策的にこれはない。乗り降りの都度操縦桿のアルコール消毒は必須になるでしょうし、そもそも重機やゲーセンの棒みたいなのをガチャガチャする UI は古くさい。今だとせめてバーチャルな方法で操作したいものです。
そこでお手元に転がってる Hololens2 です。これには手指の姿勢をトラッキングする能力があるので、Hololens2 を使ったバーチャルな非接触操作でリアルのロボットを動かしてみます。
ロボット（本物）を用意する
とりあえずロボット買ってきます。いきなり二足歩行とか目指すのはハードルが高いので、まずはアームから。迫力出すためには適度に大きくてなるべくお安い、適当なロボットアーム をネット通販で買いました。
米国の STEM 教育向けのやつっぽい。IT 先進国は楽しそうですね。
アームの仕様
サーボ（については記事末尾に雑な説明あり）が５つ付いてて、土台のほうから、土台の回転（１）、肩（２）、肘（３）、手首（４）、カニばさみ（５）を動かせます。
２番３番のサーボが高トルクタイプ、１番４番５番が高速タイプです。サーボの制御方法は PWM か独自のシリアルコマンド。ついでにサーボコントローラーボードっぽいものがついてますが、このキットに付いてるのはあまりインテリジェントなものではありません。コントローラーボード側ではサーボが繋がったバスの調停や同期などは基本的にやってないようです。サーボの生通信を外部に垂れ流すのとモーターに電源を供給することに特化した、頭の悪いボードです。
私がこの微妙なアームキットを選んだのは、XBee ↓ を直結できるというところが気に入ったからです。
XBeeってのは、シリアル通信対応の機器に後付けするだけで簡単に無線化することができる便利なボードです。Hololens2 は WiFi と Bluetooth 語を話せますので、サーボコントローラのシリアルにXBeeを繋げば、外付けのマイコンボードの助けを借りずに Hololens2 から直接サーボを制御できるんです。 ようするに下図のような構成にできるということ。
いろいろ端折って今すぐ目の前のアームを動かしたいという事情もあり、今回はこういう簡単構成で、Hololens2 で直接サーボの制御することにします。 通信部分のレイテンシが気になりますがまずは動かしてみて判断です。
さっそくアームを組み立てて結線・設定します。足がないと倒れるのでダイソーで買ったキッチンラックとアームを紐でくくりつけて転ばないようにして、さっそく動かしてみます。
Hololens2でアーム操作
今回利用するツールは Unity2019.4.x + MRTK 2.5.x。MRTK 2.5 から Experimental 機能として追加されたバーチャルジョイスティックと、ハンドトラッキング（手の位置や姿勢を追跡する）を使って動くところまで確認。
コードは特に面白くないので省略。
ジョイスティックで、各関節ごとの角度を入力してみました。非接触で動いてるし、とりあえず要件はみたしてる。けどなんだろうこのコレジャナイ感は。 ガンダムってより、いにしえの鉄人２８号がこんな操縦方法だったと聞いたことがある。バーチャルな操作って言いつつこれじゃ結局は棒ガチャガチャ、重機の操作みたいで楽しくない。ハンドトラッキングの無駄遣いな気が。
直感的な操作をもとめて
たとえば我々がボールを拾うとします。手を動かすときの意識の流れはただ「ボールを拾おう」とか「ポチに投げてやろう」とか、そんなものです。手は勝手に動く。
実際には、肘や肩をボールを拾うのに適した角度にするために、目で見た情報をもとに脳が筋肉に信号を出して力を加え各関節を動かし、目的の角度に近づくと止めるため逆方向の筋肉に信号を出して…といったことをしてるはず。しかし我々は「肘の関節を３０度にするため加減速して、肩の関節を８０度にするため加減速する」のようなことを意識することはありません。 バーチャルジョイスティックで関節の角度指定してロボットアームを動かすのは、人間が腕を動かすときの実際の意識とかけはなれてます。
…ハンドトラッキングの使い方を間違ってました。ジョイスティックを操作して間接的にアームを動かすのではなく、手の位置をアームの動きに直接的に変換できれば、直感的に操作できるんじゃないだろうか。
キャプチャした手の動きをアームの動きにマッピングする具体的な方法を考えてみる。
【案１】 人間の手の各関節の動きをそのままロボットアームの関節の動きに変換する
最初に思いつくのはこれ。ですが人間の腕とロボットの腕は形状も可動域も違います。人はひじ関節を外側に曲げたりできないし、ロボはひじ関節をひねったりできません。
【案２】 人間の手の先の位置を、ロボットアーム先端の位置と連動させる
次に思いついたのはこれ。指先で目標となる実空間上の位置や姿勢を設定し、その姿勢を実現するロボの関節角度を逆計算する。IK (Inverse Kinematics)って技術になります。以前の記事で Unity ちゃんのモデルを動かすときに使ったやつです。 実際にやってみるとわかるのですが、常時手先の動きにロボの動きを追随させると手をずっと空中に浮かした姿勢をキープする必要があってつらい。あと、頭がかゆいときとか手を不用意に大きく動かすとアームが意図せず連動して事故ります。
【案３】 ロボットアームをバーチャルなボールと連動させる。バーチャルなボールを掴んで動かす
これなら、意識的にボールを握ってるときだけ連動するので安全ではなかろうか？
今回は３番目の案でやってみたいと思います。
ロボットを用意する（バーチャル）
アームの各関節は、全方向自由自在に動くわけではありません。動く方向と動かない方向、動かせる角度があります。関節の可動域は
- 個別の関節の取付角や向きによる制約
- 複数の関節の組み合わさった制約
といった現実世界の制約を受けます。要件次第では物理的な床などの障害物との干渉も考慮する必要もあるかも。ロボット工学の教科書ではそのあたりの IK の計算方法は定番ネタのようです。ですが教科書見て自前で計算するのは面倒くさいしやりたくない。そもそも教科書持ってない。
ゲームプログラミングではロボットを実装するケースも多いからか、Unity の場合 Humanoid（人型）のモデルに特化した IK 機能は標準で付いています。ただこれは細かい融通がききません。凝ったことをしたい場合は FinalIK っていう$99ドル程の定番アセットを買ってくれば関節角度の逆算を簡単にできる、らしい。高機能っぽいし自前実装する手間や節約できる時間を考えれば絶対お得と思われる、とはいえフルプライスのゲームが買えるお値段。個人で買うには高いし、有償のアセットをカード払いで買って会社に請求書回すのも面倒くさい。
調べると、Unity 2018.4～ で使える Animation Rigging ってパッケージを使えば、非 Humanoid でも設定するだけで簡単に IK が使えるらしいので、今回はそれ使ってみます。
Unity で IK を使うには、動かす対象の 3D モデルが必要です。 現実のアームは曲げられる方向や稼働する角度の範囲に制約があります。たとえば床や、アーム自身のパーツといった障害物の存在を無視してモーターを動かすことはできません。サーボってのは設定された角度になるまで力を加え続けるものなので、ムリな角度を要求しつづけると、最悪、 燃 え あ が り ま す 。
ある関節を指定の角度にできるかどうかはほかの関節の角度にも依存します。アームの3Dモデルはそういった物理的な制約を表現するために使えます。
まず手始めに、アームを構成する部品のCADデータを作ります。実はこのサーボモーター、外装のCADデータがメーカーから公開されてたりするのですが、設計用のデータは外装裏面のリブ形状などまで細かく入ってます。重たすぎ。 ぐりぐり動かす用途で Unity にそのままインポートして使うのはオススメしません。 Hololens2 で動かすためのモデルは頂点数は極力少ないほうがいい。精度もいらず適当にそれっぽい形状と寸法になってれば充分なので、現物あわせで作ってみます。 例によってお金がないので、 OpenSCAD っていう無償(GPLv2)の 3D CAD ソフトを使ってやってみます。
こんな感じで。
このツール、操作性は独特です。スクリプトを書いて立体形状を作っていく、っていうのに最初はちょっと引くかもですがいきなりBlenderで作るより100倍マシ。 これはマインクラフトだマインクラフトだ…と1000回ぐらい唱えるとだんだん楽しくなってきますのでオススメ。 全体的にそれっぽい形状ができたら関節の部分で切り分けます。下図のようなパーツのかたまりごとに、STLファイルとしてエクスポートします。
次に各パーツを組み合わせてアームの形状を組み立てて、骨格(Bone)を入れます。この作業には Blender ってソフトを使ってみました。無償。 先ほど作った各パーツのSTLをインポートしてBoneを定義して、各ボーンと部品を紐づけていくわけですが… Blender はこれまた独特な操作性ですよね。ロボットの可動モデルを Blender で作っていく手順をステップバイステップで紹介するには余白が足りない感じなので詳細手順省略。
こんな感じにモデルを作ってみました。
armature (要するにBone) の定義を作って、各々の Bone を、3D CAD からインポートしたそれぞれの形状と紐づけます。そして各関節に可動域制約を設定していきます。 先端のカニ挟み部分は IK とは違う手段で動かすつもりなので関節入れてません。変な形ですが気にしないでください。
Blender 上で IK のシミュレーションができるので試してみます。こんな感じに自然に動くようになればOK。
うん、この感じでいいかな。Unityに取り込むために.fbx形式でエクスポートします。エクスポート設定は基本的に全部入りで OK。
Unityでロボットを動かす
まずは Unity プロジェクトに Mixed Reality Toolkit 関連のもろもろのパッケージを入れてセットアップします。手順の詳細は MRTK のドキュメントを参照してください。MRTK は頻繁にバージョンアップされてて初期設定の方法なども変わることがあり、ここで細かい手順を書くよりも最新ドキュメントあたってもらうのが確実だからです。けっして面倒なわけでは。
適当なプログラムをビルドして Hololens2 で実行して OK なら、次はプロジェクトに Animation Rigging パッケージをインポートします。汎用の IK を使うためのパッケージになります。
なお、Unity 2019.4 のパッケージマネージャで入る Animation Rigging 0.2.7 preview パッケージは Burst コンパイラ 1.1.1 に依存してます。そのままでは Hololens2 向けにビルドできない不具合があります。Burst 1.2以降で修正されていますので、新しいバージョンに手動で入れ替えてください。この記事では 1.4 を使ってます。
作成した .fbx ファイルを、エクスプローラから Unity プロジェクトの assetウインドウにドラッグ＆ドロップして、Inspector で import の細かい設定をしてから Import Constraints にチェックを入れて取り込みます。アームの親になる空のゲームオブジェクトを作成して、いまインポートしたアームをドラッグ＆ドロップでシーンに配置して、アームの部品形状ごとに Material で色つけたりなんかしてわかりやすくすると良いでしょう。
ターゲットをつかんで動かす
アームの先端に追跡させるバーチャルなターゲットを手でつかんで動かすためには、ターゲットとなる3Dオブジェクト、ここでは適当な球体に、Collider と MRTK の ObjectManipulator とNearInteractionGrabbable を追加してごにょごにょ設定すればOK。コードの記述は要りません。 アームが生えてる床面も同じようにすれば掴んで動かせるようになります。Mesh Collider 付けて Constraint Manager に Rotation Axis Constraint 追加して Y軸以外の回転を禁止しておくと、片手で掴んでの操作が安定します。わかりづらくてすいません。
さっそくIKで動かしてみます。こんな感じにTwo Bone IKとかつけて。
…みたんですが、どうもTwo Bone IK に指定した armature に設定したモデル上の制約がいろいろ無視されてて、Blender でシミュレーションしたのと同じように動かない。使い方は間違ってないと思うんだけど🥺
Unity 側でもいろいろ Constraint 付けたりして関節の動きを制限しようとすると、今度はTwo Bone IKが意図した角度に動かないとか、なにかしら問題が起こる。~~Unityはやたらとクラッシュするし~~ どうしたものか。
- 案１：モデルの制約を無視しないIKのConstraintを作る
- 案２：Final IKを買う
- 案３：見なかったことにする
案１ですが、Unity の IK 使って楽したい！って話なのでそういう解決策は求めてません。案２はお金がないので却下。
で、いま作ってるのは別に売り物じゃないので、Unity 標準の Two Bone IK で、完璧ではないけどそれらしく動く感じになんとか設定して、とりあえず細かい挙動のあやしさはさっぱりと忘れることとします。Unity 2020.x だと Animation Rigging も新しいバージョンが使えるみたいだしそのうちきっと Blender と同じように動くようになる、と思う（もし真似される方は素直に$99出してFinal IK 買ってみてちゃんと動くか教えてください）。
めでたく IK が動くようになった(?)ので、各関節の回転軸方向の角度差成分を取り出して、サーボをその角度に設定します。サーボを操作するコードは Joystick での操作コードと特に変わりません。 アームの手首にあたる部分とカニばさみの部分については、ターゲットのボールの位置を追いかけさせただけでは動かせないので、IKではなくボール自体の回転（位置ではなく、ひねり）に直接マッピングして制御することとします。
物理的なロボットを動かす
で、Hololens2 実機で動かしてみたのがこちら。(音出ます)
ガ○ダムビルドファイターズ？なにそれ知らない。ほら、ボールだって赤いし！
実際に操作してみると、ジョイスティックよりははるかに動かしやすくて、たのしい。腕を振りまわす動きも容易でこれならバトルしようって気にもなります。
モノを掴んで持ち上げるのは、触覚（頭に生やすアイテムじゃなく、触った感覚）がないとやりづらいですね。手ごたえをフィードバックするデバイスがほしいです。
まとめ
難しかった点
ここでの記事にはしてませんが、以前 Hololens の視線制御でドローンを飛ばしたことがあります。ドローン飛ばすことに比べれば空を飛ばないロボットアームの制御なんてどう考えても簡単だろうと侮ってましたが、やってみると結構つまづきました。ドローンの場合はドローン側のマイコンが姿勢制御やらの責務をもってますので、アプリのプログラマがやるべきことはドローンを動かすためのベクトルを指定するだけですが、プリミティブな多関節アームの制御は自分でアクチュエータへの指示まで落とし込まなきゃならないので。
もし真似する人がいるなら、今回 XBee でお茶を濁した部分は面倒くさがらずに Arduino 等の処理能力のあるマイコンを使うことをお勧めします。 XBee 直結で制御する場合はサーボのフィードバック制御まで Hololens2 の責務になりますが、Hololens2 ←→ XBee ←→ サーボ をリアルタイムにターンアラウンドする構成だとレイテンシ高すぎでした。モニタリング項目を増やすと時間分解能が足りなくて動きがカクカクになります。 ちゃんとやるには XBee を無線マイコンに置き換えて、そこに Hololens2 から抽象的な操作コマンドを送り、サーボのモニタリングと制御は無線マイコンの責務でやると気持ちよく動かせるかと思います。プログラム的にも Hololens2 から物理的なサーボへの依存がなくなってすっきりするでしょう。
Hololens2 の可能性
IoT 事例でみかける、ただセンサー情報を集めてクラウドでビッグデータで、みたいなのは、単なるテレメトリの延長でしかなく何か物足りない気がしてます。 せっかく IoT でフィジカルな世界を扱えるのですから、実世界の情報を収集して、コンピュータでシミュレーションして現実にフィードバック、みたいな流れで、実世界に働きかける大きなループみたいなのを作っていくのが楽しくなるポイントかと。
Hololens ってのは、現実->デジタル->現実 のフローにおいて、現実をキャプチャした情報と外部センサからの情報をこねこねしてクラウドで処理しつつ結果を IoT デバイスに戻す、という流れを単体で完結できる可能性があるのでやっぱり楽しいですね。って毎回言ってる気がしますが。お仕事に活用する方法を上手くこじつけられないのは Hololens2 の咎ではありません。面白い利用法のアイデアを思いついたらぜひ私に教えてください。
補足説明：サーボモーターって？
角度を指定して回転できるモーターです。
ただし現実世界はゲームのように都合よくはありません。角度を指定して回転できるとは言ったが、指定の角度に回転できるとは言ってません。重力や慣性などの物理法則には従うしかない。
急に動けない、急に止まれない。障害物に当たると回せない。 このあたりまでの制約は Unity 等のゲームエンジンで容易にシミュレートできますが、メカの自重や姿勢、摩擦抵抗、モーターのトルクやギア比、消費電力や温度上昇などのあらゆる特性を厳密にシミュレートするのは難しいので、思い通り動かすには現物あわせでの工夫や試行錯誤は必要です。
一般的なサーボは、
- 角度の指定はパルス幅変調(PWM)やコマンド通信で行う
- 一定角度を保持するにも電力が必要（荷重がかかると電流UP↑）
- できないことを要求しつづけると過熱する
何も考えず角度指定するだけだと操作によっては壊れるか、あるいは保護装置が働いて脱力します。脱力すると機械の自重で落下するのでそれはそれで危険です。状況をモニタリングしながら使うとよいです。
たとえば、物を掴む場合は、
- 少しずつ角度指定してハサミを締めていく
- 電流が一定値を超えると対象物を掴んだと判断
- 僅かに角度を緩めて保持。電流が閾値を下回るとまた少し締める
- 温度が一定値を超えると長時間締めすぎなので掴んだものを降ろす
といった制御を行ったり。
アームを動かすスピードについても常にフルスピードで動かしてよいってわけではなく、サーボの動きの速さ、周辺の安全性などを考慮してチューニングが要ります。 生産ラインで産業用ロボットを動かすような既知の動きを繰り返す場合より、この手の安物サーボをインタラクティブに動かすほうが難しいかもしれません。