TOPPERS/箱庭 単体ロボット向けシミュレータのロボット動力学モデルを解説

#この記事の内容
TOPPERS/箱庭 単体ロボット向けシミュレータのロボット(HackEV)のモデルは，Unityで作られていますが，このロボットの運動力学的なところが，どういう考え方で，どうやって作られているのか解説したいと思います．

なお，HackEV の Unity モデルはこちらで公開しているものです．

https://github.com/toppers/hakoniwa-Unity-HackEV/releases/tag/v2.1-practice

パッケージ名：single-robot-HackEV-practice.unitypackage
モデル配置場所：Assets/Prehabs

HackEVのモデルは，RoboModel です．
HackEVの動力学モデル説明用のモデルは，RoboModelSkelton です．

#HackEVのモデル化について
HackEVの実物をシミュレーション環境上で動かそうと思った場合，そのロボットのどの部分に着目してシミュレーションするか？ということを考える必要があります．

つまり，シミュレーションで再現したいことを抽出して，目的にかなったモデル(動力学モデル)を決める必要があります．

まず，箱庭としてはシンプルさ重視で，ロボット動作として再現したい点は以下としました．

* ライントレースできる左右モーター動作(両輪の速度差によるステアリング)
* アームの上下動作

再現精度は，以下としました．

* 実物の動力学のメカニズムと同じであれば良い(精度は問わない)
* 実物と完全一致はやらない(やれない)

本再現によるうれしさとしては，TOPPERS/EV3RT上で動作するロボットの制御アルゴリズムの妥当性確認ができる点です．

そのため，箱庭で確認したロボット制御コードはそのまま実物で同じ動作はしない可能性が高いです．実機で動作確認する場合は，パラメータチューニングが必要にはなりますが，それがアルゴリズムまでに影響を与えない範囲であれば，箱庭のシミュレーションの役割としては十分ではないかなと思っています．

#ロボット構成要素
実機のHackEVロボット構成要素としては，様々なパーツやセンサ，モーターなどがあり，部品点数は非常に多いと言えます．

これら１つ１つをすべてUnity上で再現することは非常に手間がかかります．しかし，シミュレーションで再現したいことの目的と照らし合わせて検討してみると，すべてを対応する必要はないと気づくことが出来ます．

先述のとおり，箱庭のシミュレータでは最小構成のHackEVモデルとしておりますので，動力学的な観点で再現すべきものは，下図の通り，**「アームのモーター」**と**「左右両輪のモーター」**だけです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/94b912dd-e184-7045-7671-93f057aa85b5.png)

これら以外の部品については，省略してまうことができますので，極論するとこうなります．これが今回の箱庭としてのHackEVの動力学モデルのエッセンスです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/2b31e27e-f7af-5942-a81b-6f71c045a267.png)

#Unityの動力学モデルとのマッピング
HackEVのモデル化ができましたので，これをUnityの動力学モデル/コライダとマッピングするとこうなります．
※これが正解というわけではないと思いますが，最小構成でマッピングするとこうなりました．

