# IMU で姿勢を見て踏ん張らせる

{doc}`walk` の方策は、関節角と関節速度しか見ていません。胴体が傾いても、
押されて回っても、方策にはそれが直接は伝わりません。

このページでは、胴体の前に **IMU（慣性計測装置。胴体の傾きや回転の速さを測るセンサ）**
を載せ、その値も観測に入れて学習した方策で歩かせます。押されると脚を踏ん張って
胴体を戻し、止まっている間も倒れないように立ち続けます。

```{note}
IMU 版は書籍の本文には載っていない、サポートページだけの内容です。IMU の無い
機体は、これまでどおり {doc}`walk` の方策で歩きます。どちらにするかは、
ファームウェアの書き込み先を選ぶだけで切り替わります。
```

## 何が変わるか

| | IMU なし（{doc}`walk`） | IMU 版 |
|---|---|---|
| 観測 | 87 次元（指令・歩容位相・関節角・関節速度・前回 action、各 3 履歴） | **105 次元**。上の 87 に、胴体の**角速度** 3 と**重力の向き** 3（各 3 履歴）を足す |
| 学習 | 平地の歩行 | 平地の歩行のあと、学習中に機体を押す強さを 3 倍にして仕上げる |
| 押されたとき | 胴体の傾きは見えない | 傾きと回転を見て、脚で踏ん張って戻す |
| 止まったとき（`stop`） | 関節を home 姿勢（立って歩き出す基準の姿勢）に固定する | 方策を速度 0 で回し続ける（**balance**）。押されても踏ん張る |
| ネットワークと制御周期 | 全結合 [96, 64]、50 Hz | 同じ |

重力の向きは「胴体から見て、下がどちらにあるか」を表す長さ 1 のベクトルで、
水平に立っていれば (0, 0, −1) です。頭が下がると x（前）が正に、右が下がると
y（左）が負になります。

## ブラウザで押してみる

{doc}`walk` の「ブラウザで歩かせてみる」と同じシミュレーションで、IMU 版の方策を
動かせます。下は「RL 方策（IMU あり）」、前進 0 で開きます。**押す** を押すと、
胴体にランダムな向きの速度（「強さ」で決める）を一瞬で与えます。
「制御」を「RL 方策」（IMU なし）に切り替えて、同じように押し比べてください。
倒れるとシミュレーションが止まり、右下に押した回数と倒れた回数が出ます。

```{raw} html
<p>
  <a class="sd-btn sd-btn-primary" href="../assembly/quadruped/walk/index.html?mode=imu&vx=0"
     target="_blank" rel="noopener">IMU 版のシミュレーションを開く ↗</a>
</p>
<iframe src="../assembly/quadruped/walk/index.html?mode=imu&vx=0"
        style="width:100%; height:70vh; border:1px solid var(--pst-color-border, #ddd); border-radius:8px;"
        loading="lazy"
        title="四脚ロボット IMU 版 押しのシミュレーション"></iframe>
```

同じシミュレーションで、前進 0 で立たせて、各 60 回押したときに倒れた回数です。

| 押す強さ | IMU なし | IMU 版 |
|---|---|---|
| 1.2 m/s | 3 回 | 2 回 |
| 1.6 m/s | 6 回 | 6 回 |
| 2.0 m/s | 18 回 | 6 回 |
| 2.4 m/s | 25 回 | 12 回 |

学習中に押す強さは IMU 版でも最大 0.75 m/s ですが、シミュレーションでは足先が
床を滑って逃げるので、1 m/s くらいまではどちらも倒れません。差が出るのは
2 m/s あたりからです。立っているときの胴体の傾きも、IMU なしは 6° 前後
傾いたまま、IMU 版はほぼ 0° です。

一方、**歩いているとき**（前進 0.12 m/s）に押すと差は出ず、1.6 m/s では
IMU 版のほうが多く倒れました（60 回中、IMU なし 32 回・IMU 版 41 回）。
IMU 版の方策は、止まって踏ん張るほうに効いています。

## 必要なもの

{doc}`bom`（Feetech 版）または {doc}`krs_bom`（KRS 版）の部品に加えて、次の 2 つです。

| | |
|---|---|
| IMU 基板 | QMI8658（QST 製の 6 軸 IMU。加速度 3 軸 + 角速度 3 軸）を載せた基板。I2C アドレス **0x6B** |
| ケーブル | Qwiic（4 ピンの I2C コネクタ。3.3 V）のケーブル 1 本。UNO R4 WiFi の Qwiic コネクタにつなぐ |

