MuJoCoでアームをシミュレーションする
MJCFで2リンクアームを作り、mujoco Pythonバインディングでヘッドレスにステップ実行します。関節とサイトの状態を読み出して順運動学と照合し、重力補償の有無でPDループを閉じます。
このレッスンで学ぶこと
- ヒンジ関節、モータアクチュエータ、先端のサイトをもつ平面2リンクアームのMJCFモデルを書く
- モデルを読み込んでヘッドレスでステップ実行し、mujoco Pythonバインディングからqpos、qvel、site_xposを読み出す
- シミュレーション上の先端位置が、レッスン3の順運動学と小数点以下4桁まで一致することを示す
- シミュレーションループ内で関節PDコントローラを動かし、重力補償の有無で定常偏差を測定する
- タイムステップと関節減衰がシミュレーション上のアームに与える影響を説明する
運動学からは、与えられた関節角に対してアームの先端がどこにあるかが分かります。しかし、モータが押し、重力が引き、関節に摩擦があるときに何が起きるかは分かりません。それには物理シミュレータが必要です。このレッスンは、順運動学とPID制御を学び終え、その両方をシミュレーション上のロボットで確かめたい読者向けです。MuJoCoで2リンクアームを作り、シミュレータの結果を自分で書いた運動学と照合し、そのまわりに関節空間のPDループを閉じます。すべてウィンドウを開かずにPythonスクリプトからヘッドレスで実行するので、ノートPCでもサーバでもCIランナーでも同じように動きます。
MuJoCoを一段落で
MuJoCo（Multi-Joint dynamics with Contact）は、ロボットアーム、脚式ロボット、人体モデルなどの多関節体を対象とする物理エンジンです。MuJoCoのドキュメントによれば、Roboti LLCが開発し、2021年10月にGoogle DeepMindが買収し、2022年5月にオープンソース化されました。モデルはXML形式のMJCFで記述し、公式のmujoco PythonパッケージがCライブラリをラップしています。このページのコードは、同パッケージのバージョン3.16.0で動作を確認しました。インストール方法と周辺のエコシステムはMuJoCoのプラットフォームページで、2つのモデル形式の比較はURDFとMJCFのチートシートで扱っています。
処理はすべて2つのPythonオブジェクトが担います。MjModelは、実行中に変化しないものすべてを保持します。運動学的ツリー、質量、関節の可動範囲、アクチュエータのゲイン、タイムステップです。MjDataは、状態とそこから計算されるものすべてを保持します。関節位置qpos、関節速度qvel、アクチュエータへの指令ctrl、ボディとサイトのワールド座標での位置、そして力です。MjDataを変更し、関数を呼び、MjDataを再び読む、という流れになります。
モデル
アームは鉛直なx-z平面内で動くので、重力が効きます。上腕リンクは長さ0.30 m、質量1.0 kg、前腕リンクは0.25 m、0.5 kgです。どちらもMJCFの基本形状の一つであるカプセルです。各関節は小さな粘性減衰をもつヒンジで、制御入力に等しいトルクを加えるモータアクチュエータがそれぞれに付いています。サイトは質量のないマーカー座標系です。先端に1つ置き、MuJoCoにそのワールド位置を報告させます。
レッスン3では、アームを水平なx-y平面に置き、リンク長を0.4 mと0.3 m、関節角をtheta1とtheta2と呼びました。ここでは重力が働くようにアームを鉛直なx-z平面に立て、リンクを短くし、角度はMuJoCoでの関節位置の呼び名に合わせてqとします。式は同じで、yの代わりにzを使います。最初のブロックではモデルを文字列として定義して読み込み、MuJoCoが計算した先端位置をこの順運動学と3つの関節構成で比較します。mj_forwardは、時間を進めずに現在の状態について運動学と動力学を計算します。qposを設定して位置を読むだけのときに使う関数です。
import mujoco
import numpy as np
L1, L2 = 0.30, 0.25 # link lengths, m
ARM_XML = f"""
<mujoco model="two_link_arm">
<option timestep="0.002" gravity="0 0 -9.81"/>
<worldbody>
<body name="upper" pos="0 0 0">
<joint name="shoulder" type="hinge" axis="0 -1 0" damping="0.05"/>
<geom type="capsule" fromto="0 0 0 {L1} 0 0" size="0.02" mass="1.0"/>
<body name="fore" pos="{L1} 0 0">
<joint name="elbow" type="hinge" axis="0 -1 0" damping="0.05"/>
<geom type="capsule" fromto="0 0 0 {L2} 0 0" size="0.015" mass="0.5"/>
<site name="tip" pos="{L2} 0 0" size="0.01"/>
</body>
</body>
</worldbody>
<actuator>
<motor name="shoulder_motor" joint="shoulder" ctrllimited="true" ctrlrange="-20 20"/>
<motor name="elbow_motor" joint="elbow" ctrllimited="true" ctrlrange="-10 10"/>
</actuator>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(ARM_XML)
data = mujoco.MjData(model)
print(f"nq={model.nq} nv={model.nv} nu={model.nu} timestep={model.opt.timestep} s")
tip_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "tip")
def fk(q1: float, q2: float) -> np.ndarray:
"""Planar forward kinematics in the x-z plane (lesson 3), angles from the +x axis towards +z."""
return np.array([L1 * np.cos(q1) + L2 * np.cos(q1 + q2),
L1 * np.sin(q1) + L2 * np.sin(q1 + q2)])
for q in [(0.0, 0.0), (0.5, 0.8), (1.2, -1.0)]:
data.qpos[:] = q
mujoco.mj_forward(model, data) # kinematics only, no time step
tip = data.site_xpos[tip_id]
print(f"q={q} MuJoCo tip x={tip[0]:.4f} z={tip[2]:.4f} FK x={fk(*q)[0]:.4f} z={fk(*q)[1]:.4f}")nq=2 nv=2 nu=2 timestep=0.002 s
q=(0.0, 0.0) MuJoCo tip x=0.5500 z=0.0000 FK x=0.5500 z=0.0000
q=(0.5, 0.8) MuJoCo tip x=0.3301 z=0.3847 FK x=0.3301 z=0.3847
q=(1.2, -1.0) MuJoCo tip x=0.3537 z=0.3293 FK x=0.3537 z=0.3293各ヒンジが位置と速度を1つずつもつので、nqとnvはどちらも2です（自由浮遊するボディなら、位置は3次元位置とw, x, y, zの順の四元数を合わせた7つ、速度は6つになります。2つの数が別々に管理されているのはそのためです）。nuは2つのモータに対応して2です。先端位置は、表示されたすべての桁で自分の運動学と一致しています。この一致は新しいモデルごとに確認する価値があります。ヒンジ軸の符号の誤りやボディのオフセット位置の誤りは、ここですぐに不一致として現れます。コントローラを動かし始めてからでは、はるかに見つけにくくなります。
シミュレーションを進める
mj_stepは状態を1タイムステップ進めます。デフォルトのステップは0.002 sなので、500回呼ぶと1秒分をシミュレーションします。内部でMuJoCoは重力、減衰、アクチュエータ、接触による力を計算し、関節加速度を解いて積分します。デフォルトの積分器は半陰的オイラー法で、関節減衰を陰的に扱います。そのため、強く減衰させた関節でもこのステップ幅で安定します。ステップ幅は、どのシミュレータでも速度と精度のトレードオフを決める主な要素です。半分にすれば、計算コストは2倍になります。
制御ループの形はいつも同じです。MjDataから状態を読み、指令を計算してdata.ctrlに書き込み、mj_stepを呼び、これを繰り返します。2つ目のブロックは単体で動くようにモデルを再掲し、2つの実験を行います。
1つ目の実験では、モータを切った状態でアームを水平から放し、5秒間落下させます。モデルの減衰（0.05 N m s/rad）で1回、その10倍の減衰でもう1回実行します。2つ目の実験では、PDコントローラtau = Kp (q_target - q) - Kd qdotで両関節を目標q = (0.8, 0.6) radへ動かします。そのままで1回、重力補償項を加えてもう1回実行します。モータの制御範囲により肩は20 N m、肘は10 N mに制限され、範囲外の指令はMuJoCoがクリップします。
import mujoco
import numpy as np
L1, L2 = 0.30, 0.25
ARM_XML = f"""
<mujoco model="two_link_arm">
<option timestep="0.002" gravity="0 0 -9.81"/>
<worldbody>
<body name="upper" pos="0 0 0">
<joint name="shoulder" type="hinge" axis="0 -1 0" damping="0.05"/>
<geom type="capsule" fromto="0 0 0 {L1} 0 0" size="0.02" mass="1.0"/>
<body name="fore" pos="{L1} 0 0">
<joint name="elbow" type="hinge" axis="0 -1 0" damping="0.05"/>
<geom type="capsule" fromto="0 0 0 {L2} 0 0" size="0.015" mass="0.5"/>
<site name="tip" pos="{L2} 0 0" size="0.01"/>
</body>
</body>
</worldbody>
<actuator>
<motor name="shoulder_motor" joint="shoulder" ctrllimited="true" ctrlrange="-20 20"/>
<motor name="elbow_motor" joint="elbow" ctrllimited="true" ctrlrange="-10 10"/>
</actuator>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(ARM_XML)
data = mujoco.MjData(model)
# 1. Passive: motors off, release the arm horizontal and let gravity and joint damping act.
for damping in (0.05, 0.5):
model.dof_damping[:] = damping
mujoco.mj_resetData(model, data) # qpos = 0: both links along +x, at rest
data.ctrl[:] = 0.0
while data.time < 5.0:
mujoco.mj_step(model, data)
print(f"passive, damping {damping} N m s/rad: after 5 s q1={data.qpos[0]:+.3f} rad "
f"q1dot={data.qvel[0]:+.3f} rad/s (hanging straight down is q1 = {-np.pi / 2:.3f})")
model.dof_damping[:] = 0.05
# 2. Joint PD control towards a target, with and without gravity compensation.
q_target = np.array([0.8, 0.6])
kp = np.array([40.0, 15.0]) # N m/rad
kd = np.array([4.0, 1.0]) # N m s/rad
def run(gravity_comp: bool, n_steps: int = 500, log_every: int = 10):
"""Simulate n_steps of 2 ms (1 s) and log (t, q1, q2) every log_every steps."""
mujoco.mj_resetData(model, data)
log = []
for step in range(n_steps + 1):
if step % log_every == 0:
log.append((round(data.time, 2), round(float(data.qpos[0]), 3), round(float(data.qpos[1]), 3)))
tau = kp * (q_target - data.qpos) - kd * data.qvel
if gravity_comp:
tau += data.qfrc_bias # gravity plus Coriolis terms at the current state
data.ctrl[:] = tau # MuJoCo clips this to ctrlrange
mujoco.mj_step(model, data)
return log
tip_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "tip")
for comp in (False, True):
log = run(comp)
err = np.round(q_target - data.qpos, 3) + 0.0
print(f"PD, gravity compensation {comp!s:5}: final q=({data.qpos[0]:.3f}, {data.qpos[1]:.3f}) "
f"error=({err[0]:+.3f}, {err[1]:+.3f}) rad, tip x={data.site_xpos[tip_id][0]:.3f} z={data.site_xpos[tip_id][2]:.3f} m")
print("target tip from FK: x={:.3f} z={:.3f} m".format(
L1 * np.cos(0.8) + L2 * np.cos(1.4), L1 * np.sin(0.8) + L2 * np.sin(1.4)))
print("t, q1, q2 with gravity compensation:", log)
print("q1 without compensation:", [row[1] for row in run(False)])passive, damping 0.05 N m s/rad: after 5 s q1=-1.156 rad q1dot=+0.724 rad/s (hanging straight down is q1 = -1.571)
passive, damping 0.5 N m s/rad: after 5 s q1=-1.570 rad q1dot=+0.000 rad/s (hanging straight down is q1 = -1.571)
PD, gravity compensation False: final q=(0.742, 0.590) error=(+0.058, +0.010) rad, tip x=0.280 z=0.446 m
PD, gravity compensation True : final q=(0.800, 0.600) error=(+0.000, +0.000) rad, tip x=0.252 z=0.462 m
target tip from FK: x=0.252 z=0.462 m
t, q1, q2 with gravity compensation: [(0.0, 0.0, 0.0), (0.02, 0.01, 0.079), (0.04, 0.075, 0.149), (0.06, 0.174, 0.232), (0.08, 0.283, 0.321), (0.1, 0.39, 0.404), (0.12, 0.485, 0.474), (0.14, 0.566, 0.529), (0.16, 0.632, 0.57), (0.18, 0.683, 0.597), (0.2, 0.722, 0.614), (0.22, 0.75, 0.624), (0.24, 0.769, 0.627), (0.26, 0.783, 0.628), (0.28, 0.791, 0.626), (0.3, 0.796, 0.622), (0.32, 0.799, 0.619), (0.34, 0.801, 0.615), (0.36, 0.802, 0.612), (0.38, 0.802, 0.609), (0.4, 0.801, 0.607), (0.42, 0.801, 0.605), (0.44, 0.801, 0.604), (0.46, 0.8, 0.603), (0.48, 0.8, 0.602), (0.5, 0.8, 0.601), (0.52, 0.8, 0.601), (0.54, 0.8, 0.6), (0.56, 0.8, 0.6), (0.58, 0.8, 0.6), (0.6, 0.8, 0.6), (0.62, 0.8, 0.6), (0.64, 0.8, 0.6), (0.66, 0.8, 0.6), (0.68, 0.8, 0.6), (0.7, 0.8, 0.6), (0.72, 0.8, 0.6), (0.74, 0.8, 0.6), (0.76, 0.8, 0.6), (0.78, 0.8, 0.6), (0.8, 0.8, 0.6), (0.82, 0.8, 0.6), (0.84, 0.8, 0.6), (0.86, 0.8, 0.6), (0.88, 0.8, 0.6), (0.9, 0.8, 0.6), (0.92, 0.8, 0.6), (0.94, 0.8, 0.6), (0.96, 0.8, 0.6), (0.98, 0.8, 0.6), (1.0, 0.8, 0.6)]
q1 without compensation: [0.0, 0.012, 0.076, 0.167, 0.266, 0.361, 0.447, 0.52, 0.579, 0.626, 0.661, 0.687, 0.706, 0.719, 0.728, 0.734, 0.738, 0.741, 0.742, 0.743, 0.743, 0.743, 0.743, 0.743, 0.743, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742, 0.742]数値から分かること
減衰。 0.05 N m s/radでは、5秒後もアームはまだ揺れており、q1 = -1.156 radを0.72 rad/sで通過しています。0.5 N m s/radでは-1.570 radで静止し、真下に垂れ下がっています。シミュレータの関節減衰は、減速機や軸受の摩擦を代わりに表すものです。実際の値は実機でのシステム同定から得ます。減衰が小さすぎるモデルはシミュレーションでは活発に動き、実機では違う振る舞いをします。これはsim-to-realギャップの原因の一つです。
重力補償なしのPD。 単純なPDループでは、肩が目標より0.058 rad（3.3度）下で静定します。静止時は速度項がゼロなので、コントローラが出すトルクはKpと誤差の積だけです。このトルクで重力に抗してアームを支えなければならないので、誤差はゼロになりません。手計算でも確かめられます。最終姿勢では、重力が肩に約2.31 N mのトルクを加えます（上腕リンクの1.0 kgが0.15 mの位置に、前腕の0.5 kgがその中心にあり、それぞれに9.81 m/s²と各リンクの角度の余弦を掛けたもの）。これをKp = 40 N m/radで割ると0.058 radになります。肘は前腕だけを支えるので、誤差はより小さい0.010 radです。先端は本来の位置から32 mmずれます。レッスン5では積分項で定常偏差を取り除きました。アームの場合は外乱が分かっているので、よりよい方法があります。
重力補償ありのPD。 data.qfrc_biasは、現在の状態で重力、コリオリ力、遠心力の影響を打ち消すのに必要な関節トルクです。これをPD出力に加えると、PD項はアームを支える必要がなくなり、誤差の修正だけを担います。最終誤差は小数点以下3桁でゼロになり、シミュレーション上の先端は順運動学の目標x = 0.252 m、z = 0.462 mに一致します。補償にシミュレータ自身のモデルを使うのは理想的な場合です。実機では推定した質量をもつモデルから補償量を求めるため、わずかな誤差が残ります。残りは積分項や適応推定で処理します。
肘はわずかにオーバーシュートし、0.628 radまで達してから静定します。これは関節間の連成によるものです。肩が加速すると反力トルクが前腕を振り、肘のPDがそれを抑え込まなければなりません。関節ごとに単独で調整したゲインは、関節が同時に動いた途端にこの連成の影響を受けます。一般的な調整手順はPIDチューニングのリファレンスにまとめています。
シミュレーションスクリプトの習慣
- モデルは文字列またはファイルとしてバージョン管理し、動力学を信頼する前に、最初のブロックのように自分の運動学と照合してください。
- 実験のたびに
mj_resetDataを呼んでください。MjDataは時刻を含め、前回の実行の状態を引き継ぎます。 - 値をスイープするときは、XMLを編集するのではなく、
model.dof_dampingのようなMjModelの配列を通して実行時にパラメータを変更してください。 - ログはシミュレーションより低いレートで記録してください。ここでは物理演算を500 Hz、ログを50 Hzで実行しました。
- 無人で実行するコードではビューアを開かないでください。このページのコードはすべてレンダリングなしで動きます。
演習
- タイムステップを0.02 sに設定し（
model.opt.timestep = 0.02）、PD実験をもう一度実行してください。応答はどうなるでしょうか。また、どのステップ幅で0.002 sでの実行結果と小数点以下2桁で一致しなくなるでしょうか。 - 2つの
motorアクチュエータをpositionアクチュエータに置き換えてください。肩はkp="40"とkv="4"、肘はkp="15"とkv="1"とします。data.ctrlで目標を指令し、定常偏差が単純なPDの結果と一致することを確認してください。 - 前腕のカプセルの質量を1.0 kgに上げて0.5 kgのペイロードを持たせ、補償項は元のモデルのコピーから計算したままにしてください。残る定常偏差を測定してください。
まとめ
- MuJoCoは固定されたモデル（
MjModel）と変化する状態（MjData）を分けています。状態を設定し、mj_forwardまたはmj_stepを呼び、結果をMjDataから読みます。 - 先端にサイトを置くと、そのワールド位置が
data.site_xposで得られます。自分の順運動学と照合すれば、軸やオフセットの誤りを早期に見つけられます。 mj_stepはmodel.opt.timestep（デフォルトは0.002 s）だけ時間を進めます。関節減衰は摩擦をモデル化したもので、シミュレーション上のアームの静定の仕方に大きく影響します。- 重力に抗してアームを保持するPDコントローラには、重力トルクを
Kpで割った定常偏差が残ります。ここでは肩で0.058 radです。 data.qfrc_biasを重力・コリオリ補償項として加えると、モデルが正確な場合はその誤差がなくなります。
さらに読む
- MuJoCoドキュメント「Overview」:
MjModel、MjData、ボディ、関節、サイト、アクチュエータの背後にある考え方。 - MuJoCoドキュメント「XML Reference」:
option/timestep、関節のdamping、motorとpositionアクチュエータを含む、MJCFのすべての要素と属性。 - MuJoCoドキュメント「Computation」: 運動方程式、バイアス力、積分器。
- Modern Robotics、Lynch・Park著、Cambridge University Press、2017年（第1版の改訂版、2019年プレプリント）: 第8章（Dynamics of Open Chains）でMuJoCoが解く運動方程式を導出し、第11章（Robot Control）で関節空間制御を扱っています。