はじめに
Part 3「シミュレーション」に入ります。
ここまでの章でも MuJoCo を使ってきましたが、扱ってきたのは 何にも触れないアーム だけでした。第2章の「掴む」も、手先に物体を貼り付けるだけの簡易モデルでした。しかし、フィジカルAIの本題であるロボットの操作（マニピュレーション）は、物体に触れて、押して、掴む ことの連続です。
本章では、MuJoCo を本格的に使い、次の4つを身につけます。
- MuJoCo の基本構造（MjModel と MjData）と、MJCF の書き方
- 接触と摩擦 のモデル化と、接触力の読み出し方
- 2本指グリッパー で箱を掴んで持ち上げ、成否が物理法則どおりに決まることを確かめる
- カメラ画像（RGB・深度・セグメンテーション） を取得し、深度から物体の3次元位置を求める
この章で作る「掴める環境」と「カメラ画像」は、Part 4 の強化学習・模倣学習でそのまま土台になります。
前提
- 必要なライブラリは
mujoco、numpy、（画像を保存する場合）pillowです - 本章のコードは、ディレクトリ内で完結しています
1. なぜ MuJoCo なのか
MuJoCo（Multi-Joint dynamics with Contact）は、Google DeepMind が開発・保守しているオープンソースの物理エンジンです。名前のとおり、多関節の物体と接触 を高速かつ安定に計算することに重点を置いています。
本連載で MuJoCo をメインに使う理由は次のとおりです。
| 理由 | 内容 |
|---|---|
| 軽い | CPU だけで動き、pip install mujoco だけで使える |
| 研究で広く使われている | 強化学習のベンチマークや模倣学習の研究で、標準的な環境として採用されることが多い |
| ロボットモデルが豊富 | MuJoCo Menagerie に、品質が確認されたロボットモデルが多数公開されている1 |
Menagerie には、Franka Emika Panda のような研究用アーム、Unitree G1 のような人型ロボット、ALOHA の双腕システム、第20章で扱う予定の SO-101 などのモデルが含まれています2。物理シミュレーションの結果はモデルの質に大きく左右されるため、「動作を確認済みのモデルから始められる」ことは大きな利点です3。
2. MuJoCo の基本構造：MjModel と MjData
MuJoCo のプログラムは、2つのオブジェクトを中心に動きます。
| オブジェクト | 中身 | 変わるか | ソフトウェアでたとえると |
|---|---|---|---|
| MjModel | ロボットや環境の構造（リンクの長さ、質量、関節の種類、摩擦係数など） | シミュレーション中は基本的に変わらない | クラス定義・設定ファイル |
| MjData | 現在の状態（関節角 qpos、速度 qvel、接触点、センサー値など） |
毎ステップ更新される | インスタンス・実行時の状態 |
1つの MjModel から複数の MjData を作れば、同じロボットを、異なる状態で並列に動かす ことができます。強化学習で多数の環境を並べるときの基本的な考え方です。
よく使う関数は次の3つです。
| 関数 | やること |
|---|---|
mj_step(model, data) |
物理を1ステップ進める（力の計算 → 加速度 → 速度・位置の更新） |
mj_forward(model, data) |
時間は進めずに、現在の状態から位置・接触・力などを計算し直す |
mj_kinematics(model, data) |
運動学（各部の位置・姿勢）だけを計算する（第3章で使用） |
関節角を直接書き換えたあとは、mj_forward を呼んで 派生した量（手先の位置など）を計算し直す 必要があります。第0章から何度も書いてきた mujoco.mj_forward(model, data) は、このためのものでした。
3. MJCF の書き方
3.1 木構造と主な要素
MJCF は、<worldbody> を根とする body の入れ子構造 でロボットと環境を表します。子の body は、文字どおり XML の子要素として書きます4。
| 要素 | 役割 |
|---|---|
<body> |
剛体（1つのかたまりとして動く部品）。pos で親からの位置を指定 |
<joint> |
body と親の間に 動ける自由度 を作る。joint がない body は親に固定される |
<geom> |
形状。衝突判定と見た目 の両方に使われ、質量もここから計算される |
<site> |
位置を取得するための目印。衝突しない |
<camera> |
カメラ。body に付ければ一緒に動く |
<actuator> |
モーター。どの joint をどう動かすかを指定 |
第3章で説明したとおり、MuJoCo は body の木構造をたどって、同次変換を順に掛けて各部の位置を計算しています。
3.2 関節の種類と qpos のサイズ
| 関節 | 自由度 |
qpos の数 |
qvel の数 |
用途 |
|---|---|---|---|---|
hinge |
回転 1 | 1 | 1 | アームの関節 |
slide |
直動 1 | 1 | 1 | 直動ステージ、グリッパーの指 |
ball |
回転 3 | 4（クォータニオン） | 3 | 肩関節など |
free |
並進 3 + 回転 3 | 7（位置 3 + クォータニオン 4） | 6 | 自由に動く物体（掴む対象など） |
free 関節は、位置3つとクォータニオン4つ で物体の位置と姿勢を表します。クォータニオンは第3章で学んだ $(w, x, y, z)$ の順 です。回転の自由度は3なので、速度 qvel は 6 になり、qpos と qvel の長さが一致しなくなります（実践では nq=12、nv=11）。qpos の何番目がどの関節かは、model.jnt_qposadr で確認できます。
3.3 default クラス：CSS のような既定値
MJCF では、<default> に既定値をまとめて書けます。HTML に対する CSS のように、クラスを定義して、要素ごとに class="..." で適用します4。
<default>
<geom condim="4" friction="1 0.005 0.0001"/> <!-- すべての geom の既定値 -->
<default class="finger"> <!-- 指だけの既定値 -->
<joint type="slide" range="0 0.04"/>
<geom type="box" size="0.008 0.006 0.03"/>
</default>
</default>
同じ部品が何度も出てくるロボットでは、設定の重複を大きく減らせます。
3.4 アクチュエータの種類
| アクチュエータ | 入力（ctrl） |
中身 |
|---|---|---|
motor |
トルク・力 | 指令をそのまま力として加える（第0・5章で使用） |
position |
目標位置 | 物理エンジン内で PD 制御を行う（第4章で使用）。kp と、減衰の kv または減衰比 dampratio を指定 |
velocity |
目標速度 | 速度の比例制御 |
position アクチュエータの forcerange を使うと、出せる力の上限 を設定できます。グリッパーでは、これが 把持力の上限 になります。実践パートで、この設定が結果を大きく左右することを確認します。
また、<body gravcomp="1"> と書くと、その body にかかる重力を打ち消す力が自動的に加わります。第5章で自分で計算した 重力補償 を、MuJoCo に任せる機能です。
4. 接触と摩擦
4.1 MuJoCo の接触モデル
物体どうしの接触は、物理シミュレーションで最も難しい部分の1つです。完全に硬い物体どうしの接触は、ぶつかった瞬間に速度が不連続に変わるため、数値的に扱いにくいからです。
MuJoCo は、ほんのわずかなめり込みを許す「柔らかい接触」 のモデルを採用しています。接触を凸最適化問題として解くことで、安定性と計算速度を両立しています5。このため、実践の結果でも、物体がサブミリメートル程度めり込んだり、止まっているはずの物体がごくわずかに動いたりします。
接触の硬さは solref（ばねの時定数と減衰比）と solimp（めり込みに対する硬さの変化）で調整できますが、まずは既定値のままで問題ありません。
4.2 摩擦のパラメータ
geom の friction 属性には、3つの摩擦係数を指定します6。
| 値 | 摩擦の種類 | 抵抗する動き | 典型値 |
|---|---|---|---|
| 1つ目 | すべり摩擦 | 面に沿ってすべる動き | 0.5〜1 程度 |
| 2つ目 | ねじり摩擦 | 接触面の法線まわりに回る動き | 0.005 程度 |
| 3つ目 | 転がり摩擦 | 転がる動き | 0.0001 程度 |
どの摩擦を計算に含めるかは、condim（接触の次元）で決まります。
condim |
含まれる力 | 用途 |
|---|---|---|
| 1 | 法線方向（めり込まない）だけ | 摩擦なし |
| 3 | + すべり摩擦（接線2方向） | 一般的な接触（既定値） |
| 4 | + ねじり摩擦 | 把持（掴んだ物体が指の間で回るのを防ぐ） |
| 6 | + 転がり摩擦 | ボールや車輪 |
2つの geom が接触したときの摩擦係数は、既定では 2つの geom の値の大きいほう が使われます。
4.3 摩擦円錐
すべり摩擦の大きさには上限があり、法線力（押し付ける力）を $N$、摩擦係数を $\mu$ とすると、摩擦力 $F \le \mu N$ です。
- 物体が止まっているとき（静止摩擦）：摩擦力はちょうど外力を打ち消す大きさで、$F \le \mu N$
- 物体がすべっているとき（動摩擦）：摩擦力は上限の $F = \mu N$
接線方向の2成分を合わせた摩擦力が $\mu N$ 以下という条件は、力の空間で 円錐 の形になるため、摩擦円錐 と呼ばれます。MuJoCo では、これを角錐で近似する cone="pyramidal"（既定値）と、円錐のまま扱う cone="elliptic" を選べます。本章では、物理的に忠実な elliptic を使います。
4.4 接触情報の読み出し
シミュレーション中の接触は、data.contact に入っています。
for i in range(data.ncon): # 現在の接触点の数
c = data.contact[i] # c.geom1, c.geom2, c.pos（位置）, c.frame（向き）
f6 = np.zeros(6)
mujoco.mj_contactForce(model, data, i, f6) # [法線力, 接線力1, 接線力2, ねじり, 転がり...]
力は 接触座標系（1軸目が接触面の法線方向）で返ってきます。接触力は、把持の成否の判定、触覚センサーのシミュレーション、強化学習の報酬設計などに使います。
5. 把持の物理
5.1 持ち上げられる条件
2本の指で箱を左右から挟んで持ち上げる場合を考えます。指が箱を押し付ける力（把持力）を $F$ とすると、左右の指それぞれで摩擦力の上限は $\mu F$ です。箱を落とさずに持ち上げるには、摩擦力の合計が、箱の重さと加速に必要な力を上回る必要があります。
$$
2 \mu F ;\ge; m (g + a)
$$
- $m$：箱の質量、$g$：重力加速度、$a$：持ち上げるときの上向きの加速度
ここから、把持の設計についていくつかのことがわかります。
- 滑りやすい物体ほど、強く握る必要がある（$\mu$ が小さいと $F$ を大きく）
- 速く持ち上げるほど、強く握る必要がある（$a$ が大きいと必要な力が増える）
- 壊れやすい物体は強く握れないので、ゆっくり持ち上げる 必要がある
5.2 position アクチュエータで把持力を決める
実践では、指を position アクチュエータで動かし、目標位置を箱の内側（閉じきった位置） に設定します。指は箱に当たって止まり、目標位置とのずれに比例した力で箱を押し付けます。
$$
F = \min\left(K_p \times (\text{目標位置と現在位置のずれ}),; \text{forcerange の上限}\right)
$$
第5章で説明した「目標位置と実際の位置のずれが、PD 制御を通じて力になる」の具体例です。実践パートでは、ここで1つ落とし穴にはまります。
6. カメラとレンダリング
6.1 オフスクリーンレンダリング
フィジカルAIの方策の多くは、カメラ画像 を入力にします。MuJoCo では、mujoco.Renderer を使って、ウィンドウを開かずに画像を取得（オフスクリーンレンダリング）できます7。
with mujoco.Renderer(model, height, width) as r:
r.update_scene(data, camera="top")
rgb = r.render() # (H, W, 3) の uint8 配列
同じ Renderer で、次の3種類の画像が取れます。
| 画像 | 切り替え | 中身 |
|---|---|---|
| RGB | 既定 | カラー画像 |
| 深度 | enable_depth_rendering() |
各画素の、カメラの光軸方向の距離 [m] |
| セグメンテーション | enable_segmentation_rendering() |
各画素に写っている geom の ID |
深度とセグメンテーションは、シミュレータだから追加コストなしで手に入る正解ラベル です。実機では深度カメラや人手のアノテーションが必要な情報が、自動で得られます。第14章の合成データでも、この性質を活用します。
6.2 画面表示のない環境での設定
レンダリングには OpenGL が必要です。MuJoCo では、環境変数 MUJOCO_GL で描画のバックエンドを選びます。この変数は、import mujoco より前 に設定する必要があります8。
MUJOCO_GL |
使う場面 |
|---|---|
glfw |
画面のある PC（Windows / macOS / Linux デスクトップ）。既定値 |
egl |
画面のない Linux サーバーで、GPU を使って描画する（クラウド GPU、Colab など） |
osmesa |
画面も GPU もない環境で、CPU で描画する（遅い） |
本章の実践コードは、OpenGL が使えない環境でも動くように、レイキャスト（画素ごとに光線を飛ばして、最初に当たる物体と距離を求める mj_ray）で深度とセグメンテーションを計算する代替経路を用意しています。
6.3 カメラの内部パラメータ
深度画像から3次元の点を求めるには、カメラの 内部パラメータ（焦点距離 $f_x, f_y$ と画像中心 $c_x, c_y$）が必要です。MuJoCo のカメラは縦の画角 fovy で定義されているので、画像の高さ $H$ から焦点距離を次のように求めます。
$$
f = \frac{H / 2}{\tan(\text{fovy} / 2)}
$$
画素 $(u, v)$ の深度が $z$ のとき、カメラ座標系での3次元の点は次のようになります。
$$
x = \frac{u - c_x}{f_x}, z, \qquad y = \frac{v - c_y}{f_y}, z
$$
6.4 カメラ座標系の約束事に注意
ここで、第3章の「座標系の約束事」がまた登場します。カメラ座標系には、2つの代表的な流儀があります。
| 流儀 | x | y | 視線方向 | 使われる場所 |
|---|---|---|---|---|
| OpenCV | 右 | 下 | +z | OpenCV、ROS、多くのコンピュータビジョンのライブラリ |
| OpenGL | 右 | 上 | −z | MuJoCo、3D グラフィックス |
第3章のカメラの例は OpenCV の流儀でしたが、MuJoCo のカメラは OpenGL の流儀 です。そのため、上の式で求めた点は、y の符号を反転し、z を $-z$ にしてから、カメラの姿勢 data.cam_xmat と位置 data.cam_xpos でワールド座標に変換します。ここを間違えると、物体の位置が上下反転したり、カメラの後ろ側に計算されたりします。
7. 実践：掴めるシミュレーション環境を作る
7.1 作るもの
ch06_mujoco_basics/
├── scene.py # XYZ ステージ + 2本指グリッパー + 箱 のシーン
├── contact_friction.py # 斜面の箱で、摩擦と接触力を確かめる
├── grasp.py # 掴んで持ち上げる。軌道・摩擦・把持力を変えて比較
└── camera.py # RGB・深度・セグメンテーション → 箱の3次元位置
ロボットには、第3〜5章の回転関節のアームではなく、XYZ 方向に直動するステージ（ガントリー）の先に2本指グリッパーを付けたもの を使います。IK を解かずに手先の位置を直接指定できるので、本章のテーマである 接触と把持 に集中できます。実際の工場でも、この構成の「直交ロボット」はよく使われています。
7.2 シーンの定義：scene.py
"""第6章: XYZ ステージ + 2本指グリッパー + 箱 の把持シーン（MJCF を文字列で組み立てる）"""
import mujoco
TEMPLATE = """
<mujoco model="gripper_scene">
<compiler angle="radian"/>
<option timestep="0.002" gravity="0 0 -9.81" cone="elliptic" integrator="implicitfast"/>
<visual><global offwidth="640" offheight="480"/></visual>
<!-- CSS のように既定値をまとめて指定する -->
<default>
<geom condim="4" friction="1 0.005 0.0001"/>
<default class="stage">
<joint type="slide" damping="20" armature="0.1"/>
<position kp="2000" dampratio="1"/>
</default>
<default class="finger">
<joint type="slide" damping="5" armature="0.01" range="0 0.04"/>
<!-- 指の把持力 = min(kp × 位置の誤差, forcerange)。kp を大きくして forcerange で力を決める -->
<position kp="3000" dampratio="1" ctrlrange="0 0.04" forcerange="-@GRIP@ @GRIP@"/>
<geom type="box" size="0.008 0.006 0.03" rgba="0.3 0.3 0.3 1" friction="@MU@ 0.005 0.0001"/>
</default>
</default>
<asset>
<texture name="grid" type="2d" builtin="checker" rgb1="0.85 0.85 0.85" rgb2="0.7 0.7 0.7" width="256" height="256"/>
<material name="grid" texture="grid" texrepeat="8 8"/>
</asset>
<worldbody>
<light pos="0 0 2" dir="0 0 -1" diffuse="0.8 0.8 0.8"/>
<geom name="floor" type="plane" size="1 1 0.1" material="grid"/>
<!-- 固定カメラ：真上と斜め前 -->
<camera name="top" pos="0 0 1.0" xyaxes="1 0 0 0 1 0" fovy="60"/>
<camera name="front" pos="0.6 -0.6 0.5" xyaxes="0.707 0.707 0 -0.3 0.3 0.905" fovy="50"/>
<!-- XYZ ステージ（ガントリー）：重力補償（gravcomp）付き -->
<body name="stage_x" pos="0 0 0.3" gravcomp="1">
<joint name="x" class="stage" axis="1 0 0" range="-0.3 0.3"/>
<geom type="box" size="0.02 0.02 0.02" mass="1" rgba="0.6 0.6 0.6 0.3" contype="0" conaffinity="0"/>
<body name="stage_y" gravcomp="1">
<joint name="y" class="stage" axis="0 1 0" range="-0.3 0.3"/>
<geom type="box" size="0.02 0.02 0.02" mass="1" rgba="0.6 0.6 0.6 0.3" contype="0" conaffinity="0"/>
<body name="hand" gravcomp="1">
<joint name="z" class="stage" axis="0 0 1" range="-0.28 0.2"/>
<geom name="palm" type="box" size="0.02 0.06 0.01" mass="0.5" rgba="0.2 0.4 0.8 1"/>
<site name="tcp" pos="0 0 -0.07" size="0.005"/>
<!-- 2本の指：y 方向に開閉。qpos = 指の内側の面と中心線の距離（0 で閉じきる） -->
<body name="left_finger" pos="0 0.008 -0.04" gravcomp="1">
<joint name="left" class="finger" axis="0 1 0"/>
<geom name="left_pad" class="finger" mass="0.05"/>
</body>
<body name="right_finger" pos="0 -0.008 -0.04" gravcomp="1">
<joint name="right" class="finger" axis="0 -1 0"/>
<geom name="right_pad" class="finger" mass="0.05"/>
</body>
</body>
</body>
</body>
<!-- 掴む対象：自由に動く箱（freejoint） -->
<body name="box" pos="0.1 0.05 0.02">
<freejoint name="box"/>
<geom name="box" type="box" size="0.02 0.02 0.02" mass="@MASS@" rgba="0.9 0.2 0.2 1"
friction="@MU@ 0.005 0.0001"/>
</body>
</worldbody>
<actuator>
<position name="x" joint="x" class="stage" ctrlrange="-0.3 0.3"/>
<position name="y" joint="y" class="stage" ctrlrange="-0.3 0.3"/>
<position name="z" joint="z" class="stage" ctrlrange="-0.28 0.2"/>
<position name="left" joint="left" class="finger"/>
<position name="right" joint="right" class="finger"/>
</actuator>
</mujoco>
"""
def make(mu=1.0, grip_force=5.0, box_mass=0.1):
"""摩擦係数・把持力の上限・箱の質量を変えたシーンを作る"""
xml = (TEMPLATE.replace("@MU@", str(mu))
.replace("@GRIP@", str(grip_force))
.replace("@MASS@", str(box_mass)))
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
data.qpos[3:5] = 0.04 # 指は開いた状態から始める
data.ctrl[3:5] = 0.04
mujoco.mj_forward(model, data)
return model, data
if __name__ == "__main__":
model, data = make()
print(f"body 数 {model.nbody} / joint 数 {model.njnt} / 自由度 nq={model.nq}, nv={model.nv} "
f"/ actuator 数 {model.nu} / camera 数 {model.ncam}")
for j in range(model.njnt):
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, j)
jtype = mujoco.mjtJoint(model.jnt_type[j]).name
print(f" joint {j}: {name:6s} {jtype:12s} qpos の位置 {model.jnt_qposadr[j]}")
print("箱の初期位置 qpos[5:8] =", data.qpos[5:8], " 姿勢（wxyz）qpos[8:12] =", data.qpos[8:12])
python scene.py
body 数 7 / joint 数 6 / 自由度 nq=12, nv=11 / actuator 数 5 / camera 数 2
joint 0: x mjJNT_SLIDE qpos の位置 0
joint 1: y mjJNT_SLIDE qpos の位置 1
joint 2: z mjJNT_SLIDE qpos の位置 2
joint 3: left mjJNT_SLIDE qpos の位置 3
joint 4: right mjJNT_SLIDE qpos の位置 4
joint 5: box mjJNT_FREE qpos の位置 5
箱の初期位置 qpos[5:8] = [0.1 0.05 0.02] 姿勢（wxyz）qpos[8:12] = [1. 0. 0. 0.]
3.2 節で説明したとおり、自由に動く箱（free 関節）の qpos は 7 個（位置3 + クォータニオン4）あり、nq=12 に対して nv=11 になっています。箱の姿勢の初期値は [1, 0, 0, 0]、つまり $(w, x, y, z)$ の順の「回転なし」です。
主な設計のポイントは次のとおりです。
-
gravcomp="1"：ステージとグリッパーの重さを MuJoCo が打ち消すので、position アクチュエータは位置合わせだけに集中できます（第5章の重力補償） -
armature：第5章で学んだとおり、軽い部品の数値的な不安定さを防ぐために、モーター側の慣性を入れています -
指の
forcerange：把持力の上限です。make()の引数で変えられます -
condim="4"：把持した箱が指の間で回らないように、ねじり摩擦まで含めています
7.3 摩擦と接触力：contact_friction.py
斜面に箱を置いて、すべるかどうかと、そのときの接触力を調べます。斜面の傾きを $\theta$ とすると、箱がすべり出す条件は $\tan\theta > \mu$ です。
"""第6章 (1): 斜面に置いた箱で、摩擦と接触力を確かめる"""
import mujoco
import numpy as np
XML = """
<mujoco>
<option timestep="0.002" cone="elliptic"/>
<worldbody>
<!-- y 軸まわりに傾けた斜面 -->
<geom name="slope" type="box" size="2 0.3 0.01" euler="0 @TILT@ 0" friction="@MU@ 0.005 0.0001"/>
<body name="box" pos="0 0 0.05" euler="0 @TILT@ 0">
<freejoint/>
<geom name="box" type="box" size="0.03 0.03 0.03" mass="0.2" friction="@MU@ 0.005 0.0001"/>
</body>
</worldbody>
</mujoco>
"""
def simulate(tilt_deg, mu, T=0.5):
xml = XML.replace("@TILT@", str(tilt_deg)).replace("@MU@", str(mu)) # euler は既定で「度」
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
start = data.qpos[:3].copy()
for _ in range(int(0.2 / model.opt.timestep)): # 着地して落ち着くまで
mujoco.mj_step(model, data)
settled = data.qpos[:3].copy()
# 接触情報を読む：data.contact に接触点が入っている
normal_total, tangent_total = 0.0, 0.0
f6 = np.zeros(6)
for i in range(data.ncon):
mujoco.mj_contactForce(model, data, i, f6) # 接触座標系での力 [法線, 接線1, 接線2, ...]
normal_total += f6[0]
tangent_total += np.linalg.norm(f6[1:3])
ncon = data.ncon
for _ in range(int(T / model.opt.timestep)): # その後 T 秒でどれだけ滑るか
mujoco.mj_step(model, data)
slide = np.linalg.norm(data.qpos[:3] - settled)
return slide, ncon, normal_total, tangent_total
if __name__ == "__main__":
m, g = 0.2, 9.81
print("理論：tanθ > μ なら滑る\n")
print(f"{'傾き':>4s} {'μ':>4s} {'tanθ':>6s} {'0.5秒で滑った距離':>12s} {'接触点数':>6s} "
f"{'法線力 N':>8s} {'摩擦力 F':>8s} {'F / N':>6s}")
for tilt, mu in [(20, 0.5), (30, 0.5), (30, 0.7), (40, 0.7)]:
slide, ncon, fn, ft = simulate(tilt, mu)
th = np.deg2rad(tilt)
print(f"{tilt:3d}° {mu:4.1f} {np.tan(th):6.3f} {slide * 100:10.2f} cm {ncon:6d} "
f"{fn:7.3f} N {ft:7.3f} N {ft / fn:6.3f}")
print(f"\n参考：止まっている場合の理論値 20°: N={m*g*np.cos(np.deg2rad(20)):.3f}, F={m*g*np.sin(np.deg2rad(20)):.3f}"
f" / 30°: N={m*g*np.cos(np.deg2rad(30)):.3f}, F={m*g*np.sin(np.deg2rad(30)):.3f}")
python contact_friction.py
理論：tanθ > μ なら滑る
傾き μ tanθ 0.5秒で滑った距離 接触点数 法線力 N 摩擦力 F F / N
20° 0.5 0.364 0.05 cm 4 1.839 N 0.671 N 0.365
30° 0.5 0.577 14.82 cm 2 0.748 N 0.374 N 0.500
30° 0.7 0.577 0.09 cm 4 1.697 N 0.982 N 0.579
40° 0.7 0.839 23.98 cm 2 3.384 N 2.369 N 0.700
参考：止まっている場合の理論値 20°: N=1.844, F=0.671 / 30°: N=1.699, F=0.981
すべてのケースで、理論どおりの結果になりました。
- 止まっているケース（20°・μ=0.5、30°・μ=0.7）：法線力と摩擦力が理論値（$mg\cos\theta$、$mg\sin\theta$）とほぼ一致し、摩擦力と法線力の比 $F/N$ は $\tan\theta$ になっています。静止摩擦は「すべらないのに必要な分だけ」働き、上限 $\mu N$ には達していません
- すべっているケース（30°・μ=0.5、40°・μ=0.7）：$F/N$ が ちょうど $\mu$（0.500、0.700）になっています。すべっているときは、摩擦力が摩擦円錐の縁（上限 $\mu N$）に張り付くことが確認できます
止まっているケースでも、0.5 秒で 0.05〜0.09 cm だけ動いています。これは 4.1 節で説明した、MuJoCo の 柔らかい接触 による、ごくわずかなすべりです。
角度の単位に注意
MJCF の euler などの角度は、既定では 度 です（<compiler angle="radian"/> でラジアンに切り替えられます）。最初の実装では、このファイルでラジアンの値を渡してしまい、斜面がほとんど傾かずに「どの条件でもすべらない」という結果になりました。エラーは出ないので、気づきにくいバグです。scene.py ではラジアンを明示的に指定しています。
7.4 掴んで持ち上げる：grasp.py
「箱の真上へ移動 → 下ろす → 指を閉じる → 持ち上げる」の4段階を、スクリプトで実行します。
"""第6章 (2): スクリプトで「近づく → 下ろす → 掴む → 持ち上げる」を実行し、成否を判定する"""
import mujoco
import numpy as np
from scene import make
def waypoints(box_xy):
"""(x, y, z, 指の開き, 所要時間[s])。z はステージの関節値（0 = 初期の高さ）"""
x, y = box_xy
return [
(x, y, 0.000, 0.04, 1.0), # 1. 箱の真上へ移動（指は開く）
(x, y, -0.225, 0.04, 1.0), # 2. 下ろす（指が箱の側面の高さに来る）
(x, y, -0.225, 0.00, 1.0), # 3. 指を閉じる（目標 0 = 箱より内側 → 把持力の上限で押し付ける）
(x, y, 0.000, 0.00, 1.5), # 4. 持ち上げる
]
def quintic(s):
"""第5章の5次多項式（0→1 をなめらかにつなぐ）"""
s = np.clip(s, 0.0, 1.0)
return 10 * s**3 - 15 * s**4 + 6 * s**5
def run(mu=1.0, grip_force=5.0, box_mass=0.1, smooth=True, verbose=False):
model, data = make(mu, grip_force, box_mass)
box_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "box")
dt = model.opt.timestep
prev = data.ctrl.copy()
max_box_h = 0.0
for i, (x, y, z, w, T) in enumerate(waypoints(data.xpos[box_id][:2])):
goal = np.array([x, y, z, w, w])
n = int(T / dt)
for k in range(n):
if smooth: # 所要時間の 8 割で目標まで補間し、残りは保持
data.ctrl[:] = prev + quintic(k / (0.8 * n)) * (goal - prev)
else: # ステップ指令：目標をいきなり渡す
data.ctrl[:] = goal
mujoco.mj_step(model, data)
max_box_h = max(max_box_h, data.xpos[box_id][2])
prev = goal
if verbose:
print(f" step{i + 1}: 手先 z={data.qpos[2]:+.3f} 指={data.qpos[3]:.4f}/{data.qpos[4]:.4f} m"
f" 箱の高さ={data.xpos[box_id][2]:.3f} m 指の力={data.actuator_force[3]:+.2f} N")
lifted = data.xpos[box_id][2]
return lifted, lifted > 0.15, max_box_h # 15cm 以上持ち上がっていれば成功
if __name__ == "__main__":
print("=== 1. ステップ指令 vs なめらかな軌道（μ=1.0, 把持力 5 N, 箱 100 g）===")
for smooth, name in [(False, "ステップ指令"), (True, "5次多項式")]:
print(f"[{name}]")
h, ok, hmax = run(smooth=smooth, verbose=True)
print(f" → 結果: {'成功' if ok else '失敗'}（箱の最高到達点 {hmax:.3f} m）\n")
print("=== 2. 摩擦係数と把持力を変える（5次多項式の軌道）===")
m, g = 0.1, 9.81
a_max = 10 / np.sqrt(3) * 0.225 / (0.8 * 1.5) ** 2 # 5次多項式で持ち上げるときの最大加速度
need = m * (g + a_max)
print(f"箱の重さ mg = {m * g:.2f} N、持ち上げの最大加速度 {a_max:.2f} m/s² → 必要な摩擦力 m(g+a) = {need:.2f} N")
print("理論：2 μ F ≥ m(g+a) なら滑らずに持ち上がる\n")
print(f"{'μ':>5s} {'把持力F':>7s} {'2μF':>7s} {'理論':>6s} {'箱の最終高さ':>10s} {'結果':>4s}")
for mu, F in [(1.0, 5.0), (0.3, 5.0), (0.1, 6.0), (0.1, 5.0), (0.1, 3.0), (0.05, 12.0), (0.05, 5.0)]:
h, ok, _ = run(mu, F, m)
theory = "持てる" if 2 * mu * F >= need else "滑る"
print(f"{mu:5.2f} {F:5.1f} N {2 * mu * F:5.2f} N {theory:>6s} {h:9.3f} m {'成功' if ok else '失敗'}")
python grasp.py
=== 1. ステップ指令 vs なめらかな軌道（μ=1.0, 把持力 5 N, 箱 100 g）===
[ステップ指令]
step1: 手先 z=+0.000 指=0.0400/0.0400 m 箱の高さ=0.020 m 指の力=-0.00 N
step2: 手先 z=-0.225 指=0.0400/0.0400 m 箱の高さ=0.020 m 指の力=+0.00 N
step3: 手先 z=-0.225 指=0.0174/0.0174 m 箱の高さ=0.020 m 指の力=-5.00 N
step4: 手先 z=-0.000 指=0.0000/-0.0000 m 箱の高さ=0.020 m 指の力=+0.00 N
→ 結果: 失敗（箱の最高到達点 0.206 m）
[5次多項式]
step1: 手先 z=+0.000 指=0.0400/0.0400 m 箱の高さ=0.020 m 指の力=-0.00 N
step2: 手先 z=-0.225 指=0.0400/0.0400 m 箱の高さ=0.020 m 指の力=-0.00 N
step3: 手先 z=-0.225 指=0.0174/0.0174 m 箱の高さ=0.020 m 指の力=-5.00 N
step4: 手先 z=-0.000 指=0.0174/0.0174 m 箱の高さ=0.242 m 指の力=-5.00 N
→ 結果: 成功（箱の最高到達点 0.243 m）
=== 2. 摩擦係数と把持力を変える（5次多項式の軌道）===
箱の重さ mg = 0.98 N、持ち上げの最大加速度 0.90 m/s² → 必要な摩擦力 m(g+a) = 1.07 N
理論：2 μ F ≥ m(g+a) なら滑らずに持ち上がる
μ 把持力F 2μF 理論 箱の最終高さ 結果
1.00 5.0 N 10.00 N 持てる 0.242 m 成功
0.30 5.0 N 3.00 N 持てる 0.242 m 成功
0.10 6.0 N 1.20 N 持てる 0.243 m 成功
0.10 5.0 N 1.00 N 滑る 0.020 m 失敗
0.10 3.0 N 0.60 N 滑る 0.020 m 失敗
0.05 12.0 N 1.20 N 持てる 0.243 m 成功
0.05 5.0 N 0.50 N 滑る 0.020 m 失敗
① ステップ指令だと、箱が飛んでいく
最初に作ったバージョンでは、各段階の目標位置を いきなり アクチュエータに渡していました（ステップ指令）。結果を見ると、指を閉じた時点までは同じなのに、持ち上げると 箱が 0.206 m の高さまで飛び上がり、指から離れて床に落ちて います。
持ち上げの瞬間、ステージは目標との大きなずれに対して大きな力を出し、手先に 数百 m/s² という加速度 がかかります。5.1 節の式でいえば $a$ が巨大になり、摩擦力では箱を支えきれません。箱は指からすっぽ抜け、勢いで上に飛び出したのです。
第5章の 5次多項式の軌道 で目標をなめらかに動かすと、同じ設定で箱を 0.242 m まで持ち上げることに成功しました。軌道生成は、把持の成否そのものを左右する ことがわかります。
② 成否は 2μF ≥ m(g+a) で決まる
5次多項式の軌道で、摩擦係数と把持力を変えた結果です。持ち上げの最大加速度は約 0.90 m/s² なので、必要な摩擦力は $m(g + a) \approx 1.07$ N です。
| 条件 | 2μF | 理論 | 結果 |
|---|---|---|---|
| μ=1.0、F=5 N | 10.0 N | 持てる | 成功 |
| μ=0.1、F=6 N | 1.2 N | 持てる | 成功 |
| μ=0.1、F=5 N | 1.0 N | 滑る | 失敗 |
| μ=0.05、F=12 N | 1.2 N | 持てる | 成功 |
| μ=0.05、F=5 N | 0.5 N | 滑る | 失敗 |
7つの条件すべてで、理論と結果が一致しました。特に μ=0.1・F=5 N のケースは、箱の重さ（0.98 N）だけなら持てるはずの力（1.0 N）がありますが、持ち上げる加速度の分（0.09 N）が足りずに失敗 しています。$a$ の項が実際に効いていることがわかります。また、μ=0.05 でも、把持力を 12 N に上げれば持ち上げられます。
③ つまずいたポイント：把持力は forcerange だけでは決まらない
最初のバージョンでは、指のアクチュエータの kp を 500 にしていました。この設定では、μ=0.05 で把持力の上限を 12 N や 20 N に上げても、なぜか必ず失敗 しました。
原因は 5.2 節の式にありました。指は目標位置（0）から約 0.017 m 手前で箱に当たって止まるため、実際の把持力は $K_p \times 0.017 = 500 \times 0.017 \approx 8.6$ N にしかなりません。forcerange をいくら大きくしても、8.6 N を超えることはなかった のです。$2 \times 0.05 \times 8.6 = 0.86$ N は必要な 1.07 N に届かないので、失敗は理論どおりでした。
kp を 3000 に上げて、$K_p \times$ ずれ が常に forcerange を上回るようにすると、把持力が forcerange で決まるようになり、結果が理論と一致しました。「設定したつもりの力」と「実際に出ている力」が違う というのは、シミュレーションでも実機でもよくある落とし穴です。data.actuator_force で実際の力を確認する習慣をつけましょう。
7.5 カメラ画像から箱の位置を求める：camera.py
真上のカメラ（高さ 1 m、fovy=60°）の画像から、深度とセグメンテーションだけを使って 箱の3次元位置を推定します。
"""第6章 (3): カメラ画像（RGB・深度・セグメンテーション）を取得し、深度から箱の3次元位置を求める
画面のない環境では、import より前に環境変数 MUJOCO_GL を設定する:
Linux（NVIDIA GPU）: MUJOCO_GL=egl / CPU のみ: MUJOCO_GL=osmesa
Windows / macOS（画面あり）: 設定不要
OpenGL が使えない環境では、レイキャスト（mj_ray）で深度とセグメンテーションを計算する代替経路で動く。
"""
import mujoco
import numpy as np
from scene import make
W, H = 320, 240
def intrinsics(model, cam_id, width, height):
"""MuJoCo の fovy（縦の画角）から、ピンホールカメラの内部パラメータを求める"""
fovy = np.deg2rad(model.cam_fovy[cam_id])
f = 0.5 * height / np.tan(fovy / 2) # 焦点距離 [pixel]
return f, f, (width - 1) / 2, (height - 1) / 2 # fx, fy, cx, cy
def render_gl(model, data, cam):
"""OpenGL で RGB・深度・セグメンテーションを描画する"""
with mujoco.Renderer(model, H, W) as r:
r.update_scene(data, camera=cam)
rgb = r.render()
r.enable_depth_rendering(); r.update_scene(data, camera=cam)
depth = r.render() # カメラの光軸方向の距離 [m]
r.disable_depth_rendering()
r.enable_segmentation_rendering(); r.update_scene(data, camera=cam)
seg = r.render()[..., 0] # 各画素に写っている geom の ID
return rgb, depth, seg
def render_raycast(model, data, cam):
"""OpenGL なしの代替：画素ごとにレイを飛ばして深度と geom ID を求める（遅いが確実）"""
cid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, cam)
fx, fy, cx, cy = intrinsics(model, cid, W, H)
pos, R = data.cam_xpos[cid], data.cam_xmat[cid].reshape(3, 3)
depth = np.zeros((H, W)); seg = -np.ones((H, W), dtype=int)
gid = np.zeros(1, dtype=np.int32)
for v in range(H):
for u in range(W):
# MuJoCo のカメラ座標系：x = 右、y = 上、-z = 視線方向（OpenGL の慣習）
d_cam = np.array([(u - cx) / fx, -(v - cy) / fy, -1.0])
d_world = R @ (d_cam / np.linalg.norm(d_cam))
dist = mujoco.mj_ray(model, data, pos, d_world, None, 1, -1, gid)
if dist >= 0:
depth[v, u] = dist * (-d_cam[2] / np.linalg.norm(d_cam)) # 光軸方向の距離に直す
seg[v, u] = gid[0]
return None, depth, seg
def depth_to_points(model, data, cam, depth, mask):
"""深度画像の画素 → カメラ座標 → ワールド座標の3次元点（第3章の同次変換と同じ考え方）"""
cid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, cam)
fx, fy, cx, cy = intrinsics(model, cid, W, H)
v, u = np.nonzero(mask)
z = depth[v, u]
p_cam = np.stack([(u - cx) / fx * z, -(v - cy) / fy * z, -z], axis=1) # OpenGL 慣習
R, t = data.cam_xmat[cid].reshape(3, 3), data.cam_xpos[cid]
return p_cam @ R.T + t # p_world = R p_cam + t
if __name__ == "__main__":
model, data = make()
box_geom = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "box")
cam = "top"
try:
rgb, depth, seg = render_gl(model, data, cam)
mode = "OpenGL"
except Exception as e: # OpenGL が使えない環境
print(f"OpenGL が使えないため、レイキャストで代替します（{type(e).__name__}）")
rgb, depth, seg = render_raycast(model, data, cam)
mode = "レイキャスト"
mask = seg == box_geom
pts = depth_to_points(model, data, cam, depth, mask)
print(f"[{mode}] 画像 {W}x{H} / 深度の範囲 {depth[depth > 0].min():.3f}〜{depth.max():.3f} m "
f"/ 箱が写っている画素数 {mask.sum()}")
print("深度から推定した箱の上面の中心:", np.median(pts, axis=0).round(4), "（外れ値に強い中央値）")
print("シミュレータの真値（箱の中心） :", data.xpos[mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'box')].round(4),
"→ 上面は z = 0.04")
# 画像を保存（Pillow があれば）
try:
from PIL import Image
d8 = (255 * (1 - (depth - depth.min()) / (np.ptp(depth) + 1e-9))).astype(np.uint8)
Image.fromarray(d8).save("depth_top.png")
Image.fromarray((mask * 255).astype(np.uint8)).save("mask_top.png")
if rgb is not None:
Image.fromarray(rgb).save("rgb_top.png")
print("depth_top.png / mask_top.png" + (" / rgb_top.png" if rgb is not None else "") + " を保存しました")
except ImportError:
pass
# 画面のある PC
python camera.py
# 画面のない Linux サーバー（GPU あり）
MUJOCO_GL=egl python camera.py
筆者の検証環境（OpenGL が使えないサーバー）では、レイキャストの代替経路で実行され、次の結果になりました。
OpenGL が使えないため、レイキャストで代替します（FatalError）
[レイキャスト] 画像 320x240 / 深度の範囲 0.680〜1.000 m / 箱が写っている画素数 81
深度から推定した箱の上面の中心: [0.0993 0.0485 0.04 ] （外れ値に強い中央値）
シミュレータの真値（箱の中心） : [0.1 0.05 0.02] → 上面は z = 0.04
depth_top.png / mask_top.png を保存しました
セグメンテーションで「箱が写っている画素」だけを取り出し、その深度から計算した3次元の点の中央値は (0.099, 0.049, 0.040) でした。真値は箱の中心が (0.1, 0.05, 0.02) で、上から見えるのは上面（z = 0.04）なので、ほぼ正確に箱の上面の中心を求められています。
この処理は、実機での「カメラで物体を見つけて、そこに手を伸ばす」の基本形です。実機では、セグメンテーションの部分を物体検出やセグメンテーションのモデルに置き換えます。そして、こうして得た位置に手を伸ばすのが、第4章の IK の役割です。
OpenGL 経路の動作確認について
筆者の検証環境には OpenGL がなかったため、render_gl の経路（mujoco.Renderer による描画）は実行して確認できていません。画面のある PC や、EGL が使える環境で実行すると、rgb_top.png も保存されます。OpenGL の深度画像は、光の届く範囲（near / far）の設定などにより、レイキャストとわずかに値が異なる場合があります。
7.6 演習
-
contact_friction.pyで、cone="elliptic"をcone="pyramidal"に変えて、すべり始める角度や $F/N$ の値が変わるか確かめてみましょう。 -
grasp.pyで、箱の質量を 0.3 kg に変えると、必要な把持力はいくつになるでしょうか。計算してから、シミュレーションで確かめてみましょう。 -
grasp.pyの持ち上げ時間を 1.5 秒から 0.5 秒に縮めると、5.1 節の $a$ はどう変わり、成否の境目はどう動くでしょうか。 -
scene.pyの箱のcondimを 3 に変え、箱を x 方向にずらした位置（指の中心から外れた位置）で掴んで持ち上げてみましょう。箱が指の間で回転するか観察してください。 -
camera.pyの推定位置を使って、grasp.pyのwaypointsの目標位置を決めるように書き換えてみましょう。カメラで見て、掴みに行く ロボットになります。 - （発展）
pip install robot_descriptionsなどで MuJoCo Menagerie のモデルを取得し、Franka Emika Panda のモデルを読み込んで、本章と同じ箱を掴ませてみましょう2。第4章の差分 IK が必要になります。
8. まとめ
- MuJoCo は、構造を表す MjModel と、状態を表す MjData に分かれている。関節角を書き換えたら
mj_forwardで派生量を計算し直す -
MJCF は body の入れ子構造で書き、joint で自由度を、geom で形状・衝突・質量を、actuator でモーターを定義する。default クラス で既定値をまとめられる。
free関節のqposは位置3 + クォータニオン4 - MuJoCo の接触は、わずかなめり込みを許す 柔らかい接触。摩擦は
friction（すべり・ねじり・転がり）とcondimで決まり、すべっているときは摩擦力が 摩擦円錐の縁 $\mu N$ に張り付くことを確かめた - 把持の成否は $2\mu F \ge m(g + a)$ でほぼ決まる。ステップ指令で持ち上げると加速度が大きすぎて箱が飛ぶ ため、軌道生成が把持の成否を左右する。把持力は
kp × ずれとforcerangeの小さいほうで決まることにも注意する -
mujoco.Rendererで RGB・深度・セグメンテーション を取得できる。画面のない環境ではMUJOCO_GLを設定する。MuJoCo のカメラは OpenGL の流儀（y が上、視線が −z） であることに注意して、深度から3次元の点を求められる
次回予告
MuJoCo は CPU で手軽に動かせる一方、強化学習で 数千の環境を並列に動かして大量の試行錯誤をする には、GPU を使ったシミュレーションが有利です。
第7章「Isaac Sim / Isaac Lab」 では、NVIDIA の GPU シミュレーション環境を扱います。なぜロボット学習で GPU 並列シミュレーションが重要なのか、Isaac Sim と Isaac Lab の役割の違い、そしてローカルに RTX GPU がない場合にクラウド GPU で動かす方法を紹介します。
参考文献
-
Google DeepMind, "MuJoCo Menagerie". https://github.com/google-deepmind/mujoco_menagerie ↩
-
MuJoCo Menagerie, "franka_emika_panda". https://github.com/google-deepmind/mujoco_menagerie/tree/main/franka_emika_panda ↩ ↩2
-
MuJoCo Documentation, "Model Gallery". https://mujoco.readthedocs.io/en/stable/models.html ↩
-
MuJoCo Documentation, "Modeling". https://mujoco.readthedocs.io/en/stable/modeling.html ↩ ↩2
-
MuJoCo Documentation, "Computation". https://mujoco.readthedocs.io/en/latest/computation/index.html ↩
-
MuJoCo Documentation, "XML Reference"（
geomのfriction/condim属性、optionのcone属性）. https://mujoco.readthedocs.io/en/stable/XMLreference.html ↩ -
MuJoCo Documentation, "Python". https://mujoco.readthedocs.io/en/stable/python.html ↩
-
TorchRL Documentation, "Working with MuJoCo-based environments". https://docs.pytorch.org/rl/stable/reference/generated/knowledge_base/MUJOCO_INSTALLATION.html ↩