MuJoCo + RL 機器人模擬教學

從零開始學習 MuJoCo 物理模擬與強化學習環境建置

適用於 UR5 機器手臂 + 二指夾爪

1. 課程概覽

學習目標

本教學將帶你從零開始，學會以下技能：

MuJoCo 物理模擬 — 載入模型、執行模擬、取得模擬狀態
機器人控制 — 關節控制、PID 控制器、逆運動學
視覺觀測 — 相機渲染、RGB-D 影像、座標轉換
Gymnasium 環境 — 自定義 RL 環境的標準介面
強化學習基礎 — Q-learning、經驗回放、訓練迴圈

教學檔案結構

tutorial/
  01_hello_mujoco.py -- MuJoCo 基礎：載入模型、觀察模擬
  02_control_robot.py -- 機器人控制：馬達、PID、逆運動學
  03_camera_and_images.py -- 相機影像：RGB-D、座標轉換
  04_gymnasium_env.py -- RL 環境概念：reset/step/reward
  05_simple_rl_training.py -- 真實 MuJoCo RL 訓練：Q-network + 網格動作
  robot_controller.py -- 機器人控制器（核心模組，含 reset_simulation）
  requirements.txt -- Python 套件依賴

2. 環境安裝

📋

前置需求：Python 3.8+、pip、基本的終端機（Terminal）操作能力。
建議使用 Anaconda 或 venv 建立虛擬環境。

Step 1: 建立虛擬環境

# 使用 conda（推薦）
conda create -n mujoco_rl python=3.10
conda activate mujoco_rl

# 或使用 venv
python -m venv mujoco_rl_env
# Windows:
mujoco_rl_env\Scripts\activate
# Linux/Mac:
source mujoco_rl_env/bin/activate

Step 2: 安裝依賴套件

# 進入 tutorial 目錄
cd tutorial

# 安裝所有依賴
pip install -r requirements.txt

Step 3: 驗證安裝

# 測試 MuJoCo 是否正確安裝
python -c "import mujoco; print(f'MuJoCo version: {mujoco.__version__}')"

# 測試 PyTorch
python -c "import torch; print(f'PyTorch version: {torch.__version__}')"

✅

好消息！新版 MuJoCo（3.x）不再需要額外的授權檔案（license key）。直接 pip install mujoco 就能使用，完全免費開源！

主要套件說明

套件	用途	必要性
`mujoco`	物理模擬	必要 — MuJoCo 官方 Python 套件
`numpy`	數值運算	必要 — 矩陣運算、數學計算
`torch`	深度學習	必要 — 建構 Q-Network 策略
`ikpy`	逆運動學	必要 — 將末端位置轉換為關節角度
`opencv-python`	影像處理	必要 — 相機影像擷取與處理
`gymnasium`	RL 環境介面	選用 — 接 Stable-Baselines3 時才需要

3. 系統架構

整體架構圖

UR5 機器手臂結構

4. MuJoCo 基礎 (Tutorial 01)

什麼是 MuJoCo？

MuJoCo（Multi-Joint dynamics with Contact）是一個高效的物理模擬引擎，專門用於機器人學、生物力學和強化學習研究。它能夠精確模擬剛體動力學、接觸力、摩擦等物理現象。

核心概念

MjModel（模型）

包含所有靜態資訊：

剛體結構（bodies）
關節定義（joints）
致動器（actuators）
幾何形狀（geoms）
物理參數（質量、慣性等）

從 XML 檔案載入，模擬過程中不會改變。

MjData（數據）

包含所有動態狀態：

qpos — 關節位置
qvel — 關節速度
ctrl — 控制輸入（馬達力矩）
xpos — 剛體世界座標
time — 模擬時間

每次 mj_step() 都會更新。

mj_step() 內部流程

呼叫 mujoco.mj_step(model, data) 時，MuJoCo 內部會執行以下管線：

💡

關鍵理解：mj_step() 每次呼叫只推進 一個時間步（dt=0.002 秒）。要讓機器人移動到目標位置，需要在迴圈中反覆呼叫數百到數千次，每次根據 PID 控制器更新 data.ctrl。 Renderer 是獨立的 — 它只是讀取當前 data 的狀態來渲染影像，不影響物理模擬。

基本程式碼

import mujoco
import mujoco.viewer

# 1. 載入模型（從 XML 檔案）
model = mujoco.MjModel.from_xml_path("UR5gripper_2_finger.xml")

