作るもの

このプロジェクトでは、入門レベルの3つの考え方を1つのプログラムにまとめます。実行時間は1秒未満です。数行のMJCFで定義した平面2リンクアームが、先端にペンを持ちます。このアームに、デカルト座標で表した正方形を与えます。プログラムは各頂点と辺上の点を閉形式の逆運動学で関節角に変換し、その角度をシミュレーション上の2つの位置サーボに渡します。そしてMuJoCo物理エンジンのステップを進めながら、ペンが実際にどこを通ったかを記録します。最後に追従誤差と描いた軌跡を出力し、描画速度を変えて誤差の原因を示します。

結果として、一辺160 mmの正方形が、80 mm/sで約4 mmの一定の遅れを伴って描かれます。この遅れは逆運動学のバグではありません。原因はサーボで、その大きさはモデルファイル中の2つの数値から予測できます。これを導くことが、このプロジェクトの主な学びです。

事前に順運動学と逆運動学のレッスンを読んでおいてください。ここで使うMuJoCoの呼び出しはMuJoCoでアームをシミュレーションするのレッスンで詳しく扱っています。各サーボ内部の比例微分則はPID制御で説明しています。

必要なもの

Python 3.10以降とnumpy・mujoco（MuJoCo 3.x、pip install mujoco numpy）。筆者らはApple Silicon搭載のMac miniで、MuJoCo 3.16.0とNumPy 2.5.4を使って実行しました。
ほかには何も要りません。モデルはインライン文字列で、ビューアは使わず、ダウンロードもありません。
設計を順に追い、最後の変更例も試す場合は約2時間。プログラム自体の実行は1秒を大きく下回ります。

設計

幾何と逆運動学

アームはワールド座標系の水平なx-y平面上にあります。2つの関節はどちらもz軸まわりのヒンジです。そのため重力（z軸の負方向）は関節に負荷をかけず、問題は純粋に運動学のものになります。リンク1は0.30 m、リンク2は0.25 mです。肩の角度q1はワールドのx軸から測ります。肘の角度q2はリンク1から測ります。コード中の角度の単位はラジアンです。

上から見た2リンクアームと目標の正方形。2つの関節はどちらも紙面の手前向きのz軸まわりに回転する。図のアームは、q1 = -9.1度、q2 = 74.6度で正方形の右上の頂点に到達している。

目標点 (x, y) に対して、肘の角度は余弦定理から求まります。c2 = (x² + y² - L1² - L2²) / (2 L1 L2) とすると、肘の角度は q2 = atan2(±√(1 - c2²), c2) です。符号で2つの解のどちらかを選びます。肩からペンへの直線に対して、肘が一方の側にあるか、もう一方の側にあるかです。|c2| が1より大きい場合、その点は作業領域の外にあり、関数はエラーを送出します。肩の角度は q1 = atan2(y, x) - atan2(L2 sin q2, L1 + L2 cos q2) で求まります。

実用上、2つの選択が重要です。1つ目は、すべての経由点で同じ肘の解を使うことです。辺の途中でソルバが解を切り替えると、サーボは肘を大きく振り、ペンは正方形から外れます。2つ目は、シミュレーションの前に順運動学で逆運動学の結果を確認することです。プログラムはこの確認を行い、最大の不一致を出力します。この値は浮動小数点の丸め誤差程度になるはずです。

正方形の中心は (0.32, 0.10) m です。肘が正の側の解を使うと、肩の角度は-49.0度から-9.1度、肘の角度は74.6度から129.1度の範囲になります。これにより、肘は0度と180度から十分に離れます。0度と180度ではアームが完全に伸びるか折りたたまれ、ヤコビ行列のランクが落ちます（特異点）。

制御と予想される遅れ

プログラム内のデータの流れ。逆運動学は経由点ごとに1回実行する。関節の参照値は2 msの物理ステップごとに補間する。誤差はデカルト空間で、同じ速さで正方形に沿って動く参照点に対して測定する。

