1. はじめに

前回の記事（四足歩行ロボットのUnitree Go2をシミュレーション環境（Mujoco)で動かしてみる）では、👇のリポジトリで公開されているunitree_mujocoというシミュレーション環境を利用して、サンプルプログラムではありますが、Unitree go2が立ちあがるところをシミュレーションできました。

今回は、unitree_ros2 などを利用して、キーボードからの遠隔操作(teleop)を試みましたが、失敗しました（どう失敗したのかは後述の動画で説明します。）。最終的にできたのは「立ち上がる ⇔ 伏せる」動作の繰り返しだけでしたが、それを実装する過程で unitree_mujocoでUnitree Go2を動かす際の制約や課題が分かってきましたので、この記事にまとめます。

2. 実行環境

CPU: CORE i7 7th Gen
メモリ: 32GB
GPU: GeForce RTX 2070
OS: Ubuntu22.04(WSL2ではなくPCに直接インストール)
ROS2: Humble

3. キーボードからの遠隔操作（失敗）

3.1 これまでの流れ

前回の記事では、以下のコマンドで unitree_mujoco のシミュレーションを動作させました。

terminal(1)

cd ~/unitree_mujoco/simulate_python
python3 unitree_mujoco.py

以下のコマンドでUnitree Go2を立ちあげました。

terminal(2)

cd unitree_mujoco/
python3 ./example/python/stand_go2.py

3.2 cmd_vel を使った遠隔操作に挑戦

次に、ROS2 の cmd_vel トピックを受け取って歩行するようなプログラム go2_cmd_vel.py（あまりに出来が悪いので、この記事では公開しません）を作成し、以下のコマンドを使ってロボットを遠隔操作しようとしました。

terminal(3)

source /opt/ros/humble/setup.bash
ros2 run teleop_twist_keyboard teleop_twist_keyboard

3.3 シミュレーション結果（失敗例）

その結果、まず「立ち上がる時点でロボットがひっくり返る」という問題が発生しました。さらに、cmd_vel のトピックを送信すると「床で駄々っ子のように足をバタバタさせるだけ」という状態になりました。

また別のケースでは、以下の動画のように立ち上がった瞬間に後ろへ飛んでしまうものや震えながらジャンプを繰り返すこともありました。

👇後ろに飛ぶ

👇震えながらジャンプを繰り返す。

3.4　原因と考えられるもの

実機の Unitree Go2 であれば SportModeを使用した高レベル制御が可能で、cmd_vel のような移動コマンドを受け取って歩行ができます（ROS2のメッセージをそのまま受け取れるわけではなく、若干工夫する必要はありそうです。）。しかし unitree_mujoco では現在のバージョンで高レベル制御には対応しておらず、低レベル制御（LowCmd_ を使った個別モーター制御）やゲームコントローラーによる移動のみが可能なようです。ただし、シミュレーション環境でも SportModeState のデータを取得することはできるため、解析目的には使用可能の模様です。

4. 立ち⇔伏せの連続動作

4.1 実装結果

最終的に、「立ち⇔伏せを繰り返す」動作を実装しました。しかしながら、姿勢遷移が急激すぎると振動が発生し、まともに動かなくなることがありました。

その様子が以下の動画になります。

4.2 実行したプログラム

👇のプログラムで立ち⇔伏せの切り替えを行いました。

stand_sit.py

import time
import sys
import numpy as np

from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
from unitree_sdk2py.idl.default import unitree_go_msg_dds__LowCmd_
from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_
from unitree_sdk2py.utils.crc import CRC

# 立ちと伏せの目標関節角度
stand_up_joint_pos = np.array([
    0.00571868, 0.608813, -1.21763, -0.00571868, 0.608813, -1.21763,
    0.00571868, 0.608813, -1.21763, -0.00571868, 0.608813, -1.21763
], dtype=float)

stand_down_joint_pos = np.array([
    0.0473455, 1.22187, -2.44375, -0.0473455, 1.22187, -2.44375,
    0.0473455, 1.22187, -2.44375, -0.0473455, 1.22187, -2.44375
], dtype=float)

dt = 0.002
switch_interval = 7.0  # 立ち⇔伏せの切り替え間隔を7秒
transition_time = 3.5  # なめらかに遷移する時間を3.5秒に設定
crc = CRC()

input("Press enter to start")

if __name__ == '__main__':
    if len(sys.argv) < 2:
        ChannelFactoryInitialize(1, "lo")
    else:
        ChannelFactoryInitialize(0, sys.argv[1])

    pub = ChannelPublisher("rt/lowcmd", LowCmd_)
    pub.Init()

    cmd = unitree_go_msg_dds__LowCmd_()
    cmd.head[0] = 0xFE
    cmd.head[1] = 0xEF
    cmd.level_flag = 0xFF
    cmd.gpio = 0
    for i in range(20):
        cmd.motor_cmd[i].mode = 0x01
        cmd.motor_cmd[i].q = 0.0
        cmd.motor_cmd[i].kp = 0.0
        cmd.motor_cmd[i].dq = 0.0
        cmd.motor_cmd[i].kd = 0.0
        cmd.motor_cmd[i].tau = 0.0

    last_switch_time = time.perf_counter()
    stand_mode = True 

    while True:
        step_start = time.perf_counter()

        # 立ち ⇔ 伏せ の切り替え
        if step_start - last_switch_time >= switch_interval:
            stand_mode = not stand_mode
            last_switch_time = step_start

        # なめらかに遷移させる
        elapsed_time = step_start - last_switch_time
        phase = np.clip(elapsed_time / transition_time, 0.0, 1.0)

        # 目標姿勢を設定
        if stand_mode:
            target_joint_pos = stand_up_joint_pos
            source_joint_pos = stand_down_joint_pos
        else:
            target_joint_pos = stand_down_joint_pos
            source_joint_pos = stand_up_joint_pos

        for i in range(12):
            cmd.motor_cmd[i].q = (1 - phase) * source_joint_pos[i] + phase * target_joint_pos[i]
            cmd.motor_cmd[i].kp = 50.0
            cmd.motor_cmd[i].dq = 0.0
            cmd.motor_cmd[i].kd = 3.5
            cmd.motor_cmd[i].tau = 0.0

        cmd.crc = crc.Crc(cmd)
        pub.Write(cmd)

        # 次のループまで時間調整
        time_until_next_step = dt - (time.perf_counter() - step_start)
        if time_until_next_step > 0:
            time.sleep(time_until_next_step)

なお、実行は以下のコマンドで行いました。

terminal

source /opt/ros/humble/setup.bash
python3 ~/unitree_mujoco/example/python/stand_sit.py

5. まとめ

unitree_mujocoを利用して今回の記事から分かったこと：

現在のシミュレーション環境のバージョンでは高レベル制御（SportModeなど）は未実装で、低レベル制御のみ対応
低レベル制御のみで歩行を実装するには、制御ゲインの調整が難しく、振動が発生しやすい
「立ち⇔伏せ」の動作は比較的簡単に実装可能

今後は「低レベル制御でキーボード操作による歩行を実装する」ために、様々な制御方法を試しつつ、高レベル制御が本当にできないのかも調査していきたいと思います。