# 2. 建立模擬數據
data = mujoco.MjData(model)

# 3. 查看模型資訊
print(f"關節數量: {model.njnt}")
print(f"致動器數量: {model.nu}")
print(f"時間步長: {model.opt.timestep}")

# 4. 執行模擬步進
mujoco.mj_step(model, data)
print(f"關節位置: {data.qpos[:7]}")

# 5. 啟動互動式檢視器
viewer = mujoco.viewer.launch_passive(model, data)
while viewer.is_running():
    mujoco.mj_step(model, data)
    viewer.sync()

💡

執行範例： python 01_hello_mujoco.py
你會看到 UR5 機器手臂的 3D 檢視器。用滑鼠拖曳旋轉視角，滾輪縮放。

MJCF XML 模型格式

MuJoCo 使用 MJCF（MuJoCo XML Format）來定義模型。以下是關鍵元素：

<!-- UR5 模型結構簡化版 -->
<mujoco model="ur5gripper">
  <!-- 編譯器設定 -->
  <compiler angle="radian" meshdir="mesh/visual/"/>

  <!-- 模擬參數 -->
  <option timestep="0.002"/>

  <!-- 世界主體 -->
  <worldbody>
    <!-- 相機 -->
    <camera name="top_down" pos="0 -0.6 2.0"/>

    <!-- 機器人本體（巢狀結構 = 運動鏈） -->
    <body name="base_link">
      <body name="shoulder_link">
        <joint name="shoulder_pan_joint" axis="0 0 1"/>
        <geom type="mesh" mesh="shoulder"/>
        <!-- ... 更多關節 ... -->
      </body>
    </body>

    <!-- 桌上物件 -->
    <body name="box_1">
      <joint type="slide"/> <!-- 自由移動 -->
      <geom type="box" size="0.02 0.02 0.02"/>
    </body>
  </worldbody>

  <!-- 致動器（馬達） -->
  <actuator>
    <motor name="shoulder_pan_T" joint="shoulder_pan_joint"/>
    <!-- ... 更多馬達 ... -->
  </actuator>
</mujoco>

5. 機器人控制 (Tutorial 02)

控制層級

低階控制：直接設定力矩

# 直接控制：設定每個馬達的控制訊號
data.ctrl[0] = 1.0   # 正力矩 -> 肩關節順時針旋轉
data.ctrl[0] = -1.0  # 負力矩 -> 肩關節逆時針旋轉
data.ctrl[6] = -1.0  # 關閉夾爪

# 執行一步模擬
mujoco.mj_step(model, data)

中階控制：PID 控制器

PID 控制器根據「目標值」與「當前值」的誤差，自動計算適當的力矩：

P (比例)：誤差越大，力矩越大
I (積分)：累積誤差，消除穩態誤差
D (微分)：抑制振盪，提高穩定性

# 建立 PID 控制器（使用模擬時間步長，非系統時鐘）
pid = SimPID(kp=7.0, ki=0.0, kd=1.1,
             setpoint=0.0, dt=model.opt.timestep)

# 控制迴圈
for step in range(5000):
    current_angle = data.qpos[joint_id]      # 讀取當前角度
    torque = pid(current_angle)               # PID 計算力矩
    data.ctrl[actuator_id] = torque           # 施加力矩
    mujoco.mj_step(model, data)                # 模擬步進

ℹ️

注意：本專案使用自製的 SimPID 類別，以模擬時間步長（dt=0.002s）計算微分項。標準的 simple_pid 套件使用系統時鐘，在無視窗高速模擬時會導致 PID 發散。

高階控制：末端執行器移動

from robot_controller import RobotController

controller = RobotController()

# 移動末端執行器到世界座標 [x, y, z]
controller.move_ee([0.0, -0.6, 0.95])

# 內部流程：
# 1. IK 逆運動學：[x,y,z] -> [q1,q2,q3,q4,q5] 關節角度
# 2. PID 控制：每個關節獨立追蹤目標角度
# 3. 模擬步進：直到所有關節到達目標（或超時）

逆運動學 (IK) 圖解

6. 相機與影像 (Tutorial 03)

RGB-D 觀測

視覺型 RL 的核心：從模擬相機擷取 RGB 彩色影像和深度圖。

RGB 影像

Depth 深度圖

# 擷取影像
rgb, depth_raw = controller.get_image(
    width=200, height=200, camera='top_down'
)

# 將深度轉換為公尺
depth_meters = controller.depth_to_meters(depth_raw)