各関節にはMuJoCoのpositionアクチュエータがあります。このアクチュエータは kp (q_ref - q) - kv dq/dt のトルクを加えます。これは比例微分則で、微分項は計測した速度に作用します。肩は kp = 400 N m/rad、kv = 20 N m s/rad を使います。肘はそれぞれその半分です。各関節には、さらに0.5 N m s/radの受動的な減衰があります。

ここで、一定の速さで動く参照値に関節が追従する場合を考えます。運動が落ち着くと加速度はゼロなので、サーボのトルクは減衰トルクをちょうど打ち消さなければなりません。つまり kp e = (kv + 0.5) dq/dt で、e は遅れです。両辺を速さで割ると、遅れは (kv + 0.5) / kp の一定の時間遅れになります。肩では 20.5 / 400 = 51 ms です。肘では 10.5 / 200 で、こちらも約51 msです。両関節が同じ時間だけ遅れるので、ペンは正しい経路をたどりますが、参照より約51 ms遅れた位置にあります。80 mm/sでは約4.1 mmです。シミュレーション結果はこの見積もりと一致するはずで、最後の速度スイープでそれを検証します。

間違えやすい2つの設定

積分器。 MuJoCoのデフォルトの積分器は半陰的オイラー法です。MuJoCo XMLリファレンスは、アクチュエータがkvを使う場合には常にimplicitfastまたはimplicit積分器を推奨しています。筆者らはこのプロジェクトを書く中でその理由を知りました。デフォルトの積分器では、2 msのステップとリンク2の小さな慣性のために、肘の減衰項が陽的積分の安定限界に近くなります。関節は高速に振動し、角度は参照値の近くにとどまる一方で、速度は数十rad/sに達しました。平均誤差は一見もっともらしい値のままでしたが、最も遅い2つの速度では、ある頂点の近くで最大誤差が35 mmを超えました。integrator="implicitfast"という属性1つで減衰が陰的に扱われ、この問題は解消します。

関節空間での補間。 プログラムは4 mm間隔の161個の経由点でのみ逆運動学を解き、その間の関節角を線形補間します。近接した2つの経由点の間では、関節空間の直線はデカルト空間で直線の辺にきわめて近い曲線に写ります。経由点の間隔を大きくすると、たとえば4つの頂点だけにすると、ペンは辺ではなく弧を描きます。これは「試してみること」で確認します。

コード

プロジェクト全体は1つのファイルです。draw_square.pyとして保存してください。

# draw_square.py: a two-link planar arm in MuJoCo draws a square from analytic IK waypoints.
import math

import mujoco
import numpy as np

L1, L2 = 0.30, 0.25          # link lengths (m)
DT = 0.002                   # physics time step (s)

ARM_XML = f"""
<mujoco model="two_link_arm">
  <option timestep="{DT}" integrator="implicitfast"/>
  <worldbody>
    <geom name="table" type="plane" size="1 1 0.01" pos="0 0 -0.05"/>
    <body name="link1" pos="0 0 0">
      <joint name="shoulder" type="hinge" axis="0 0 1" range="-170 170" damping="0.5"/>
      <geom type="capsule" fromto="0 0 0 {L1} 0 0" size="0.02" mass="1.0"/>
      <body name="link2" pos="{L1} 0 0">
        <joint name="elbow" type="hinge" axis="0 0 1" range="-170 170" damping="0.5"/>
        <geom type="capsule" fromto="0 0 0 {L2} 0 0" size="0.015" mass="0.6"/>
        <site name="pen" pos="{L2} 0 0" size="0.01"/>
      </body>
    </body>
  </worldbody>
  <actuator>
    <position name="shoulder_servo" joint="shoulder" kp="400" kv="20"/>
    <position name="elbow_servo" joint="elbow" kp="200" kv="10"/>
  </actuator>
</mujoco>
"""