|パーツ|動力学モデル|コライダ|
|----|----|----|
|左右両輪のモーター|[Rigidbody](https://docs.unity3d.com/ja/2019.4/Manual/class-Rigidbody.html), [Hinge Joint](https://docs.unity3d.com/ja/2018.4/Manual/class-HingeJoint.html)|[Mesh Collider](https://docs.unity3d.com/ja/2018.4/Manual/class-MeshCollider.html)
|左右両輪を繋げる回転軸|[Rigidbody](https://docs.unity3d.com/ja/2019.4/Manual/class-Rigidbody.html)|なし
|アームモーターを設置するための部品|[Rigidbody](https://docs.unity3d.com/ja/2019.4/Manual/class-Rigidbody.html),  [Configurable Joint](https://docs.unity3d.com/ja/2018.3/Manual/class-ConfigurableJoint.html)|なし
|アームのモーター|[Rigidbody](https://docs.unity3d.com/ja/2019.4/Manual/class-Rigidbody.html), [Hinge Joint](https://docs.unity3d.com/ja/2018.4/Manual/class-HingeJoint.html)，[Fixed Joint](https://docs.unity3d.com/ja/2019.4/Manual/class-FixedJoint.html)|なし
|アームのモーターの回転軸|[Rigidbody](https://docs.unity3d.com/ja/2019.4/Manual/class-Rigidbody.html)|なし
|アーム|なし|[Box Collider](https://docs.unity3d.com/ja/2018.4/Manual/class-BoxCollider.html)

簡単に説明しますと，すべてのパーツに割当たっている Rigidbody，これは Unity 上で剛体力学系の運動をさせる場合に必ず割り当てるもので，質量や重力等の設定ができます．現状はすべてデフォルト値(1kg)を設定しています．

次に xxx Joint についてですが，これらは Rigidbody を割り当てたモデル同士を繋げるための動力学モデルです．運動特性に応じて様々なバリエーションがあり，HackEVのモーター動作にあわせて適切なものを割り当てててみました．詳細は後述します．

最後にコライダについてですが，これは Unity 上のモデル間の衝突を扱うためのモデルです．今回のケースですと，左右両輪のモーターが重力の影響をうけますから，コライダを付けないと，床をすり抜けて落ちてしまいます．アームについては，モノを運べるようにするために取り付けています．コライダも形状に応じた種類がいくつかありますので，形状に合わせて適切と思えるものを割り当てています．

##左右両輪のモーターと回転軸
左右両輪のタイヤは，ある軸を中心に回転運動させるための動力学モデルである Hinge Joint を割り当てています．ここで左右両輪のタイヤはそれぞれ独立して運動してしまいますと，タイヤがロボットから離れていってしまいますので，左右両輪の回転軸となるモデルを作って繋げています．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/53a7c08b-cdc4-7d4a-2ed3-9c3cbf9026a5.png)

ちなみに，左右両輪のモーターの回転速度が同じ場合はまっすぐ進みます．一方で，回転速度が偏りますと，偏った方向に曲がります．

##アームモーターを設置するための部品
左右両輪のモーターは床面と接触していますので，落ちることはないのですが，アーム側は接地面がないので，ロボット内のどこかの部品に設置させる必要があります．

ただし，左右両輪のモーターおよび回転軸は，モーターを動かすと回転してしまいますので，これらのモデルとアームをがっちりと固定してしまうと，アームもそれと連動して回転してしまうことになります．

ですので，ロボット内の中心辺りに，モーター回転の影響を受けず，重力の影響で落ちない，かつ，ロボット移動と連動して移動するモデルが必要と考えました．このモデルとアームを固定化できれば，アームもロボットの移動して動くことができるというわけです．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/94657121-03b9-a4c8-6380-8024f8fb3c82.png)

上図のように，回転軸の中心点にそのモデルを置いています．このモデルは回転軸と Configurable Joint でつなげています．Configurable Joint は様々な用途に対応できるように固定化させる方向等をパラメータで設定可能になっています．今回の場合は，回転軸方向には回転しないように設定をしています．


##アームのモーターと回転軸およびアーム
アームのモーターと回転軸は，先述の左右両輪のモーターと考え方は基本的に同じで，Hinge Joint でモーターと回転軸を繋げて回しています．ただし，アームの回転角度には制限がありますので，Hinge Joint のパラメータである Limit で最大角度と最小角度設定をしています．また，アームモーターは Fixed Joint を用いて，「アームモーターを設置するための部品」と位置の固定化をしています．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/ac575e4b-75a2-ed95-b5d0-03692d28098c.png)

一方で，左右のアームについては剛体力学モデルとせずに，コライダだけ割り当てて，アームモーターの回転と連動して移動させるだけにしました．剛体力学モデルを割り当てて Fixed Joint でつなげる方法もありますが，シンプルに作りたかったので，省略しました．

#デモ
では，実際にこのモデルを動かしてみましょう．
※見栄えがするように，センサ等もつけていますが，動力学モデルは先述のとおりのものです．

![demo_skelton.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/32d77de9-db36-e389-fd19-0abde082193f.gif)


次に，この動力学モデルをそのまま HackEV に割り当ててみたものが以下になります．

![demo_hackev.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/40ad3cd6-320b-8da8-3563-5250d1087fd9.gif)

車輪の直径が先ほどのものより大きいので進み幅長くなりますが，基本的な動きは同じです．
見た目は全然違いますが，物理的な動き方はまったく同じルールに基づいています．

もし，実機との違いが気になってどうしようもなくなった場合は，このエッセンスとなるモデルに立ち戻って，どこがダメなのか，どこを追加すべきか/変更すべきかを検討していけばよいのではないかなと思います．