# 新 MuJoCo API 的渲染方式：
renderer = mujoco.Renderer(model, height=200, width=200)
renderer.update_scene(data, camera='top_down')
rgb = renderer.render()       # RGB 影像

renderer.enable_depth_rendering()
renderer.update_scene(data, camera='top_down')
depth = renderer.render()     # 深度影像

像素-世界座標轉換

7. Gymnasium 環境 (Tutorial 04) Optional

Gymnasium 是什麼？

Gymnasium（原 OpenAI Gym）是強化學習的標準環境介面。所有 RL 演算法庫（Stable-Baselines3、CleanRL、RLlib 等）都使用這個介面。

掌握 Gymnasium API = 可以對接任何 RL 演算法！

ℹ️

注意：本節為選修內容。如果你的研究不直接使用 Gymnasium，可以先跳過此部分。核心概念（MuJoCo 模擬 + 機器人控制）在前面的章節已經涵蓋。

核心 API

自定義環境結構

import gymnasium as gym
from gymnasium import spaces

class SimpleGraspEnv(gym.Env):
    def __init__(self):
        # 定義觀測空間
        self.observation_space = spaces.Dict({
            'rgb': spaces.Box(0, 255, shape=(200,200,3), dtype=np.uint8),
            'depth': spaces.Box(0, 10, shape=(200,200), dtype=np.float32),
        })
        # 定義動作空間
        self.action_space = spaces.Discrete(40000)  # 200x200 像素

    def reset(self, seed=None):
        # 重置環境，回傳初始觀測
        obs = self._get_observation()
        return obs, {}

    def step(self, action):
        # 執行動作，回傳結果
        # 1. 解碼動作 (像素 -> 世界座標)
        # 2. 控制機器人執行抓取
        # 3. 計算獎勵
        return obs, reward, terminated, truncated, info

使用環境

# 建立環境
env = gym.make('SimpleGrasp-v0', render_mode='human')

# 重置
obs, info = env.reset(seed=42)

# 互動迴圈
for step in range(10):
    action = env.action_space.sample()   # 隨機動作
    obs, reward, terminated, truncated, info = env.step(action)
    print(f"Step {step}: reward={reward}")
    if terminated or truncated:
        obs, info = env.reset()

env.close()

Gymnasium vs 舊版 Gym 差異

舊版 (gym)

import gym
env = gym.make('Env-v0')
obs = env.reset()
obs, r, done, info = env.step(a)

新版 (gymnasium)

import gymnasium as gym
env = gym.make('Env-v0')
obs, info = env.reset(seed=42)
obs, r, term, trunc, info = env.step(a)

8. RL 強化學習基礎 (Tutorial 05)

強化學習核心概念

Q-Learning 演算法

Q-Learning 是最基本的 RL 演算法之一。核心思想：

Q(s, a)：在狀態 s 執行動作 a 的預期總獎勵
Epsilon-Greedy：以 epsilon 機率隨機探索，否則選最佳動作
更新規則：Q(s,a) = r + gamma * max Q(s', a')

# Epsilon-Greedy 動作選擇
if random.random() < epsilon:
    action = env.action_space.sample()       # 隨機探索
else:
    q_values = policy_net(state)              # 用神經網路預測 Q 值
    action = q_values.argmax().item()       # 選最大 Q 值的動作

訓練迴圈（Tutorial 05 實際使用的程式碼）

Tutorial 05 使用網格化動作空間：將桌面分為 5x5 = 25 格，每格代表一個抓取候選位置。狀態是每格的最小深度值（物件深度 < 桌面深度）。

for episode in range(NUM_EPISODES):
    state = env.reset()  # 重置機器人 + 隨機散佈物件

    for step in range(STEPS_PER_EPISODE):
        # 1. Epsilon-greedy 動作選擇
        if random.random() < epsilon:
            action = random.randint(0, 24)  # 隨機探索
        else:
            q = q_net(torch.FloatTensor(state))
            action = q.argmax().item()      # 選 Q 值最高的格子

        # 2. 在 MuJoCo 中執行抓取
        next_state, reward, done, info = env.step(action)

        # 3. 存入回放緩衝區
        buffer.push(state, action, reward, next_state, done)

        # 4. 從緩衝區取樣訓練 Q-network
        if len(buffer) >= BATCH_SIZE:
            train_step(buffer, q_net, optimizer)

        state = next_state
        if done: break

⚠️