def ik_2link(x: float, y: float, elbow_up: bool = False) -> tuple[float, float]:
    """Analytic IK for a planar two-link arm. Returns (q1, q2) in radians."""
    c2 = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    if abs(c2) > 1.0:
        raise ValueError(f"target ({x:.3f}, {y:.3f}) is outside the workspace")
    s2 = math.sqrt(1.0 - c2 * c2)
    if elbow_up:
        s2 = -s2
    q2 = math.atan2(s2, c2)
    q1 = math.atan2(y, x) - math.atan2(L2 * s2, L1 + L2 * c2)
    return q1, q2


def fk_2link(q1: float, q2: float) -> tuple[float, float]:
    return (L1 * math.cos(q1) + L2 * math.cos(q1 + q2),
            L1 * math.sin(q1) + L2 * math.sin(q1 + q2))


def square_path(cx: float, cy: float, side: float, per_side: int) -> np.ndarray:
    """Points around a square, anticlockwise from the lower-left corner, closed."""
    h = side / 2
    corners = np.array([[cx - h, cy - h], [cx + h, cy - h], [cx + h, cy + h], [cx - h, cy + h]])
    pts = []
    for i in range(4):
        a, b = corners[i], corners[(i + 1) % 4]
        for s in np.linspace(0.0, 1.0, per_side, endpoint=False):
            pts.append(a + s * (b - a))
    pts.append(corners[0])
    return np.array(pts)


def draw(model: mujoco.MjModel, targets: np.ndarray, joint_targets: np.ndarray,
         seg_time: float) -> tuple[np.ndarray, np.ndarray]:
    """Track the waypoints with the position servos. Returns (pen xy per waypoint, error per step)."""
    data = mujoco.MjData(model)
    pen_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "pen")
    data.qpos[:] = joint_targets[0]          # start at rest on the first waypoint
    data.ctrl[:] = joint_targets[0]
    mujoco.mj_forward(model, data)
    steps_per_seg = round(seg_time / DT)
    drawn, errors = [], []
    for k in range(1, len(joint_targets)):
        for s in range(steps_per_seg):
            a = (s + 1) / steps_per_seg
            data.ctrl[:] = (1 - a) * joint_targets[k - 1] + a * joint_targets[k]
            mujoco.mj_step(model, data)
            ref_xy = (1 - a) * targets[k - 1] + a * targets[k]
            errors.append(np.linalg.norm(data.site_xpos[pen_id][:2] - ref_xy))
        drawn.append(data.site_xpos[pen_id][:2].copy())
    return np.array(drawn), np.array(errors) * 1000.0


model = mujoco.MjModel.from_xml_string(ARM_XML)
targets = square_path(cx=0.32, cy=0.10, side=0.16, per_side=40)
joint_targets = np.array([ik_2link(x, y) for x, y in targets])

# Check IK against forward kinematics before simulating anything.
fk_err = max(math.dist(fk_2link(*q), p) for q, p in zip(joint_targets, targets))
print(f"waypoints: {len(targets)}, worst IK/FK mismatch: {fk_err:.1e} m")
print(f"joint range used: shoulder {np.degrees(joint_targets[:, 0]).min():.1f} to {np.degrees(joint_targets[:, 0]).max():.1f} deg, "
      f"elbow {np.degrees(joint_targets[:, 1]).min():.1f} to {np.degrees(joint_targets[:, 1]).max():.1f} deg")

drawn, err_mm = draw(model, targets, joint_targets, seg_time=0.05)
print(f"seg_time 0.05 s: {len(err_mm)} steps, {len(err_mm) * DT:.2f} s simulated")
print(f"tracking error: mean {err_mm.mean():.2f} mm, rms {np.sqrt((err_mm ** 2).mean()):.2f} mm, max {err_mm.max():.2f} mm")
print("every 4th waypoint (m): target_x target_y drawn_x drawn_y")
for k in range(0, len(drawn), 4):
    print(f"{targets[k + 1][0]:.4f} {targets[k + 1][1]:.4f} {drawn[k][0]:.4f} {drawn[k][1]:.4f}")

