Unitree Go2をジャンプさせてみた

# はじめに
Unitree Go2を使って「ジャンプ動作」を試みましたが、思うようにいかない部分も多く、ロボット制御の難しさを実感しました。
この記事ではその過程で得られた知見や試行錯誤の内容をまとめています。今後、同様に挑戦する方の参考になれば幸いです。

# 目次
1. [使用環境](#Chapter1)
1. [背景と狙い](#Chapter2)
1. [関節と制御パラメータ](#Chapetr3)
1. [試したこと](#Chapter4)
1. [実装コード](#Chapter5)
1. [シミュレーション結果](#Chapter6)
1. [今後の展望](#Chapter7)
1. [参考文献](#reference)

<a id="#Chapter1"></a>
# 使用環境
PC: Windows 11  
仮想環境: WSL2  
OS: Ubuntu 24.04 LTS  
Python: 3.10  
シミュレーション: unitree_mujoco（Go2公式の物理エンジン）  

参考にした環境構築記事:  
- https://qiita.com/bebebefu/items/6dda8fa9f12c74779231 [1]

<a id="#Chapter2"></a>
# 背景と狙い
Unitree Go2のロボットには、脚ごとに 12個の関節角度（モータ） を指定できます。
この制御は複雑で、ゼロから設計するのは困難だと感じました。
そこで、Unitree公式が提供している stand_go2.py を参考に、以下の方針でジャンプ動作を試みました：
- 公式が提供しているスタンド姿勢（stand_up_joint_pos）をベースに使用
- モータ制御パラメータ（kp, kd, tau など）を調整して、ジャンプを試みる

<a id="#Chapter3"></a>
# 関節と制御パラメータ
関節の配列は以下の順序で格納されています：
~~~
0  : FR_hip    （右前・股関節）
1  : FR_thigh  （右前・大腿）
2  : FR_calf   （右前・膝/スネ）

3  : FL_hip    （左前・股関節）
4  : FL_thigh  （左前・大腿）
5  : FL_calf   （左前・膝/スネ）

6  : RR_hip    （右後・股関節）
7  : RR_thigh  （右後・大腿）
8  : RR_calf   （右後・膝/スネ）

9  : RL_hip    （左後・股関節）
10 : RL_thigh  （左後・大腿）
11 : RL_calf   （左後・膝/スネ）
~~~

モータ制御に関わる主要なパラメータは以下の通りです：
~~~
q   : 指定する関節角度
kp  : 位置制御の強さ（Pゲイン）
dq  : 目標速度（0にすることで静止を目指す）
kd  : 速度制御の強さ（Dゲイン）
tau : トルク（kp, kdで制御しない場合はこれを直接指定）
~~~
これらを適切に設定することで、ジャンプの「瞬発力」や「制動力」を細かく調整できますが、人力で最適化するのは非常に困難でした。

<a id="#Chapter4"></a>
# 試したこと
いきなり関節角度を切り替えると、ロボットが激しく揺れてしまいます。
最初に stand_up_joint_pos に瞬時に遷移させた後、時間経過とともに「着地姿勢」へと緩やかに変化。
前脚と後脚を同じ力で制御しても、後脚の蹴りが弱いことがわかりました。
そこで、後脚（index 6〜11）のゲイン（kp, kd）を高めに設定し、より強く蹴り上げるように修正。

<a id="#Chapter5"></a>
# 実装コード
~~~python:jump_go2.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

# 関節の目標角度（4脚×3関節）
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
runing_time = 0.0
crc = CRC()

input("Press enter to start")

if __name__ == '__main__':

    # 初期化処理らしい
    if len(sys.argv) <2:
        ChannelFactoryInitialize(1, "lo")
    else:
        ChannelFactoryInitialize(0, sys.argv[1])

    # Create a publisher to publish the data defined in UserData class
    pub = ChannelPublisher("rt/lowcmd", LowCmd_)
    pub.Init()
    # LowCmdの初期化
    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  # (PMSM) mode
        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

    # メインループ
    jump_executed = False  # 一度だけジャンプ姿勢にするためのフラグ

while True:
    step_start = time.perf_counter()
    runing_time += dt

    if runing_time < 1.2:
        if not jump_executed:
            # インデックス：後脚（RRとRL） = 6〜11
            rear_leg_indices = [6, 7, 8, 9, 10, 11]

            for i in range(12):
                cmd.motor_cmd[i].q = stand_up_joint_pos[i]
                cmd.motor_cmd[i].dq = 0.0
                cmd.motor_cmd[i].tau = 0.0

                if i in rear_leg_indices:
                    cmd.motor_cmd[i].kp = 300.0  # 後ろ足を強めに
                    cmd.motor_cmd[i].kd = 15.0
                else:
                    cmd.motor_cmd[i].kp = 100.0  # 前足は弱めに
                    cmd.motor_cmd[i].kd = 6.0
            jump_executed = True

    else:
        # Stand down
        phase = np.tanh((runing_time - 1.2) / 0.7)
        for i in range(12):
            cmd.motor_cmd[i].q = phase * stand_down_joint_pos[i] + (
                1 - phase) * stand_up_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)
~~~

<a id="#Chapter6"></a>
# シミュレーション結果

以下が成功例になります。
![1-ezgif.com-video-to-gif-converter.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/4075273/f845a6a3-980b-4e9d-b313-6d6f8a0e1d8a.gif)


以下が失敗例になります。
![2-ezgif.com-video-to-gif-converter.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/4075273/6920c422-2082-46c8-bf9f-37f0bfcce680.gif)

<a id="#Chapter7"></a>
# 今後の展望
ジャンプ動作そのものは一応可能でしたが、成功率が不安定であったため、次の改善が必要と感じました：
- tau を直接操作する「トルク制御」への移行
- 着地と脚のタイミング制御

また、最終的には強化学習や最適化アルゴリズムを使ってパラメータ探索することで、人手では困難な調整も自動化できるかもしれません。

本記事を通じて、ロボット制御の奥深さとPD制御の難しさを改めて実感しました。


<a id="#reference"></a>
# 参考文献
- [1][https://qiita.com/bebebefu/items/6dda8fa9f12c74779231](URL)