為什麼成功率很低？10 回合只有 50 次抓取嘗試，5x5 網格太粗糙（每格約 24x18 像素，但物件只有 4~8 像素寬）。真正的機器人 RL 通常需要 1000+ 回合才能學到有用的策略。

9. API 遷移指南：舊版 vs 新版

本專案原本使用已廢棄的 mujoco-py 和 gym。以下是新舊 API 的對照表：

MuJoCo API 對照

功能	舊版 (mujoco-py)	新版 (mujoco)
匯入	`import mujoco_py as mp`	`import mujoco`
載入模型	`mp.load_model_from_path(path)`	`mujoco.MjModel.from_xml_path(path)`
建立模擬	`sim = mp.MjSim(model)`	`data = mujoco.MjData(model)`
模擬步進	`sim.step()`	`mujoco.mj_step(model, data)`
讀取狀態	`sim.data.qpos`	`data.qpos`
設定控制	`sim.data.ctrl[i] = v`	`data.ctrl[i] = v`
檢視器	`mp.MjViewer(sim)`	`mujoco.viewer.launch_passive(model, data)`
渲染影像	`sim.render(w, h, camera_name=c)`	`mujoco.Renderer(model, h, w)`
名稱查詢	`model.body_name2id("name")`	`mujoco.mj_name2id(model, TYPE, "name")`
剛體位置	`sim.data.body_xpos[id]`	`data.xpos[id]`

Gymnasium API 對照

功能	舊版 (gym)	新版 (gymnasium)
匯入	`import gym`	`import gymnasium as gym`
reset()	`obs = env.reset()`	`obs, info = env.reset(seed=42)`
step()	`obs, r, done, info`	`obs, r, terminated, truncated, info`
渲染模式	`env.render(mode='human')`	`gym.make(..., render_mode='human')`
環境類別	`class Env(gym.Env)`	`class Env(gym.Env)` (相同)

10. 練習題

練習 1: 探索模型 (Tutorial 01)

修改 01_hello_mujoco.py，列出所有 body 的世界座標位置。

提示：使用 data.xpos[i] 和 mujoco.mj_id2name()

練習 2: 軌跡控制 (Tutorial 02)

讓機器人依序移動到以下四個位置，畫出一個正方形：

positions = [
    [0.2, -0.5, 1.0],
    [-0.2, -0.5, 1.0],
    [-0.2, -0.7, 1.0],
    [0.2, -0.7, 1.0],
]

練習 3: 影像分析 (Tutorial 03)

擷取 top-down 相機影像，找出深度圖中最高的物件（深度最小的點），並將機器人移動到該位置上方。

提示：使用 np.argmin(depth_meters) 找最淺的像素

練習 4: 環境重置 (Tutorial 04)

修改 04_gymnasium_env.py 的 SimpleGraspEnv，加入以下功能：

在 step() 中加入夾爪旋轉：根據動作的第二維度旋轉 wrist_3 關節
在 reset() 中用 controller.reset_simulation() 確認物件被隨機散佈
觀察不同隨機種子下，物件位置如何變化

練習 5: 改善 RL 訓練 (Tutorial 05)

Tutorial 05 使用 5x5 網格，成功率很低。嘗試以下改善：

將 GRID_SIZE 從 5 改為 10（100 格，更精細）
增加 N_EPISODES 到 50 或 100
觀察成功率是否隨訓練回合數增加而提升

GRID_SIZE = 10   # 10x10 = 100 格
N_EPISODES = 50  # 更多探索

進階挑戰

挑戰: 完整抓取系統

結合所有學到的知識，建立一個完整的抓取系統：

設計一個 CNN 策略網路，直接從 RGB-D 影像輸出像素級 Q 值
加入 shaped reward（接近物件 +0.1，抓到 +1.0）
使用 target network 穩定 DQN 訓練
訓練 500+ 回合，繪製學習曲線

MuJoCo + RL ロボットシミュレーションチュートリアル

ゼロから学ぶ MuJoCo 物理シミュレーションと強化学習環境構築

UR5 ロボットアーム + 二指グリッパー対応

1. コース概要

学習目標

本チュートリアルでは、ゼロから以下のスキルを習得します：

MuJoCo 物理シミュレーション — モデルの読み込み、シミュレーション実行、状態取得
ロボット制御 — 関節制御、PID コントローラ、逆運動学
視覚観測 — カメラレンダリング、RGB-D 画像、座標変換
Gymnasium 環境 — カスタム RL 環境の標準インターフェース
強化学習の基礎 — Q-learning、経験リプレイ、訓練ループ