print("speed sweep: seg_time (s), pen speed (mm/s), rms error (mm), max error (mm)")
perimeter_mm = 4 * 160.0
for seg_time in (0.2, 0.1, 0.05, 0.025):
    _, e = draw(model, targets, joint_targets, seg_time)
    speed = perimeter_mm / (seg_time * (len(targets) - 1))
    print(f"{seg_time:.3f} {speed:.0f} {np.sqrt((e ** 2).mean()):.2f} {e.max():.2f}")

draw()は毎回新しいMjDataを作るので、速度スイープのどの速度も同じ状態から始まります。data.site_xposには各ステップ後のペンの位置が入ります。そのため誤差の測定は逆運動学のコードから独立しています。誤差は、シミュレーションされた関節角に対するMuJoCo自身の順運動学から得られます。

実行

python -m pip install mujoco numpy
python draw_square.py

プログラムは概要の行、目標座標と描画座標の40行（4つおきの経由点）、速度スイープを出力します。ノートPCでの実行時間は1秒を大きく下回ります。

テスト

以下は筆者らが実行したときの出力です。MuJoCoは同じマシン上では決定的なので、出力される最後の桁まで同じ数値が得られるはずです。別のCPUやMuJoCoのバージョンでは、差は100分の数mm以内に収まるはずです。

waypoints: 161, worst IK/FK mismatch: 1.2e-16 m
joint range used: shoulder -49.0 to -9.1 deg, elbow 74.6 to 129.1 deg
seg_time 0.05 s: 4000 steps, 8.00 s simulated
tracking error: mean 4.08 mm, rms 4.09 mm, max 4.24 mm
every 4th waypoint (m): target_x target_y drawn_x drawn_y
0.2440 0.0200 0.2414 0.0200
0.2600 0.0200 0.2558 0.0200
0.2760 0.0200 0.2718 0.0200
0.2920 0.0200 0.2878 0.0201
0.3080 0.0200 0.3038 0.0201
0.3240 0.0200 0.3198 0.0201
0.3400 0.0200 0.3358 0.0201
0.3560 0.0200 0.3518 0.0201
0.3720 0.0200 0.3678 0.0201
0.3880 0.0200 0.3838 0.0201
0.4000 0.0240 0.3985 0.0214
0.4000 0.0400 0.4000 0.0359
0.4000 0.0560 0.4000 0.0519
0.4000 0.0720 0.4000 0.0679
0.4000 0.0880 0.4000 0.0839
0.4000 0.1040 0.4000 0.0999
0.4000 0.1200 0.4000 0.1159
0.4000 0.1360 0.4000 0.1319
0.4000 0.1520 0.4000 0.1479
0.4000 0.1680 0.4000 0.1639
0.3960 0.1800 0.3985 0.1787
0.3800 0.1800 0.3842 0.1800
0.3640 0.1800 0.3682 0.1800
0.3480 0.1800 0.3522 0.1800
0.3320 0.1800 0.3362 0.1800
0.3160 0.1800 0.3202 0.1800
0.3000 0.1800 0.3042 0.1800
0.2840 0.1800 0.2882 0.1800
0.2680 0.1800 0.2722 0.1800
0.2520 0.1800 0.2562 0.1801
0.2400 0.1760 0.2415 0.1787
0.2400 0.1600 0.2401 0.1641
0.2400 0.1440 0.2401 0.1481
0.2400 0.1280 0.2401 0.1321
0.2400 0.1120 0.2401 0.1161
0.2400 0.0960 0.2401 0.1001
0.2400 0.0800 0.2401 0.0841
0.2400 0.0640 0.2401 0.0681
0.2400 0.0480 0.2401 0.0521
0.2400 0.0320 0.2401 0.0361
speed sweep: seg_time (s), pen speed (mm/s), rms error (mm), max error (mm)
0.200 20 1.03 1.06
0.100 40 2.06 2.12
0.050 80 4.09 4.24
0.025 160 8.38 8.82