本書の機体では、QMI8658 を載せた自作のセンサ基板を使っています。この基板は販売を予定しています。

基板は**胴体の前面の 45° の面に、4 ピンのコネクタを下に向けて**取り付けます。
方策は学習したときと同じ座標で値を受け取る必要があるので、ファームウェアは
この向きを前提に、チップの軸を胴体の軸（x 前・y 左・z 上）へ回しています
（`code/13_quadruped/src/quad_imu.cpp` の `R_BODY_CHIP`）。別の基板や別の向きで
付けるときは、ここを書き換えてください。下の「向きを確かめる」で合っているか
分かります。

## 書き込む

{doc}`walk` の「実機で歩かせる」と同じ手順で、書き込み先（PlatformIO の env）の
名前に **`_imu`** を付けます。

```bash
cd code
pio run -d 13_quadruped -e uno_r4_wifi_rcb4_imu -t upload   # KRS（RCB-4mini）版
pio run -d 13_quadruped -e uno_r4_wifi_imu -t upload        # Feetech 版
```

WiFi で動かすときは `uno_r4_wifi_rcb4_udp_imu` / `uno_r4_wifi_udp_imu` です。
`_imu` を付けると、方策が `include/arduino_quad_policy.h` の代わりに
`include/arduino_quad_policy_imu.h` になり、IMU を読むコードが入ります。

```{warning}
電源を入れてから約 10 秒は、ジャイロ（角速度センサ）のゼロ点を取っています。
**この間は機体を動かさないでください。** 起動時のメッセージに
`IMU: chip acc[g] ...` と出れば成功です。
```

IMU が見つからない・校正に失敗したときは、`!! IMU 無しで動かす` と出て、
方策には水平に止まっているときの値（角速度 0、重力 (0, 0, −1)）を渡します。
毎秒の行の最後に `imu=level` が付きます。平らな床ならこのままでも歩けますが、
傾いても押されても方策には伝わりません。

## 向きを確かめる

歩かせる前に、シリアルモニタ（115200 baud）で `imu` と打ち、値を確かめます。
`body g` が胴体から見た重力の向き、`target rad` が方策の出した関節の目標角です。

1. 床に置いて `imu`。`body g` が `0 0 -1` に近いこと
2. 頭を下げて `imu`。`body g` の 1 つ目（x）が**正**になること。
   右側を下げて `imu`。2 つ目（y）が**負**になること。
   逆になるなら、基板の向きと `R_BODY_CHIP` が合っていません
3. **持ち上げて足を床から離し**、水平に持って `stop`。そのまま傾けるたびに
   `imu` を打ち、脚が動いて `target rad` が変わること。変わらなければ、
   IMU の値が方策に届いていません

足が床に着いていない状態は学習していないので、3 で脚がどう動くかに意味は
ありません。届いているかどうかだけを見ます。KRS 版の実機で 3 をやったときの値です
（並びは FL_hip, FL_knee, RL_hip, RL_knee, RR_hip, RR_knee, FR_hip, FR_knee）。

| 持ち方 | `body g` | `target rad` |
|---|---|---|
| 水平 | −0.04 0.00 −1.00 | 1.11 −1.77 1.28 −1.76 1.27 −1.73 1.17 −1.77 |
| 頭を約 45° 下げる | 0.71 −0.01 −0.71 | 1.10 −1.16 0.57 −1.59 0.94 −1.73 0.54 −1.81 |
| 右を約 50° 下げる | 0.04 −0.78 −0.63 | 0.98 −1.41 0.83 −1.52 0.98 −1.77 0.87 −1.63 |

## 歩かせる・押してみる

コマンドは {doc}`walk` と同じです。最初は吊るしてから試してください。

| コマンド | 動き |
|---|---|
| `rl 0.1 0` | 前進。2 つ目の数字は旋回（rad/s） |
| `rl 0 0` | その場で立つ。押されると踏ん張る |
| `stop` | 止まって balance（速度 0 で方策を回し続ける） |
| `stand` | 方策を切り、関節を home 姿勢に固定する |
| `imu` | 角速度・重力の向き・関節の目標角を 1 回出す |

PC のキーボードで動かす `host/quad_host.py serial` / `wifi` もそのまま使えます。
スペースキーで止めると balance になります。