チュートリアルファイル構成

tutorial/
  01_hello_mujoco.py -- MuJoCo基礎：モデル読み込み、シミュレーション観察
  02_control_robot.py -- ロボット制御：モーター、PID、逆運動学
  03_camera_and_images.py -- カメラ画像：RGB-D、座標変換
  04_gymnasium_env.py -- RL環境概念：reset/step/reward
  05_simple_rl_training.py -- 実際のMuJoCo RL訓練：Q-network + グリッド行動
  robot_controller.py -- ロボットコントローラー（コアモジュール、reset_simulation含む）
  requirements.txt -- Python パッケージ依存関係

2. 環境構築

📋

前提条件：Python 3.8+、pip、基本的なターミナル操作スキル。
Anaconda または venv で仮想環境を作成することを推奨します。

Step 1: 仮想環境の作成

# conda を使用（推奨）
conda create -n mujoco_rl python=3.10
conda activate mujoco_rl

# または venv を使用
python -m venv mujoco_rl_env
# Windows:
mujoco_rl_env\Scripts\activate
# Linux/Mac:
source mujoco_rl_env/bin/activate

Step 2: 依存パッケージのインストール

# tutorial ディレクトリに移動
cd tutorial

# すべての依存パッケージをインストール
pip install -r requirements.txt

Step 3: インストールの確認

# MuJoCo が正しくインストールされたかテスト
python -c "import mujoco; print(f'MuJoCo version: {mujoco.__version__}')"

# PyTorch のテスト
python -c "import torch; print(f'PyTorch version: {torch.__version__}')"

✅

朗報！新版 MuJoCo（3.x）では追加のライセンスファイル（license key）が不要になりました。 pip install mujoco だけで使用可能で、完全に無料のオープンソースです！

主要パッケージの説明

パッケージ	用途	必要性
`mujoco`	物理シミュレーション	必須 — MuJoCo 公式 Python パッケージ
`numpy`	数値計算	必須 — 行列演算、数学計算
`torch`	深層学習	必須 — Q-Network ポリシーの構築
`ikpy`	逆運動学	必須 — エンドエフェクタ位置を関節角度に変換
`opencv-python`	画像処理	必須 — カメラ画像の取得と処理
`gymnasium`	RL環境インターフェース	選択 — Stable-Baselines3 連携時のみ必要

3. システムアーキテクチャ

全体アーキテクチャ図

UR5 ロボットアーム構造

4. MuJoCo基礎 (Tutorial 01)

MuJoCo とは？

MuJoCo（Multi-Joint dynamics with Contact）は高効率な物理シミュレーションエンジンで、ロボット工学、バイオメカニクス、強化学習研究に特化しています。剛体力学、接触力、摩擦などの物理現象を精密にシミュレーションできます。

コアコンセプト

MjModel（モデル）

すべての静的情報を含みます：

剛体構造（bodies）
関節定義（joints）
アクチュエータ（actuators）
幾何形状（geoms）
物理パラメータ（質量、慣性など）

XML ファイルから読み込まれ、シミュレーション中は変更されません。

MjData（データ）

すべての動的状態を含みます：

qpos — 関節位置
qvel — 関節速度
ctrl — 制御入力（モータートルク）
xpos — 剛体のワールド座標
time — シミュレーション時間

mj_step() のたびに更新されます。

mj_step() 内部パイプライン

mujoco.mj_step(model, data) を呼び出すと、MuJoCo 内部で以下のパイプラインが実行されます：

💡

重要なポイント：mj_step() は呼び出すたびに 1タイムステップ（dt=0.002秒）だけ進みます。ロボットを目標位置に移動させるには、ループ内で数百〜数千回繰り返し呼び出し、毎回 PID コントローラーに基づいて data.ctrl を更新する必要があります。 Renderer は独立しており — 現在の data の状態を読み取って画像を描画するだけで、物理シミュレーションには影響しません。

基本コード

import mujoco
import mujoco.viewer

# 1. モデルの読み込み（XML ファイルから）
model = mujoco.MjModel.from_xml_path("UR5gripper_2_finger.xml")

# 2. シミュレーションデータの作成
data = mujoco.MjData(model)

# 3. モデル情報の確認
print(f"関節数: {model.njnt}")
print(f"アクチュエータ数: {model.nu}")
print(f"タイムステップ: {model.opt.timestep}")