上から順に読んでいきます。逆運動学と順運動学は1e-16 mの精度で一致しているので、関節の目標値は正確です。関節角の範囲は、肘の特異な角度から離れています。追従誤差は平均4.08 mm、最大4.24 mmで、「設計」の節で (kv + 0.5) / kp から予測した4.1 mmと一致します。最大値は平均値よりわずかに大きいだけなので、誤差は頂点でのオーバーシュートではなく、一定の遅れです。

描画座標の行には、遅れがそのまま表れています。下の辺では、ペンはxが約4 mm遅れています。右の辺では、yが約4 mm遅れています。各頂点ではペンがわずかに内側を通ります。遅れているペンは、頂点に達する前に次の辺へ進み始めるためです。

目標の正方形と、シミュレーション上のペンが描いた軌跡。上に出力した行から作成した（描画点は4つおきの経由点と始点）。ペンは目標から約4 mm遅れて追従し、頂点をわずかに丸める。

速度スイープは、この説明の検証です。二乗平均平方根（RMS）誤差は、速度が2倍になるたびに2倍になります。20、40、80、160 mm/sで、それぞれ1.03、2.06、4.09、8.38 mmです。一定の時間遅れなら、まさにこの結果になります。誤差を速度で割ると、どの行でも約51 msになり、サーボゲインから予測した遅れと一致します。

ペンの速度に対するRMS追従誤差。上に出力した速度スイープから作成した。サーボの51 msの一定遅れから予測されるとおり、誤差は速度に比例して増える。

数値がこれと大きく異なる場合は、3つの点を確認してください。最大誤差が平均よりはるかに大きい場合、たいていは積分器がまだオイラー法のままです。逆運動学と順運動学の不一致が1e-9 mより大きい場合、式に符号の誤りがあります。ValueErrorが出る場合は、経由点が届かない位置にあるので、正方形を肩の方へ寄せてください。

実機での実装

このアームを机に載る大きさで作るのは、最初の製作としてよくある題材です。ホビー用またはスマートサーボ2個、アルミから切り出すか樹脂で3Dプリントした2本のリンク、先端のホルダに取り付けたペンで構成します。逆運動学と経由点のコードはそのまま使えます。ほかのいくつかの点は変わります。

キャリブレーション。 実際のリンク長と関節のゼロ点オフセットは、数mm・数度ずれています。実機のL1とL2を関節軸の間で測ってください。各サーボのゼロ点は、0を指令してリンクの角度を測って求めます。肩で1度の誤差があると、リーチ0.4 mの位置でペンが約7 mm動きます。
サーボはブラックボックス。 ホビー用サーボは固定ゲインで独自の位置ループを回しており、kpとkvは設定できません。数十msの遅れと、ある程度の不感帯やバックラッシを想定してください。遅れは速度スイープで測定し、速度を落とすか、各関節の目標値を測定した遅れの分だけ早く送ります。
ペンの高さ。 平面アームにはz軸がありません。ストロークの間にペンを持ち上げるには、3つ目のアクチュエータ（小型のサーボやソレノイド）が必要です。また、ペンが紙に接し続けるように、ペンにはある程度の柔軟性が必要です。
関節の可動範囲とケーブル。 ほとんどのホビー用サーボの回転範囲は約180度です。キャリブレーション後に、出力された関節角の範囲がその中に収まることを確認してください。

以下のコードは、関節の目標値をシリアルポート経由でマイコンに送ります。実機のロボットとpyserialパッケージが必要です。テキストプロトコル（度単位のJ <q1> <q2>を、更新ごとに1行）は一例です。対応するファームウェアは自分で書き、角度をサーボのパルス幅に変換させます。