KRS 版の実機で `rl 0 0` にして横や前後から押したときは、胴体が最大 25° 傾いても、
脚で踏ん張って戻し、倒れませんでした。

```{note}
WiFi 版は、通信が途切れても最後に受けた速度で **5 秒**歩き続け、5 秒途切れたら
止まります（IMU 版では balance）。WiFi はパケットがまとめて遅れることがあり、
すぐ止めると、歩いては止まるのを繰り返してカクつくためです。PC が落ちても
5 秒は歩き続けるので（0.1 m/s なら約 50 cm）、周りを空けて使ってください。
```

## 方策を自分で学習する

{doc}`train` の「手元の GPU で学習する」と同じ環境で、環境変数
`ARDUINO_QUAD_IMU=full` を付けると、方策の観測に角速度と重力の向きが入ります。
配布している方策は、歩行を 3000 iteration 学習したあと、学習中に機体を押す強さを
3 倍（`ARDUINO_QUAD_PUSH_SCALE=3`）にして、`ArduinoQuad-Robust` で 1000 iteration
続けたものです。

```bash
cd 13_quadruped
source rl/scripts/env.sh
export ARDUINO_QUAD_IMU=full

# 1. 歩行を学習する
./rl/scripts/train.sh ArduinoQuad-Walk --env.scene.num-envs=4096 \
    --agent.max-iterations 3000

# 2. 押しを 3 倍にして、1 の方策から続ける
ARDUINO_QUAD_PUSH_SCALE=3 ./rl/scripts/train.sh ArduinoQuad-Robust \
    --env.scene.num-envs=4096 --agent.max-iterations 1000 \
    --agent.resume True --agent.load-run <1 の run> \
    --agent.load-checkpoint <1 の最後のチェックポイント>
```

`<1 の run>` は 1 で `logs/rsl_rl/arduino_quad_velocity/` の下にできたフォルダの名前、
`<1 の最後のチェックポイント>` はその中でいちばん番号の大きい `model_*.pt` です。

| 環境変数 | 意味 | 既定値 |
|---|---|---|
| `ARDUINO_QUAD_IMU` | `none`（IMU なし）/ `gyro`（角速度だけ）/ `full`（角速度 + 重力の向き） | `none` |
| `ARDUINO_QUAD_PUSH_SCALE` | 学習中に機体を押す強さの倍率 | 1 |
| `ARDUINO_QUAD_IMU_TILT_DEG` | 基板の取り付けの傾きを、最大この角度 [°] までばらつかせる | 0 |
| `ARDUINO_QUAD_IMU_GYRO_BIAS` | ジャイロのゼロ点のずれを、各軸最大この値 [rad/s] までばらつかせる | 0 |
| `ARDUINO_QUAD_IMU_DELAY_MAX` | IMU の値を 0〜この周期数（1 周期 20 ms）遅らせる | 0 |

下の 3 つは、方策（actor）が見る値だけにずれを入れます。価値関数（critic）は
正確な値のまま学習します。

書き出すときも `ARDUINO_QUAD_IMU=full` を付けたままにしてください。観測の並びは
学習時の環境から読み出すので、付け忘れると IMU の無い並びで書き出されます。
出てきた `arduino_quad_policy.h` を `include/arduino_quad_policy_imu.h` という
名前で置き換えると、`_imu` の env に入ります。

```bash
cd "$ARDUINO_QUAD_UPSTREAM"
ARDUINO_QUAD_IMU=full python "$ARDUINO_QUAD_ROOT/host/export_quad_policy.py" \
    logs/rsl_rl/arduino_quad_velocity/<2 の run>/<2 の最後のチェックポイント> \
    出力先ディレクトリ ArduinoQuad-Robust
```

```{note}
GPU なしで学習する CPU 版（{doc}`train` の「GPU なしで学習する」）は、まだ IMU 版に
対応していません。方策の観測は IMU なしの 87 次元に固定です。
```

## まだできないこと

- **転倒からの起き上がり**は、IMU 版でもまだ学習していません。
- **坂や傾いた床**では学習していません。学習中の床はいつも水平なので、坂の上で
  胴体を水平に保つような動きは期待できません。
- **歩きながら押されたとき**は、上の「ブラウザで押してみる」のとおり、
  シミュレーションでは IMU なしより良くなっていません。
- 実機で確かめたのは KRS 版（USB と WiFi）です。Feetech 版の `_imu` はビルドが
  通ることだけ確かめています。