# 4. シミュレーションステップの実行
mujoco.mj_step(model, data)
print(f"関節位置: {data.qpos[:7]}")

# 5. インタラクティブビューアの起動
viewer = mujoco.viewer.launch_passive(model, data)
while viewer.is_running():
    mujoco.mj_step(model, data)
    viewer.sync()

💡

サンプルの実行： python 01_hello_mujoco.py
UR5 ロボットアームの 3D ビューアが表示されます。マウスドラッグで視点を回転、スクロールでズームできます。

MJCF XML モデルフォーマット

MuJoCo は MJCF（MuJoCo XML Format）を使用してモデルを定義します。以下が主要な要素です：

<!-- UR5 モデル構造の簡略版 -->
<mujoco model="ur5gripper">
  <!-- コンパイラ設定 -->
  <compiler angle="radian" meshdir="mesh/visual/"/>

  <!-- シミュレーションパラメータ -->
  <option timestep="0.002"/>

  <!-- ワールドボディ -->
  <worldbody>
    <!-- カメラ -->
    <camera name="top_down" pos="0 -0.6 2.0"/>

    <!-- ロボット本体（ネスト構造 = 運動連鎖） -->
    <body name="base_link">
      <body name="shoulder_link">
        <joint name="shoulder_pan_joint" axis="0 0 1"/>
        <geom type="mesh" mesh="shoulder"/>
        <!-- ... さらに関節が続く ... -->
      </body>
    </body>

    <!-- テーブル上のオブジェクト -->
    <body name="box_1">
      <joint type="slide"/> <!-- 自由移動 -->
      <geom type="box" size="0.02 0.02 0.02"/>
    </body>
  </worldbody>

  <!-- アクチュエータ（モーター） -->
  <actuator>
    <motor name="shoulder_pan_T" joint="shoulder_pan_joint"/>
    <!-- ... さらにモーターが続く ... -->
  </actuator>
</mujoco>

5. ロボット制御 (Tutorial 02)

制御の階層

低レベル制御：トルクの直接設定

# 直接制御：各モーターの制御信号を設定
data.ctrl[0] = 1.0   # 正トルク -> 肩関節が時計回りに回転
data.ctrl[0] = -1.0  # 負トルク -> 肩関節が反時計回りに回転
data.ctrl[6] = -1.0  # グリッパーを閉じる

# シミュレーション1ステップ実行
mujoco.mj_step(model, data)

中レベル制御：PID コントローラ

PID コントローラは「目標値」と「現在値」の誤差に基づいて、適切なトルクを自動計算します：

P (比例)：誤差が大きいほどトルクが大きい
I (積分)：累積誤差を解消し、定常偏差を除去
D (微分)：振動を抑制し、安定性を向上

# PID コントローラの作成（シミュレーションタイムステップを使用、システムクロックではない）
pid = SimPID(kp=7.0, ki=0.0, kd=1.1,
             setpoint=0.0, dt=model.opt.timestep)

# 制御ループ
for step in range(5000):
    current_angle = data.qpos[joint_id]      # 現在の角度を読み取り
    torque = pid(current_angle)               # PID でトルクを計算
    data.ctrl[actuator_id] = torque           # トルクを適用
    mujoco.mj_step(model, data)                # シミュレーションステップ

ℹ️

注意：本プロジェクトでは自作の SimPID クラスを使用し、シミュレーションタイムステップ（dt=0.002s）で微分項を計算しています。標準の simple_pid パッケージはシステムクロックを使用するため、ウィンドウなしの高速シミュレーションでは PID が発散する問題があります。

高レベル制御：エンドエフェクタの移動

from robot_controller import RobotController

controller = RobotController()

# エンドエフェクタをワールド座標 [x, y, z] に移動
controller.move_ee([0.0, -0.6, 0.95])

# 内部処理フロー：
# 1. IK 逆運動学：[x,y,z] -> [q1,q2,q3,q4,q5] 関節角度
# 2. PID 制御：各関節が独立して目標角度を追従
# 3. シミュレーションステップ：全関節が目標に到達（またはタイムアウト）まで

逆運動学 (IK) の図解

6. カメラと画像 (Tutorial 03)

RGB-D 観測

視覚ベース RL の核心：シミュレーションカメラから RGB カラー画像と深度マップを取得します。

RGB 画像

Depth 深度マップ

