toioと同じように動くWHILL Model CR

#はじめに

toioを動かすと人が乗れる大きさのWHILL Model CRが動くやつを作りました。

どういうものかはこちらの動画をみてください。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/toio?src=hash&amp;ref_src=twsrc%5Etfw">#toio</a> に追従するやつ出来た！<a href="https://twitter.com/hashtag/WHILL?src=hash&amp;ref_src=twsrc%5Etfw">#WHILL</a><a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://t.co/JMLzQI70Fi">pic.twitter.com/JMLzQI70Fi</a></p>&mdash; Katsu Shun (@katsushun89) <a href="https://twitter.com/katsushun89/status/1152222886196703232?ref_src=twsrc%5Etfw">July 19, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

これをどうやって動かしているかを簡単ですがまとめました。

# toioとは
SONYが出しているCube型のおもちゃです。
![toio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/302503/e82a8841-18f6-3724-3e2c-7517baa58296.png)


技術仕様が公開された時に記事を書いたので詳しくはそちらを参照ください。

[toioのjavascriptライブラリが公開されたのでexample動かしてみた](https://qiita.com/KatsuShun89/items/5f86a9811565267f6b30)

# WHILL Model CRとは
人が乗れるパーソナルモビリティWHILL Model Cの、外部から制御出来る研究開発用のモデルがModel CRです。
![cr.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/302503/f8bd2632-2c80-405f-a749-d51feff49ee3.png)

# M5Stackとは
ESP32というWiFi/BLEを搭載したLCD,ボタン付きでモジュール追加するといろいろなI/Fを追加できる電子工作でよく使われるデバイスです。

![m5.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/302503/2d0f659c-4e47-ee8a-959e-268b8fc16692.jpeg)


こちらは以前Model CRをM5Stackで動かしたときの記事です。

[M5StackのJoystickでWHILL Model CRを動かしてみた](https://qiita.com/KatsuShun89/items/5d18ba24622d3536520a)

# システム構成
toioとModel CRは発売されている製品の仕様通りに制御するだけなので、開発は全てM5Stackのみで行っています。

toioの記事にある通り、toioは専用のマット上では位置(x,y)とz軸周りの回転角度が取れます。

この位置姿勢の初期値とゴール位置をM5Stackで設定して、それによって移動するための入力値を計算して、Model CR側に送ります。

その結果Model CRが動いた移動量はモーターの回転角からホイールオドメトリとして計算して自己位置を更新します。

![system.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/302503/e8ee40a3-e125-7855-4cd7-87198dbf421b.jpeg)

# toioの初期位置と目標位置の取得
toioはマット上で座標(x, y)とz軸周りの回転角度が取れますので、それを覚えるようにしました。

最初はデバッグ用にM5StackのAボタンを押すと、現在位置を初期位置としてリセットします。（そのとき同時にWHILLの現在の自己位置姿勢もリセットするようにしています。）

次にtoioをマット上で移動させてボタンBを押すと、そのときの現在位置をゴール位置として与えました。

またtoioから取得出来る座標位置は0 - 数百のオーダーでしたので、これをModel CRの移動量とオーダーを合わせるために、toio移動量:100 = Model CR移動量:1mとするように係数0.01をかけています。

```cpp:M5Stack_whill_toio_controller.ino
void checkPushButton(void)
{
  if(M5.BtnA.wasPressed()){
    setStartPosition();
    is_requested_track = false;
  }

  if(M5.BtnB.wasPressed()){
    if(setGoalPosition()) is_requested_track = true; 
  }
}

bool setStartPosition(void)
{
  tracker.resetArea();

  bool is_set_start = false;

  position_t position = convertPosition(position_id);
  Serial.println("reset pos");
  Serial.printf("%f %f %f\n", position.x, position.y, position.theta);
  if(tracker.setStartPosition(position)){
    is_set_start = true;
    odom.reset(position);
  }
  return is_set_start;
}

bool setGoalPosition(void)
{
  bool is_set_goal = false;

  if(position_id.x_cube_center != 0 || 
     position_id.y_cube_center != 0 ||
     position_id.angle_cube_center != 0){
    position_t position = convertPosition(position_id);
    
    if(tracker.setGoalPosition(position)){
      is_set_goal = true;
    }
  }
  return is_set_goal;
}
```


# 移動目標軌跡の計算
これはPythonRoboticsのmove to poseという初期位置とゴール位置を指定するとその間の軌跡（Pose）を算出するアルゴリズムを使わせてもらいました。(Path tracking)

これを見つけられたので結構シンプルにやりたいことが実現出来ました。
Python Roboticsはいろいろなアルゴリズムがあるので大変参考になります。

[PythonRobotics move-to-a-pose-control](https://github.com/AtsushiSakai/PythonRobotics#move-to-a-pose-control)

こちらのコードをC++で実装し、係数を調整しています。

実際にModel CR無しで、toioの位置からPoseの軌跡を描画した様子です。
初期位置とゴール位置をつなぐパスが求められているのがわかると思います。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">座標修正 <a href="https://t.co/5RNKgnAT0C">pic.twitter.com/5RNKgnAT0C</a></p>&mdash; Katsu Shun (@katsushun89) <a href="https://twitter.com/katsushun89/status/1148250396994883586?ref_src=twsrc%5Etfw">July 8, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>


# Model CRの自己位置の計算
自己位置はホイールオドメトリをモーター回転角度から算出して求めています。
実際にWHILLが公開しているModel CRを制御するROSのパッケージにホイールオドメトリを算出するコードがあるのでこれを流用しています。実際にはちょこちょこ変更していますが今回は説明割愛します。

[ros_whill/src/odom.cpp](https://github.com/WHILL/ros_whill/blob/melodic-devel/src/odom.cpp)

# 初期位置からゴール位置への移動
ここまで実装すると、初期位置とゴールを指定してあげればModel CRの機体を動かすことが出来ました。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">遂に <a href="https://twitter.com/hashtag/toio?src=hash&amp;ref_src=twsrc%5Etfw">#toio</a> の位置をトレースするように、<a href="https://twitter.com/hashtag/WHILL?src=hash&amp;ref_src=twsrc%5Etfw">#WHILL</a> を動かせるところまで出来た！<a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> にも軌跡を描画。<br>ようやく形になったー！ <a href="https://t.co/m7WWrDIket">pic.twitter.com/m7WWrDIket</a></p>&mdash; Katsu Shun (@katsushun89) <a href="https://twitter.com/katsushun89/status/1151483988768219138?ref_src=twsrc%5Etfw">July 17, 2019</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

この動画では、Bボタン押したときの位置をゴール位置としていますが、この記事の最初の追従動画では0.5秒ごとに現在位置をゴールとして更新するように変更するだけで、追従するような動きが実現出来ました。

あとは補足になりますが、FailsafeとしてCボタンでModel CRへの制御量の送信可否を切り替えられるようにしています。（何かあってもCボタンを押せば止まります。）


#さいごに
簡単ですが、toioと同じようにModel CRを動かすデモを作ってみました。

最初、toioを小さいモビリティと見立てて、それを動かすと実際に人が乗っているモビリティが動いたら面白いのではと思い作り始めました。

なんとなくヒューマンオーグメンテーションならぬモビリティオーグメンテーションっぽいかなと個人的には考えています。（モビリティ拡張）

toioとModel CRとM5Stackを組み合わせるだけでこういうことも出来るので、興味があったら試してみてください。
（Model CRお持ちの方はあまりいないかもしれないですが。。）