# needs-hardware
import math
import time

import serial  # pyserial, not part of the project environment

PORT = "/dev/tty.usbmodem1101"   # your board's serial port
UPDATE_HZ = 50


def send_path(joint_targets: list[tuple[float, float]], seg_time: float = 0.05) -> None:
    """Stream joint targets in degrees to firmware that drives two servos."""
    with serial.Serial(PORT, 115200, timeout=1) as link:
        time.sleep(2.0)                     # many boards reset when the port opens
        steps = max(1, round(seg_time * UPDATE_HZ))
        for (a1, a2), (b1, b2) in zip(joint_targets[:-1], joint_targets[1:]):
            for s in range(1, steps + 1):
                t = s / steps
                q1, q2 = a1 + t * (b1 - a1), a2 + t * (b2 - a2)
                link.write(f"J {math.degrees(q1):.2f} {math.degrees(q2):.2f}\n".encode())
                time.sleep(1.0 / UPDATE_HZ)

最初はアームを紙から浮かせ、seg_timeを0.2 sと遅くして始めてください。初めて動かすときは、電源スイッチの近くに手を置いておいてください。

試してみること

頂点だけ。 per_side=40をper_side=1に、セグメント時間を2 sに変更して、アームが4つの頂点の間を関節空間で補間するようにします。描いた軌跡を出力し、辺が弓なりに曲がる様子を見てください。筆者らの実行では、同じペンの速度で最大誤差が4.2 mmから13.9 mmに増えました。産業用アームの直線移動がデカルト空間で補間し、制御ステップごとに逆運動学を解くのはこのためです。
遅れをなくす。 関節の参照値を、予測した遅れの分だけ前にずらします。つまり、現在より51 ms先の経由点に向かって補間します。筆者らがこの変更を加えたバージョンでは、平均誤差が0.13 mmに下がり、頂点には2.1 mmの誤差が残りました。これは速度フィードフォワードの簡易版です。
より硬いサーボ。 両方のkpを2倍にし、kvはそのままにします。実行する前に、新しい遅れを予測してください。
もう一方の肘。 ik_2link(x, y, elbow_up=True)を呼び出し、関節角の範囲を比べてください。どちらの解でも同じ正方形が描けますが、アームの姿勢は異なります。
円。 circle_path(cx, cy, radius, n)を書き、半径0.07 mの円を描いてください。今度は遅れが、丸まった頂点ではなく、目標からわずかに回転して遅れた円として現れます。
積分器を壊す。 integrator="implicitfast"を削除して、スイープを再実行してください。20 mm/sでの最大誤差を見てください。

まとめ

平面2リンクアームの解析的な逆運動学は、atan2の2回の呼び出しと余弦定理による確認だけで書けます。経路全体で肘の解を1つに固定し、結果を順運動学で確認してください。
密な経由点と関節空間の補間で、ほぼ直線の辺が得られます。経由点がまばらだと直線になりません。
動く目標に追従するPD位置サーボは、(kv + 減衰) / kp 秒だけ遅れます。ここではそれが51 msなので、80 mm/sでペンは4.1 mm遅れ、誤差は速度に比例します。
アクチュエータがkvを使う場合は、MuJoCo XMLリファレンスの推奨どおり、MuJoCoのimplicitfast積分器を使ってください。
実機でもコードは同じですが、リンク長とゼロ点オフセットのキャリブレーションと、サーボの遅れの測定が必要です。

モデル形式の詳細はURDF・MJCFチートシートを参照してください。positionアクチュエータと積分器についてはMuJoCo XMLリファレンス、関連する話題は運動学トピックハブを参照してください。

実行環境	ノートPCのCPU（macOS、Linux、Windows）、MuJoCo 3、1分未満
所要時間	約2時間
使用ライブラリ	python, numpy, mujoco