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

#はじめに

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

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

#toio に追従するやつ出来た！#WHILL#M5Stack pic.twitter.com/JMLzQI70Fi

— Katsu Shun (@katsushun89) July 19, 2019

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

toioとは

SONYが出しているCube型のおもちゃです。

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

toioのjavascriptライブラリが公開されたのでexample動かしてみた

WHILL Model CRとは

人が乗れるパーソナルモビリティWHILL Model Cの、外部から制御出来る研究開発用のモデルがModel CRです。

M5Stackとは

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

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

M5StackのJoystickでWHILL Model CRを動かしてみた

システム構成

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

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

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

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

toioの初期位置と目標位置の取得

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

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

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

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

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

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

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

座標修正 pic.twitter.com/5RNKgnAT0C

— Katsu Shun (@katsushun89) July 8, 2019

Model CRの自己位置の計算

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

ros_whill/src/odom.cpp

初期位置からゴール位置への移動

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

遂に #toio の位置をトレースするように、#WHILL を動かせるところまで出来た！#M5Stack にも軌跡を描画。
ようやく形になったー！ pic.twitter.com/m7WWrDIket

— Katsu Shun (@katsushun89) July 17, 2019

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

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

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

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

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

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