M5Stack+ROSで4脚ロボットを作る

この記事は[M5Stack Advent Calendar 2022](https://qiita.com/advent-calendar/2022/m5stack)の16日目の記事であり、[ROS Advent Calendar 2022](https://qiita.com/advent-calendar/2022/ros)の16日目の記事でもあります。記事1本で2つのアドベントカレンダーに参加しますので、2本分の濃さにしました。

# champ

[champ](https://github.com/chvmp/champ/tree/master)は、4脚ロボットを制御できるROSのパッケージ(群)です。このGIFアニメのように、市販されているロボットや研究で有名なロボットを制御できるよう、汎用的に作られています。また、ホビー用ロボットとして販売されている[mini pupper](https://github.com/mangdangroboticsclub/mini_pupper_ros)もchampで動いています。[^1]

![robots.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/932d0290-f473-921a-662e-2ecf123a6a6b.gif)

この優れたchampパッケージを自分のロボット上で動かしたく、レゴでロボットを自作しました。[^2]

# システム構成

![スクリーンショット 2022-12-04 14.52.44.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/c0f0077f-b51b-7f7f-c6f8-b0b50035cde5.png)

## PC
PCでは、Ubuntu20.04上でROS Noeticを動かしています。

## M5Stack

M5Stack Grayに、レゴ穴がある[M5GO/FIRE バッテリーボトム](https://www.switch-science.com/products/6356?_pos=1&_sid=322c60993&_ss=r)を取り付けています。Grayはたまたま手元にあったので使っただけで、M5Stackなら何でも良いです。

## Servo Driver

サーボドライバは[M5Stack用Servo2モジュール - 16チャンネル サーボドライバ](https://www.switch-science.com/products/6737)を使用しています。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/m5stack?src=hash&amp;ref_src=twsrc%5Etfw">#m5stack</a> のサーボモジュールで12個のサーボが動いた。こんなにコンパクトにまとまるなんて。<br><br>Module SERVO literally drives 12 servo motors. Powerful but compact. <a href="https://t.co/gNv6pOmBrh">pic.twitter.com/gNv6pOmBrh</a></p>&mdash; nisshan_ にっしゃん (@nisshan_) <a href="https://twitter.com/nisshan_/status/1389557429189238786?ref_src=twsrc%5Etfw">May 4, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

## M5Stack周りの配線

![IMG_3719.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/97afceeb-a7eb-3f6b-a236-43e3d9490936.jpeg)

## 電源

ロボットの電源は、[2セルのLiPoバッテリー](https://www.amazon.co.jp/gp/product/B085XBC65G/)をサーボモジュールに挿して供給しています。サーボモジュールに外部から電源供給すると、5vに降圧してM5Stack本体に供給してくれます。ドライバのコネクタはXT60、このバッテリーはXT30なので、コネクタの大きさを変換して繋いでいます。

## サーボ

[Geekservoのサーボ](https://www.amazon.co.jp/dp/B0B12P5Y1F/)を12個使っています。これを使っている理由は、小さく安価でありレゴ互換であることです。反面、2kgf.cmしかトルクが出ません。ギアも金属ではなく樹脂製なので、強度や耐久性を求めず「趣味の範囲」と割り切ってで利用しています(壊れたら買い替えれば良い)。

なお余談ですが、Geekservo製品は型番がなく識別する方法がないので「グレーの、直方体のやつ」で覚えてます。

![FgZ1JRGagAAP1PG.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/a9f3ebcc-a7a6-51b8-8407-e11df99d2dde.jpeg)

## M5Stack上のrosコード 

ros_serialのサンプルをベースに、サーボの制御指示値を受信する部分を書いています。M5Stackでsubscribeするトピック名は`/joint_group_position_controller/command`です。champ_controllerノードから制御指示値としてpublishされます。

サーボドライバに角度を指示するコードも、M5Stackのサンプルをそのまま使っています。

# 工夫した点、苦労した点

champのパッケージを起動すればサーボをよしなに動かしてれますので、システムを構築することは簡単なのです。しかし、やっぱり4脚ロボットならではの苦労が沢山ありましたので、書いて~~供養したい~~共有したいと思います。

## ① メカデザイン

初期のデザインは、Spot Microを真似て足にサーボを埋め込んだものでした。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">At long last LEGO quadruped robot walked. It’s just toddling, but moving forward.<br>よちよち歩きだけど、やっと歩いた。 <a href="https://t.co/goe5he8EbZ">pic.twitter.com/goe5he8EbZ</a></p>&mdash; nisshan_ にっしゃん (@nisshan_) <a href="https://twitter.com/nisshan_/status/1422195786830336002?ref_src=twsrc%5Etfw">August 2, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

このメカデザインのようにそれぞれの足に重いサーボが2個もあると、ブンブンと足を振るたびに大きな慣性モーメントが発生して、胴体が揺れてしまいます。これを解決するためには足を軽くするしかありません。具体的にはmini pupperのメカ構造を真似て、胴体に全てのサーボを格納しました。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">よーし、いい子だ。 <a href="https://t.co/RCor5UhWgG">pic.twitter.com/RCor5UhWgG</a></p>&mdash; nisshan_ にっしゃん (@nisshan_) <a href="https://twitter.com/nisshan_/status/1586584216216932352?ref_src=twsrc%5Etfw">October 30, 2022</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

動画：テストの様子[^3]

なお、レゴの挿し込みが抜けると困る部分には、ボルトとナットで固定しています。この固定方法が取れない部分は諦めて、「抜けたら挿す」という運用でカバーしています。

## レゴの3Dモデルを使ったROS URDFの作成

[ROS2で表示できるレゴ3Dモデルを作る方法](https://qiita.com/nisshan_/items/e43c638a12fdfa0473a6)に書きました。レゴでROS対応ロボットを作る際には参考になると思います。

## ② champ_setup_assistant

作った4脚ロボットに合うようchampをコンフィギュレーションする必要がありますが、そのツールが[champ_setup_assistant](https://github.com/chvmp/champ_setup_assistant)として用意されています。

![setup.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/669a78bb-6233-b0ee-abf3-f8315e54f11b.png)

使い方はリポジトリに書かれていますが、簡単に解説すると以下のようになります。

1. ROSで表示できるURDF用の3Dモデルを用意します。具体的には↑の記事を参照。
1. ROSで表示できるURDFを作ります。ゼロから書くのは大変ですので、[champ/champ_description/urdf/champ.urdf.xacro](https://github.com/chvmp/champ/blob/master/champ_description/urdf/champ.urdf.xacro)を改変するのが早いです。
    1. xacroを書きます
    1. ターミナルで `./gen_urdf` を実行し、xacroをurdfに変換します
1. champ_setup_assistantを `roslaunch champ_setup_assistant setup_assistant.launch ` で起動します。
    1. urdfを読み込みます
    1. Gait Configurationタブで数値を調整します。関節の向き (＞＞型や＞＜型)、最高速度、足を上げる高さ、重心の調整など設定します。ここの数値は後で直接書き換えもできます。
    1. Generate Config Packageで出力します。

出来上がったConfig Packageの中のlaunchファイルを起動すれば、ロボットを制御できます。

## ③ サーボ角度のキャリブレーション

サーボの出力角度が正確であることが、ロボット制御には必須です。指示した角度とサーボの出力角度が一致している必要があります。このGeekservoはPWM制御であり、12個のばらつきがかなり大きいです。そのため、以下の手順でキャリブレーションしました。

![IMG_3551.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/1e7b8917-d6b2-c81b-49cb-5e70794de972.jpeg)

1. 関節角度が目的の角度(0度、45度、90度のように使いたい範囲)になるまでサーボを動かし、指示値を記録します。
1. 単回帰分析により、各サーボごとに補正用の一次関数を得ます。

単回帰分析はグーグルスプレッドシートの「トレンドライン」表示すれば簡単に出ます。決定係数R2の値をチェックし、低い場合はデータを取り直すようにしました。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">やっとツール作りが終わった。<br>(サーボ角度のキャリブレーションツールアプリ) <a href="https://t.co/TNLPrWJCg2">pic.twitter.com/TNLPrWJCg2</a></p>&mdash; nisshan_ にっしゃん (@nisshan_) <a href="https://twitter.com/nisshan_/status/1521387663324237824?ref_src=twsrc%5Etfw">May 3, 2022</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

## ④ サーボの改造

Geekservoのギアには、過大な力がかかることによるギア破損を防止するためにクラッチ機構が入っています。
![IMG_0175.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/8a4375c8-5eb0-35b8-92d6-7404f06c835d.jpeg)
保護のためには良い仕組みなのですが、ぐっと力を入れて踏ん張ってほしいときにクラッチが働いてしまい、関節の角度を保持できないことがありました。なので、このクラッチをボンドで固めてしまって、固定しました。

また、もう1つ固定したい理由があります。それは、↑でせっかく角度のキャリブレーションしてもクラッチが効くとずれてしまう、ということです。

![IMG_0177.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/1074b3d2-d169-f9f0-a769-dda39cc907d8.jpeg)

接着剤で固めてしまいました。これを12個分やりました。

![IMG_0181.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/ad1a88e2-27c2-0590-827f-c9bc1b18d74e.jpeg)

## ⑤ 重心の調整

champの`config/gait/gait.yaml`内の`com_x_translation`の値によって、X方向(前後方向)の重心を調整できます。サーボやフレームはシンメトリーに配置できても、搭載したセンサーやバッテリーをすべて中心には置けませんので、この変数で調整できるのはありがたいです。

なお、この自作ロボットは何も搭載していない状態なので、無調整で大丈夫でした。

# 完成

![IMG_7031.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/9550727e-2ab4-dbb4-95ae-e718cfc46d8c.jpeg)
![IMG_7032.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/338b8f56-58b3-9c9a-ed1a-1eeee26b8e5f.jpeg)
![IMG_7026.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/6c2ecc9c-e917-d08c-b703-ed4a593ee048.jpeg)
![IMG_7027.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/630585/c0b5e255-820c-9003-f5a4-0604bdfaa90c.jpeg)


動画
<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">4本足で歩くロボットを作りました。<br><br>Lego Compatible Quadruped Robot <a href="https://t.co/o9JNLtx91l">https://t.co/o9JNLtx91l</a><a href="https://twitter.com/hashtag/%E3%83%AC%E3%82%B4%E4%BA%92%E6%8F%9B?src=hash&amp;ref_src=twsrc%5Etfw">#レゴ互換</a> <a href="https://twitter.com/hashtag/lego?src=hash&amp;ref_src=twsrc%5Etfw">#lego</a> <a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://twitter.com/hashtag/ROS?src=hash&amp;ref_src=twsrc%5Etfw">#ROS</a> <a href="https://twitter.com/hashtag/champ?src=hash&amp;ref_src=twsrc%5Etfw">#champ</a> <a href="https://twitter.com/hashtag/minipupper?src=hash&amp;ref_src=twsrc%5Etfw">#minipupper</a></p>&mdash; nisshan_ にっしゃん (@nisshan_) <a href="https://twitter.com/nisshan_/status/1586913348032946176?ref_src=twsrc%5Etfw">October 31, 2022</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

M5Stackの画面には全知全能のコンピュータが考えているような幾何学模様が表示されていますが、これはただランダムに正方形を表示しているだけですw

# 今後

* 安価なPWMサーボの次は、シリアルサーボを使ったロボットを作りたい。
* 歩行の制御の実現を既成のパッケージに頼らず、自作したい。

# 参考
* [ROSのrosserialを使って、M5stackとwifi通信でLチカする](https://qiita.com/nnn112358/items/1cd4517ea2faa57e9c13) by nnn112358さん

[^1]: mini pupperの動作モードは、「Stanford Pupperのコードでラジコンのように動かす」モードと、「champで制御し、ROSのnavigationができるモード」の2つあります。ここでは後者を指しています。
[^2]: 正確にはレゴ互換品です。「レゴ規格に合ったもの」という広い意味で使っています。
[^3]: 動画の足の動きは、実は少し変です。角度の計算を間違っていることが原因なのですが、当時、撮った動画を見て気づきました。