# 画像の取得
rgb, depth_raw = controller.get_image(
    width=200, height=200, camera='top_down'
)

# 深度をメートルに変換
depth_meters = controller.depth_to_meters(depth_raw)

# 新 MuJoCo API でのレンダリング方法：
renderer = mujoco.Renderer(model, height=200, width=200)
renderer.update_scene(data, camera='top_down')
rgb = renderer.render()       # RGB 画像

renderer.enable_depth_rendering()
renderer.update_scene(data, camera='top_down')
depth = renderer.render()     # 深度画像

ピクセル-ワールド座標変換

7. Gymnasium環境 (Tutorial 04) Optional

Gymnasium とは？

Gymnasium（旧 OpenAI Gym）は強化学習の標準環境インターフェースです。すべての RL アルゴリズムライブラリ（Stable-Baselines3、CleanRL、RLlib など）がこのインターフェースを使用しています。

Gymnasium API を習得すれば、あらゆる RL アルゴリズムと連携可能です！

ℹ️

注意：このセクションは選択科目（選修）です。研究で Gymnasium を直接使用しない場合は、このパートをスキップしても構いません。コアとなる概念（MuJoCo シミュレーション + ロボット制御）は前のセクションで既にカバーされています。

コア API

カスタム環境の構造

import gymnasium as gym
from gymnasium import spaces

class SimpleGraspEnv(gym.Env):
    def __init__(self):
        # 観測空間の定義
        self.observation_space = spaces.Dict({
            'rgb': spaces.Box(0, 255, shape=(200,200,3), dtype=np.uint8),
            'depth': spaces.Box(0, 10, shape=(200,200), dtype=np.float32),
        })
        # 行動空間の定義
        self.action_space = spaces.Discrete(40000)  # 200x200 ピクセル

    def reset(self, seed=None):
        # 環境をリセットし、初期観測を返す
        obs = self._get_observation()
        return obs, {}

    def step(self, action):
        # 行動を実行し、結果を返す
        # 1. 行動のデコード (ピクセル -> ワールド座標)
        # 2. ロボットを制御して把持を実行
        # 3. 報酬を計算
        return obs, reward, terminated, truncated, info

環境の使用方法

# 環境の作成
env = gym.make('SimpleGrasp-v0', render_mode='human')

# リセット
obs, info = env.reset(seed=42)

# インタラクションループ
for step in range(10):
    action = env.action_space.sample()   # ランダム行動
    obs, reward, terminated, truncated, info = env.step(action)
    print(f"Step {step}: reward={reward}")
    if terminated or truncated:
        obs, info = env.reset()

env.close()

Gymnasium vs 旧版 Gym の違い

旧版 (gym)

import gym
env = gym.make('Env-v0')
obs = env.reset()
obs, r, done, info = env.step(a)

新版 (gymnasium)

import gymnasium as gym
env = gym.make('Env-v0')
obs, info = env.reset(seed=42)
obs, r, term, trunc, info = env.step(a)

8. RL 強化学習の基礎 (Tutorial 05)

強化学習のコアコンセプト

Q-Learning アルゴリズム

Q-Learning は最も基本的な RL アルゴリズムの一つです。核心となる考え方：

Q(s, a)：状態 s で行動 a を実行した場合の期待累積報酬
Epsilon-Greedy：epsilon の確率でランダム探索、それ以外は最適行動を選択
更新則：Q(s,a) = r + gamma * max Q(s', a')

# Epsilon-Greedy 行動選択
if random.random() < epsilon:
    action = env.action_space.sample()       # ランダム探索
else:
    q_values = policy_net(state)              # ニューラルネットワークで Q 値を予測
    action = q_values.argmax().item()       # Q 値が最大の行動を選択

訓練ループ（Tutorial 05 で実際に使用するコード）

Tutorial 05 ではグリッド化行動空間を使用：テーブル面を 5x5 = 25 マスに分割し、各マスが把持候補位置を表します。状態は各マスの最小深度値（物体の深度 < テーブルの深度）です。

for episode in range(NUM_EPISODES):
    state = env.reset()  # ロボットリセット + 物体ランダム配置

    for step in range(STEPS_PER_EPISODE):
        # 1. Epsilon-greedy 行動選択
        if random.random() < epsilon:
            action = random.randint(0, 24)  # ランダム探索
        else:
            q = q_net(torch.FloatTensor(state))
            action = q.argmax().item()      # Q値が最大のマスを選択

        # 2. MuJoCo で把持を実行
        next_state, reward, done, info = env.step(action)

        # 3. リプレイバッファに保存
        buffer.push(state, action, reward, next_state, done)

        # 4. バッファからサンプリングして Q-network を訓練
        if len(buffer) >= BATCH_SIZE:
            train_step(buffer, q_net, optimizer)

        state = next_state
        if done: break

