ゆるふわフィジカルAI
MuJoCoでロボットアームを制御するとは、関節に「アクチュエータ」を追加し、PIDコントローラが計算した力を毎ステップ data.ctrl に流し込むことです。 MuJoCo入門で動かした振り子は重力に身を任せるだけでしたが、ここでは重力に逆らって目標の角度で止めるところまでやります。PID制御の実装で使った「質量1の物体」を、本物の物理エンジンの中の2関節アームに置き換えるのが今回のゴールです。
制御が効くまでの流れ
アームを定義
MJCFに関節とアクチュエータ
角度を読む
data.qposで今の関節角度
PIDで力を決める
目標との差からdata.ctrlへ
アクチュエータとは何か
アクチュエータとは、MuJoCoのモデルに「ここは外から力や位置を指定できる」と宣言する要素です。関節（joint）を定義しただけでは、重力や慣性に身を任せるだけの死んだ物体にしかなりません。<actuator> を足して初めて、data.ctrl という配列を通じて外部から力を与えられるようになります。
アクチュエータにはいくつか種類があり、ロボットアームでよく使うのは次の2つです。
motor（力・トルク指定）
- ctrlに入れた値がそのままトルクになる
- PID等の制御則は自分でPythonに書く
- この記事で使う方式
position（目標角度指定）
- ctrlに入れた値が目標角度（ラジアン）になる
- PD制御をMuJoCo内部が代わりに計算する
- 手早く動かしたいときに便利
なぜpositionを使わずmotorにするのか
position アクチュエータを使えばMuJoCoが内部でPD制御をやってくれるので、Pythonで書くことはなくなります。でもそれでは「PIDの3項が何をしているか」が見えません。この記事では motor（トルクをそのまま渡す）を使い、PID制御の実装のPIDコントローラをそのまま持ち込みます。
2関節アームをMJCFで定義する
肩（shoulder）と肘（elbow）、2つの回転関節（hinge）を持つ平面アームを作ります。それぞれに motor アクチュエータを1つずつ対応させます。
import mujoco
import numpy as np
XML = """
<mujoco>
<option gravity="0 0 -9.81"/>
<worldbody>
<light pos="0 0 3"/>
<geom type="plane" size="2 2 0.1" rgba="0.9 0.9 0.95 1"/>
<body name="upper_arm" pos="0 0 1.0">
<joint name="shoulder" type="hinge" axis="0 1 0"/>
<geom type="capsule" fromto="0 0 0 0.35 0 0" size="0.03" mass="1.0"/>
<body name="forearm" pos="0.35 0 0">
<joint name="elbow" type="hinge" axis="0 1 0"/>
<geom type="capsule" fromto="0 0 0 0.3 0 0" size="0.025" mass="0.6"/>
</body>
</body>
</worldbody>
<actuator>
<motor name="shoulder_motor" joint="shoulder" ctrlrange="-20 20"/>
<motor name="elbow_motor" joint="elbow" ctrlrange="-20 20"/>
</actuator>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(XML)
data = mujoco.MjData(model)
print("アクチュエータの数:", model.nu) # 2（shoulder, elbow）body の中に body をネストすることで、肩から肘がぶら下がる構造になります。これはURDFとはで扱った「link と joint の親子関係」と同じ考え方です。ctrlrange はこのモーターが出せる力の上限で、ここでは ±20 に制限しています。
PIDで目標角度まで動かす
PID制御の実装で作った PID クラスをそのまま使い、肩と肘それぞれに1つずつPIDを割り当てます。誤差（目標角度 − 今の角度）から力を計算し、data.ctrl に渡して1ステップ進める——このループを繰り返すだけです。
class PID:
def __init__(self, kp, ki, kd, dt):
self.kp, self.ki, self.kd, self.dt = kp, ki, kd, dt
self.integral = 0.0
self.prev_error = 0.0
def update(self, target, current):
error = target - current
self.integral += error * self.dt
derivative = (error - self.prev_error) / self.dt
self.prev_error = error
return self.kp * error + self.ki * self.integral + self.kd * derivative
dt = model.opt.timestep # 0.002秒
shoulder_pid = PID(kp=60.0, ki=20.0, kd=8.0, dt=dt)
elbow_pid = PID(kp=40.0, ki=15.0, kd=5.0, dt=dt)
target_shoulder = np.deg2rad(45) # 肩を45度に
target_elbow = np.deg2rad(-30) # 肘を-30度に
for step in range(4000):
shoulder_angle = data.qpos[0]
elbow_angle = data.qpos[1]
data.ctrl[0] = shoulder_pid.update(target_shoulder, shoulder_angle)
data.ctrl[1] = elbow_pid.update(target_elbow, elbow_angle)
mujoco.mj_step(model, data)実行すると、4000ステップ（timestepが0.002秒なので約8秒分のシミュレーション）でアームが目標の姿勢に収束します。
step= 0 shoulder= 0.0deg elbow= 0.0deg
step= 200 shoulder= 48.3deg elbow= -28.4deg
step=1000 shoulder= 47.9deg elbow= -30.1deg
step=3999 shoulder= 45.4deg elbow= -30.0deg
目標: shoulder=45.0deg, elbow=-30.0deg200ステップ目では目標を少し行き過ぎて（オーバーシュート）、そこから落ち着いていく様子が数字で見えます。これがまさにPID制御の実装で説明したP・I・Dの役割です。P（比例）だけでは重力による定常誤差が残り、I（積分）がそれを徐々に消し、D（微分）がオーバーシュートを抑えます。
ゲインを変えて壊してみる
kp を極端に大きく（例: 300）すると、アームは目標に向けて強く振られすぎて振動が収まらなくなります。逆に ki=0 にすると、重力の分だけ目標より少し低い角度で止まってしまいます（定常誤差）。数値を変えて壊してみると、3項の役割がはっきり体感できます。
画面で見る（ビューア）
数字だけでは味気ないので、ビューアで動きを確認しましょう。PIDループをそのまま mujoco.viewer.launch_passive に渡します。
import mujoco.viewer
with mujoco.viewer.launch_passive(model, data) as viewer:
while viewer.is_running():
shoulder_angle, elbow_angle = data.qpos[0], data.qpos[1]
data.ctrl[0] = shoulder_pid.update(target_shoulder, shoulder_angle)
data.ctrl[1] = elbow_pid.update(target_elbow, elbow_angle)
mujoco.mj_step(model, data)
viewer.sync()ウィンドウが開いたら、アームが震えながら目標の姿勢に落ち着いていくのが見えます。マウスでアームを少し押してバランスを崩してみると、PIDが押し戻してくる様子も観察できます。
つまずきやすい点
- アームが脱力してダランと落ちる →
<actuator>にその関節のモーターを書き忘れている可能性が高い。model.nu（アクチュエータの数）を確認する - 振動が収まらない →
kpが大きすぎる、またはkdが小さすぎる。まずkpを半分にしてみる - 目標よりわずかに低い角度で止まる → 重力による定常誤差。
kiを増やすか、ki=0のままにしているなら足す ctrlrangeを超えた値を入れても壊れない → MuJoCoはctrlrangeを超えたctrlを自動でクリップする。想定より弱い力しか出ていない場合はまずここを疑う
まとめ
- アクチュエータを
<actuator>に追加しないと、関節は外部から力を受け付けない motorアクチュエータはdata.ctrlの値がそのままトルクになる。PID等の計算はPython側で書く- PID制御の実装のPIDクラスは、トイモデルだけでなく本物の物理エンジン上の多関節アームにもそのまま使える
- P・I・Dそれぞれの役割（追従・定常誤差の解消・振動の抑制）は、ゲインを変えて壊してみると体感できる
よくある質問
Q. positionアクチュエータを使えばPIDを自分で書かなくて済むのでは？
A. はい、positionアクチュエータはMuJoCo内部でPD制御（PIDのI項を除いたもの）を行うため、目標角度をctrlに入れるだけで動きます。ただしI項がない分、重力による定常誤差を自分で消す手段がなく、仕組みを理解する教材としてはmotorアクチュエータ＋自作PIDの方が向いています。
Q. 実機のロボットアームでも同じコードが使えますか？
A. 考え方（誤差からPIDで力や目標値を計算するループ）は同じですが、実機では関節角度をセンサーやモーターのエンコーダから読み取り、計算した値をモータードライバに送る処理に置き換える必要があります。安全のため、まずはこの記事のようにシミュレータで十分にゲインを調整してから実機に移すのがおすすめです。
Q. 3関節以上のアームでも同じ方法で制御できますか？
A. はい。関節ごとに<actuator>とPIDインスタンスを1つずつ用意し、ループの中でそれぞれの誤差を計算してdata.ctrlに入れる、という構造を関節の数だけ繰り返すだけです。関節同士の力学的な干渉が増えるため、ゲイン調整はより難しくなります。
📚 もう少し詳しく（背景と理論）気になる人向け・タップで開く
MuJoCo（Multi-Joint dynamics with Contact）は Todorov らが2012年に発表した物理エンジンで、関節と接触を安定・高速に解けることから強化学習・制御研究の標準になりました1。この記事で使った motor アクチュエータは、ctrl の値をそのまま一般化力（トルク）として扱う最も単純な形で、MuJoCoには他に目標位置・目標速度を指定できる position / velocity アクチュエータや、ゲインや伝達特性を自分で定義できる general アクチュエータもあります2。
PIDのように「誤差から力を計算して毎ステップ与える」方式は、産業用ロボットの関節制御でも広く使われる古典的な手法です。より複雑なアームでは、関節同士の力学的な干渉（遠心力・コリオリ力）を打ち消す計算モデルベース制御（逆動力学）が使われることもありますが、まずはPIDで「狙った角度に落ち着く」体験をするのが理解の近道です。
次の一歩 🌸
実機のアームで同じことをやりたくなったらSO-101組み立てガイド、PIDの3項の仕組み自体をもっと深く知りたい場合はPID制御の実装へ戻ってどうぞ。模倣学習でアームの動かし方自体をAIに学ばせる方法はLeRobot入門で解説しています。