toioでバランスゲーム

toio……ソニーの発売している小さなロボットトイ。プログラム制御可能。

シーソーの上に乗せて、落ちないように前後に動き続けさせる。制御のルールを人が書くのではなく、シミュレータ上で強化学習させた方策を実機に載せる方式にした。

落ちそうで落ちない。

構成

MuJoCo（物理シミュレータ）
Gymnasium
Stable-Baselines3 / PPO
toio.py（実機のBLE制御）

シミュレータ側と実機側で別々のプログラムを書くが、両者が同じ形の観測と行動をやりとりするようにしておくのがこの方式の要点になる。

Mujoco

MuJoCoで強化学習を試してみる。シーソーでバランスを取ろうとするtoioのシミュレーション。

シーソーのモデル

支柱にヒンジジョイントを1つ置き、その上の板の上をtoioがスライドジョイントで前後する、という最小構成にした。質量は実物に合わせてtoioを28.6g、板を100gにしている。

<body name="pivot" pos="0 0 0.05">
  <joint name="tilt" type="hinge" axis="0 1 0"
    damping="0.1" range="-0.15 0.15"/>

  <body name="seesaw" pos="0 0 0.05">
    <geom type="box" size="0.3 0.037 0.01" mass="0.1"/>

    <body name="toio" pos="0 0 0.025">
      <joint name="slide" type="slide" axis="1 0 0" damping="10.0"
        limited="true" range="-0.25 0.25"/>
      <geom type="box" size="0.032 0.032 0.024" mass="0.0286"/>
    </body>
  </body>
</body>

環境

観測は板の傾き角と角速度の2つだけ、行動は前後方向の1次元。報酬は傾きの絶対値にマイナスを付けたもので、「水平に近いほど良い」とだけ伝える。

def step(self, action):
    self.data.ctrl[0] = np.clip(action[0], -1, 1) * 1.0
    for _ in range(self.sim_steps):
        mujoco.mj_step(self.model, self.data)

    angle  = self.data.qpos[0]
    angvel = self.data.qvel[0]
    obs = np.array([angle, angvel], dtype=np.float32)

    reward = -abs(angle)
    return obs, reward, terminated, False, {}

この観測を2つに絞ったことが、あとで実機に移すときに効いてくる。toioから取れるセンサ値だけで同じベクトルを組み立てられるようにしてある。

学習はPPOのMlpPolicyで5万ステップ。

model = PPO("MlpPolicy", env, verbose=1)
model.learn(total_timesteps=50000)
model.save(os.path.join(model_dir, "ppo_toio_balance"))

シーソー板を円盤に差し替えたバージョンも作っていて、そちらは2軸に傾く。

実機へ移す

学習済みの重みをそのまま読み込み、シミュレータの代わりにtoio本体をつなぐ。ループの中身は「センサを読む → 方策に通す → モーターに流す」だけになる。

観測を作る

toioは姿勢角をオイラー角で返してくれるので、pitchをラジアンに直せば1つ目の要素になる。角速度は返ってこないため、前回の値との差を経過時間で割って作っている。

pitch_rad = math.radians(angle.pitch)
now = time.time()
ang_vel = 0.0 if prev_pitch is None else (pitch_rad - prev_pitch) / (now - prev_t)
prev_pitch, prev_t = pitch_rad, now

obs = np.array([pitch_rad, ang_vel], dtype=np.float32)
action, _ = model.predict(obs, deterministic=True)

シミュレータ側の [angle, angvel] と同じ並びになった。ここが揃っていれば、方策は自分がシミュレータにいるのか実機にいるのか区別しない。

出力を実機に合わせる

方策の出力は-1〜1の連続値で出てくるが、toioのモーターは小さい値を渡しても動き出さない。そのため下限を設けて、その範囲へ引き伸ばしている。

MIN_SPEED = 8
MAX_FWD = MAX_BWD = 60

def scale_speed(u: float) -> int:
    if u >= 0:
        return max(MIN_SPEED, min(int(u * MAX_FWD), MAX_FWD))
    else:
        return -max(MIN_SPEED, min(int(abs(u) * MAX_BWD), MAX_BWD))

このあたりの、シミュレータには存在しない実機側の都合をどこで吸収するかが、sim-to-realのいちばん手を動かす部分になる。傾きの符号とtoioの進行方向の関係も、実際に載せる向きに合わせて反転させている。

衝突時の扱い

toioには衝突検出があるので、ぶつかったフレームはモーターを止めて少し待ち、方策の推論をスキップしている。シーソーの端に当たった瞬間の異常な観測が方策に入らないようにするため。

motion = await read_sensor(cube, MotionDetectionData)
if motion and motion.collision:
    await cube.api.motor.motor_control(0, 0)
    await asyncio.sleep(COLLISION_PAUSE)
    continue