⚠️

なぜ成功率が低いのか？10 エピソードで 50 回の把持試行のみ、5x5 グリッドは粗すぎます（各マス約 24x18 ピクセル、しかし物体は 4〜8 ピクセル幅）。実際のロボット RL では通常 1000+ エピソードが必要です。

9. API移行ガイド：旧版 vs 新版

本プロジェクトは元々非推奨の mujoco-py と gym を使用していました。以下は新旧 API の対照表です：

MuJoCo API 対照表

機能	旧版 (mujoco-py)	新版 (mujoco)
インポート	`import mujoco_py as mp`	`import mujoco`
モデル読み込み	`mp.load_model_from_path(path)`	`mujoco.MjModel.from_xml_path(path)`
シミュレーション作成	`sim = mp.MjSim(model)`	`data = mujoco.MjData(model)`
シミュレーションステップ	`sim.step()`	`mujoco.mj_step(model, data)`
状態の読み取り	`sim.data.qpos`	`data.qpos`
制御の設定	`sim.data.ctrl[i] = v`	`data.ctrl[i] = v`
ビューア	`mp.MjViewer(sim)`	`mujoco.viewer.launch_passive(model, data)`
画像レンダリング	`sim.render(w, h, camera_name=c)`	`mujoco.Renderer(model, h, w)`
名前検索	`model.body_name2id("name")`	`mujoco.mj_name2id(model, TYPE, "name")`
剛体位置	`sim.data.body_xpos[id]`	`data.xpos[id]`

Gymnasium API 対照表

機能	旧版 (gym)	新版 (gymnasium)
インポート	`import gym`	`import gymnasium as gym`
reset()	`obs = env.reset()`	`obs, info = env.reset(seed=42)`
step()	`obs, r, done, info`	`obs, r, terminated, truncated, info`
レンダリングモード	`env.render(mode='human')`	`gym.make(..., render_mode='human')`
環境クラス	`class Env(gym.Env)`	`class Env(gym.Env)` (同じ)

10. 演習問題

演習 1: モデルの探索 (Tutorial 01)

01_hello_mujoco.py を修正して、すべての body のワールド座標位置を一覧表示してください。

ヒント：data.xpos[i] と mujoco.mj_id2name() を使用

演習 2: 軌道制御 (Tutorial 02)

ロボットを以下の4つの位置に順番に移動させ、正方形を描いてください：

positions = [
    [0.2, -0.5, 1.0],
    [-0.2, -0.5, 1.0],
    [-0.2, -0.7, 1.0],
    [0.2, -0.7, 1.0],
]

演習 3: 画像分析 (Tutorial 03)

top-down カメラ画像を取得し、深度マップで最も高いオブジェクト（深度が最小の点）を見つけて、ロボットをその位置の上方に移動させてください。

ヒント：np.argmin(depth_meters) で最も浅いピクセルを検索

演習 4: 環境リセット (Tutorial 04)

04_gymnasium_env.py の SimpleGraspEnv を修正して、以下の機能を追加してください：

step() でグリッパー回転を追加：行動の第二次元に基づいて wrist_3 関節を回転
reset() で controller.reset_simulation() を使用して物体がランダムに配置されることを確認
異なるランダムシードで物体の位置がどう変化するか観察

演習 5: RL 訓練の改善 (Tutorial 05)

Tutorial 05 は 5x5 グリッドを使用しており、成功率が非常に低いです。以下の改善を試してください：

GRID_SIZE を 5 から 10 に変更（100 マス、より精細）
N_EPISODES を 50 または 100 に増加
訓練エピソード数の増加に伴い成功率が向上するか観察

GRID_SIZE = 10   # 10x10 = 100 マス
N_EPISODES = 50  # より多くの探索

上級チャレンジ

チャレンジ: 完全な把持システム

学んだ知識をすべて組み合わせて、完全な把持システムを構築してください：

実際の MuJoCo 環境（SimpleGrasp-v0）を使用
RGB-D 画像を処理する CNN ポリシーネットワークを設計
完全な DQN 訓練ループを実装
訓練後の把持成功率を評価