PyBullet 入門：URDF で2軸アームを自分で書く ── リンクの長さは joint の origin に書く

前回の記事では、PyBullet に床と小箱を置いて落としました。そのとき plane.urdf と cube_small.urdf を、名前を書くだけで読み込んでいます。今回は約束どおり、その URDF（Unified Robot Description Format：ロボットの形と関節を XML で書くファイル形式）を自分で書き、2軸アームを1体作ります。

URDF を初めて手で書くと、だいたい次のどれかに当たります。

症状	原因
腕の長さが2倍になる、またはゼロになる	リンクの長さを書く場所を間違えている
読み込んだアームがその場で倒れる	土台を床に固定していない
重さや回り方が書いた値と違う	PyBullet が <inertial> の値を補ったり計算し直したりしている

この記事では、

• URDF の主役である <link>（リンク）と <joint>（関節）の書き方
• リンクの長さが <link> ではなく、次の <joint> の origin に現れること
• 書いた URDF を読み込み、自分で計算した手先の位置と PyBullet の答えを突き合わせること

を扱います。突き合わせの結果、差は $1.55 \times 10^{-8}$ m でした。必要なのは pip install pybullet numpy だけで、この記事の2ファイルだけで動きます。

動作環境

Python 3.10.9
numpy      1.26.4
pybullet   3.2.7

1. ロボットはリンクと関節の「木」

URDF に出てくる部品は2種類です。

• リンク（link）：曲がらない硬い部品。腕の1本1本や土台
• 関節（joint）：リンクどうしのつなぎ目。ここが回ったり伸び縮みしたりする

つながりは木構造（tree structure：1つの根から枝分かれし、枝どうしが合流しない形）でなければなりません。根元から順にたどれば、各部品の位置が1通りに決まるからです。今回の2軸アームは一直線の木です。

<link> が4枚、<joint> が3本で、動く関節は2本です。いちばん下の tool_link は手先の位置を取り出すための目印です。

2. <link> には性格の違う3つを書く

リンクのタグの中には、次の3つが入ります。

タグ	決めること	書き忘れると
<visual>	見た目の形と色	当たり判定の形がそのまま表示される（物理は正常）
<collision>	ぶつかり判定の形	何にもぶつからず、すり抜ける
<inertial>	質量・重心・慣性モーメント	重力やトルクの計算が狂う

慣性モーメント（moment of inertia：物体の回しにくさ。重いもの・長いものほど回りにくい）は、形と質量から公式で出せます。今回の腕は円柱なので、

$$
I_{\text{軸まわり}} = \frac{1}{2} m r^2, \qquad
I_{\text{軸に垂直}} = \frac{1}{12} m (3r^2 + L^2)
$$

（$m$＝質量、$r$＝半径、$L$＝長さ）を使います。腕（$m = 1.0$、$r = 0.04$、$L = 1.0$）なら、軸まわりが $0.0008$、軸に垂直な向きが $0.083733$ です。<visual> と <collision> は別々に書けますが、今回は両方に同じ円柱を使います。

<inertial> を書き忘れても、PyBullet はエラーで止まりません。No inertial data for link という警告を出し、質量 1 kg などの仮の値を補って読み込みます。意図しない重さのまま動くので、<inertial> は毎回書いてください。

3. リンクの長さは、次の関節の origin に現れる

関節のタグには、親と子のリンク、取り付け位置、回転軸、可動範囲を書きます。

<joint name="joint2" type="revolute">
  <parent link="link1"/>                ← 親（根元側）のリンク
  <child  link="link2"/>                ← 子（先端側）のリンク
  <origin xyz="1.0 0 0" rpy="0 0 0"/>   ← 親から見た、子の取り付け位置と向き
  <axis   xyz="0 0 1"/>                 ← z軸まわりに回る
  <limit  lower="-3.14159" upper="3.14159" effort="100.0" velocity="3.0"/>
</joint>

今回使う type は、腕の revolute（可動範囲のある回転関節）と、手先の目印の fixed（動かない関節）の2つです。rpy（roll-pitch-yaw：x・y・z軸まわりの回転角。単位はラジアン）は取り付ける向きです。いちばん間違えやすいのが origin です。

origin は「親の原点から、どれだけずらした所に子をくっつけるか」を表します。「link1 の長さが 1.0 m」は、「link1 の根元から 1.0 m 先に joint2 が付いている」と言い換えられます。なので 1.0 という数字は、<link name="link1"> の中ではなく、joint2 の origin xyz="1.0 0 0" に書きます。link1 の中の円柱の length="1.0" は見た目と当たり判定の大きさで、手先の位置には効きません。

4. 円柱を腕の形に置く

もう1つの origin が、<link> の中の <visual>・<collision>・<inertial> に書く origin です。こちらは「リンクの原点から見て、形をどこにずらして置くか」を表します。

ずらす必要があるのは、URDF の円柱が「中心が原点で、z軸方向に伸びる」形で置かれるからです。腕としては「根元が原点で、x軸方向に伸びる」形がほしいので、2段階で直します。

1. rpy="0 1.5708 0" で、y軸まわりに $90°$（$1.5708 \approx \pi/2$ ラジアン）倒して x軸方向へ寝かせる
2. xyz="0.5 0 0" で、長さの半分だけずらして根元を原点に合わせる

xyz は倒す前の x・y・z で測ります。<inertial> の origin は重心（center of mass：質量が1点に集まっていると見なせる場所）の位置を表すので、3つのタグに同じ origin を書いて、見た目・当たり判定・重心を腕の真ん中にそろえます。

どこの origin	表すもの
<joint> の中	関節の位置（リンクの長さが現れる場所）
<visual>・<collision> の中	形の置き方
<inertial> の中	重心の位置

腕が二重にずれるバグは、たいていこの表の1行目と2行目の取り違えです。

5. 全コード①：two_dof.urdf

ここまでの部品を並べると、2軸アームの URDF ができます。土台は厚さ 0.05 m・半径 0.1 m の円盤で、joint1 の origin xyz="0 0 0.025" は土台の上面の高さです。

two_dof.urdf

<?xml version="1.0"?>
<!-- 2軸ロボットアーム（リンク長 [1.0, 1.0]）。2本とも z軸まわりに回るので，xy 平面の中だけで動く -->
<robot name="two_dof_arm">

  <!-- ベース（土台）。床に固定して使う -->
  <link name="base_link">
    <visual>
      <geometry><cylinder radius="0.1" length="0.05"/></geometry>
      <material name="gray"><color rgba="0.5 0.5 0.5 1.0"/></material>
    </visual>
    <collision>
      <geometry><cylinder radius="0.1" length="0.05"/></geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.002708" ixy="0.0" ixz="0.0" iyy="0.002708" iyz="0.0" izz="0.005000"/>
    </inertial>
  </link>

  <!-- リンク1（1本目の腕・長さ1.0m）
       rpy で y軸まわりに90度倒して x軸方向へ寝かせ，xyz で半分（0.5m）ずらして根元を原点に合わせる -->
  <link name="link1">
    <visual>
      <origin xyz="0.5 0 0" rpy="0 1.5708 0"/>
      <geometry><cylinder radius="0.04" length="1.0"/></geometry>
      <material name="orange"><color rgba="0.9 0.5 0.1 1.0"/></material>
    </visual>
    <collision>
      <origin xyz="0.5 0 0" rpy="0 1.5708 0"/>
      <geometry><cylinder radius="0.04" length="1.0"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0.5 0 0" rpy="0 1.5708 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.083733" ixy="0.0" ixz="0.0" iyy="0.083733" iyz="0.0" izz="0.000800"/>
    </inertial>
  </link>

  <!-- リンク2（2本目の腕・長さ1.0m）。作り方はリンク1と同じで，色だけ青 -->
  <link name="link2">
    <visual>
      <origin xyz="0.5 0 0" rpy="0 1.5708 0"/>
      <geometry><cylinder radius="0.04" length="1.0"/></geometry>
      <material name="blue"><color rgba="0.2 0.4 0.9 1.0"/></material>
    </visual>
    <collision>
      <origin xyz="0.5 0 0" rpy="0 1.5708 0"/>
      <geometry><cylinder radius="0.04" length="1.0"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0.5 0 0" rpy="0 1.5708 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.083733" ixy="0.0" ixz="0.0" iyy="0.083733" iyz="0.0" izz="0.000800"/>
    </inertial>
  </link>

  <!-- 手先（エンドエフェクタ）。形を持たない「点」で，位置を取り出すためだけに置く -->
  <link name="tool_link">
    <inertial>
      <mass value="0.001"/>
      <inertia ixx="1e-6" ixy="0.0" ixz="0.0" iyy="1e-6" iyz="0.0" izz="1e-6"/>
    </inertial>
  </link>

  <!-- 関節1：ベース → リンク1。origin は「親から見た子の取り付け位置」＝ベースの上面 -->
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child  link="link1"/>
    <origin xyz="0 0 0.025" rpy="0 0 0"/>
    <axis   xyz="0 0 1"/>
    <limit  lower="-3.14159" upper="3.14159" effort="100.0" velocity="3.0"/>
  </joint>

  <!-- 関節2：リンク1 → リンク2。origin の x=1.0 が「リンク1の長さ」そのもの -->
  <joint name="joint2" type="revolute">
    <parent link="link1"/>
    <child  link="link2"/>
    <origin xyz="1.0 0 0" rpy="0 0 0"/>
    <axis   xyz="0 0 1"/>
    <limit  lower="-3.14159" upper="3.14159" effort="100.0" velocity="3.0"/>
  </joint>

  <!-- 手先の取り付け：リンク2の先端に固定する（type="fixed" は動かない関節） -->
  <joint name="tool_joint" type="fixed">
    <parent link="link2"/>
    <child  link="tool_link"/>
    <origin xyz="1.0 0 0" rpy="0 0 0"/>
  </joint>

</robot>

tool_link は <visual> も <collision> も持たない、見えずぶつからない目印です。質量は 0.001 kg にして、アームの動きに影響しないようにしました。

6. 全コード②：読み込んで答え合わせする

この URDF を読み込み、手先の位置を自分の計算と突き合わせます。答え合わせの相手は、2軸アームの順運動学（forward kinematics：関節の角度から手先の位置を求める計算）です。

$$
x = l_1 \cos\theta_1 + l_2 \cos(\theta_1 + \theta_2), \qquad
y = l_1 \sin\theta_1 + l_2 \sin(\theta_1 + \theta_2)
$$

肘の角度 $\theta_2$ は1本目の腕から測った相対角度なので、2本目の腕の床から見た向きは $\theta_1 + \theta_2$ になります。

two_dof.urdf と同じフォルダに check_two_dof.py として保存し、python check_two_dof.py で動きます。

check_two_dof.py

# 手書きの2軸アームURDFを PyBullet に読み込み，自作の順運動学と手先位置を突き合わせる

# ライブラリの読み込み
import numpy as np          # 数値計算
import pybullet as p        # 物理シミュレータ本体
import pybullet_data        # PyBullet に同梱されたURDF（床）の置き場


# 2軸アームのモデル。URDF の joint2・tool_joint の origin と同じ値（答え合わせ用）
URDF_FILE    = "two_dof.urdf"   # 読み込むURDFファイル
LINK_LENGTHS = (1.0, 1.0)       # リンク長 [l1, l2]（m）
DIMENSION_2D = 2                # 平面なので2次元

CHECK_ANGLES_DEG = (30.0, 60.0) # 答え合わせに使う関節角度（度）


def load_arm(client_id: int) -> tuple:
    """
    URDF を読み込み，動かせる関節と手先のインデックスを調べる

    戻り値
        (物体ID, 動かせる関節のインデックス, 関節名, 可動範囲, 手先のリンクインデックス)
    """
    # useFixedBase=True で土台を床に固定する（固定しないと重力で倒れる）
    # URDF_USE_INERTIA_FROM_FILE で <inertia> に書いた値をそのまま使わせる
    body_id = p.loadURDF(URDF_FILE, (0.0, 0.0, 0.0), useFixedBase=True,
                         flags=p.URDF_USE_INERTIA_FROM_FILE, physicsClientId=client_id)

    # getJointInfo() の戻り値：1=名前, 2=種類, 8=可動範囲の下限, 9=上限, 12=子リンク名
    indices, names, limits, tool_index = [], [], [], -1
    for index in range(p.getNumJoints(body_id, physicsClientId=client_id)):
        info = p.getJointInfo(body_id, index, physicsClientId=client_id)
        if info[2] != p.JOINT_FIXED:            # 固定関節はモーターが無いので除く
            indices.append(index)
            names.append(info[1].decode())      # 名前はバイト列で返るので文字列にする
            limits.append((info[8], info[9]))
        if info[12].decode() == "tool_link":    # 子が tool_link の関節＝手先の取り付け
            tool_index = index
    return body_id, indices, names, limits, tool_index


def reset_joint_angles(client_id: int, body_id: int, indices: list,
                       angles: np.ndarray) -> None:
    """
    関節角度を（物理を無視して）即座に書き換える。実機にはできない「瞬間移動」
    """
    if len(angles) != len(indices):
        raise ValueError(f"関節数が合いません（渡された数={len(angles)}, 必要な数={len(indices)}）")
    for index, angle in zip(indices, angles):
        p.resetJointState(body_id, index, angle, physicsClientId=client_id)


def tool_position(client_id: int, body_id: int, tool_index: int) -> np.ndarray:
    """
    手先の位置 [x, y]（ワールド座標）を取得する
    """
    # getLinkState() の 4番目＝リンク座標系の原点（0番目は重心なので取り違えない）
    state = p.getLinkState(body_id, tool_index, physicsClientId=client_id)
    return np.array(state[4])[:DIMENSION_2D]    # 2軸は平面で動くので xy だけを見る


def forward_kinematics(thetas: np.ndarray) -> np.ndarray:
    """
    2軸アームの順運動学（関節角度 [theta1, theta2] → 手先の位置 [x, y]）
    """
    l1, l2 = LINK_LENGTHS
    theta1, theta2 = thetas
    # 2本目は1本目の上に乗っているので，床から見た向きは theta1 + theta2
    x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
    y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
    return np.array([x, y])


def main() -> None:
    """
    メイン処理
    """
    # 画面なし（DIRECT）で世界を作り，重力と床を用意する（前回と同じ手順）
    client_id = p.connect(p.DIRECT)
    p.setAdditionalSearchPath(pybullet_data.getDataPath(), physicsClientId=client_id)
    p.setGravity(0.0, 0.0, -9.81, physicsClientId=client_id)
    p.loadURDF("plane.urdf", physicsClientId=client_id)

    # ① URDF が意図どおりに読めたかを確かめる
    body_id, indices, names, limits, tool_index = load_arm(client_id)
    print(f"動かせる関節の数 = {len(indices)}")
    print(f"関節の名前 = {names}")
    for name, (lower, upper) in zip(names, limits):
        print(f"  {name}: 可動範囲 = {np.rad2deg(lower):.1f}度 〜 {np.rad2deg(upper):.1f}度")

    # ② 1点で突き合わせる：PyBullet の手先位置 vs 自作の式
    thetas = np.deg2rad(np.array(CHECK_ANGLES_DEG))
    reset_joint_angles(client_id, body_id, indices, thetas)
    tool_pybullet = tool_position(client_id, body_id, tool_index)
    tool_ours = forward_kinematics(thetas)
    difference = np.abs(tool_pybullet - tool_ours)
    print(f"関節角度 = ({CHECK_ANGLES_DEG[0]}度, {CHECK_ANGLES_DEG[1]}度)")
    print(f"  PyBullet の手先位置 = [{tool_pybullet[0]:.4f}, {tool_pybullet[1]:.4f}]")
    print(f"  自作の順運動学     = [{tool_ours[0]:.4f}, {tool_ours[1]:.4f}]")
    print(f"  差                 = [{difference[0]:.3e}, {difference[1]:.3e}]")

    # ③ でたらめな角度を500通り試して，差の最大値を見る（seed=42 で毎回同じ）
    rng = np.random.default_rng(42)
    max_difference = 0.0
    for _ in range(500):
        thetas = rng.uniform(-np.pi, np.pi, DIMENSION_2D)
        reset_joint_angles(client_id, body_id, indices, thetas)
        difference = np.abs(tool_position(client_id, body_id, tool_index)
                            - forward_kinematics(thetas))
        max_difference = max(max_difference, float(np.max(difference)))
    print(f"500通りの関節角度で試した差の最大値 = {max_difference:.3e} m")

    p.disconnect(physicsClientId=client_id)


if __name__ == "__main__":
    main()

押さえておきたいのは次の3つです。

useFixedBase=True：URDF のロボットは、既定では床に置いただけの物体なので倒れます。土台を床に固定すると伝えるのがこの引数です。

flags=p.URDF_USE_INERTIA_FROM_FILE：PyBullet は既定では <inertia> の値を使わず、<collision> の形から慣性モーメントを計算し直します。flags なしで link1 を getDynamicsInfo() で調べると、軸まわりの値は書いた $0.0008$ ではなく $0.00123$ でした。

getLinkState() の4番目：0番目は重心の位置、4番目はリンクの座標系の原点（URDF に書いた origin の場所）です。$\theta_1 = 30°$、$\theta_2 = 60°$ のときに測ると、

リンク	0番目（重心）	4番目（座標系の原点）
link2	$[0.866, 1.000, 0.025]$	$[0.866, 0.500, 0.025]$
tool_link	$[0.866, 1.500, 0.025]$	$[0.866, 1.500, 0.025]$

link2 では重心が腕の真ん中にあるので 0.5 m ずれます。手先だけなら一致しますが、途中のリンクで混ざると原因を探しにくいので、4番目にそろえています。

なお resetJointState() は、物理を飛び越えて関節角度を書き換える「瞬間移動」です。今回は形を確かめるのが目的なので、これで十分です。

7. 実行結果

動かせる関節の数 = 2
関節の名前 = ['joint1', 'joint2']
  joint1: 可動範囲 = -180.0度 〜 180.0度
  joint2: 可動範囲 = -180.0度 〜 180.0度
関節角度 = (30.0度, 60.0度)
  PyBullet の手先位置 = [0.8660, 1.5000]
  自作の順運動学     = [0.8660, 1.5000]
  差                 = [1.554e-08, 0.000e+00]
500通りの関節角度で試した差の最大値 = 5.959e-08 m

最初に出る pybullet build time: … の1行は省いています。

固定関節の tool_joint は除かれ、動かせる関節は2本です。手先の位置は、PyBullet も自分の式も $[0.8660, 1.5000]$ で、差は $1.55 \times 10^{-8}$ m です。$10^{-8}$ m は 10 ナノメートルで、浮動小数点数の丸め誤差（rounding error：小数を有限の桁で持つために出る端数）の大きさです。手計算でも、$\theta_1 + \theta_2 = 90°$ なので

$$
x = \cos 30° + \cos 90° = 0.8660, \qquad y = \sin 30° + \sin 90° = 1.5000
$$

と同じ値になります。でたらめな角度を500通り試しても、差の最大値は $5.96 \times 10^{-8}$ m でした。自分で書いた URDF と式が、同じロボットを表しています。$10^{-8}$〜$10^{-7}$ m 台なら丸め誤差、という目安があれば、$10^{-2}$ m のずれを見たときに「URDF のほうが違う」とすぐ判断できます。

この gif は、前回の記事の capture_image() のカメラを真上寄り（注視点 (0.4, 0.4, 0)・距離 2.4 m・yaw 90度・pitch −70度）に変え、関節1を 0→180度、関節2を 0→−120度 まで60コマ動かして撮りました（コードは省略）。2軸とも z軸まわりに回るので、腕は床と平行な面の中だけで動きます。

8. 手元で試すなら

• リンクの長さを変える：joint2 の origin を xyz="1.5 0 0" にすると、手先の位置が変わって差が大きくなります。LINK_LENGTHS も (1.5, 1.0) に直すと、また一致します。URDF とコードの2か所を直さないと合わないのは、両方が同じモデルを表しているからです
• 見た目だけ変える：link1 の円柱の length だけを 0.5 にすると、見た目の腕は短くなるのに、手先の位置は変わりません
• 可動範囲を狭める：joint2 の <limit> を lower="0" upper="1.57" にし、CHECK_ANGLES_DEG を (30.0, -60.0) にします。可動範囲の表示は 0.0度〜90.0度に変わるのに、アームは $-60°$ の姿勢になります。resetJointState() は可動範囲も飛び越えるからです
• 固定関節を revolute にする：tool_joint の type だけを revolute に変えると、Joint is of type REVOLUTE but it does not specify limits と出て読み込みに失敗します。revolute には <limit> が必須です

まとめ

• URDF はリンクと関節の木。2軸アームは <link> 4枚・<joint> 3本で書ける
• <link> には見た目・当たり判定・慣性の3つを書く。<inertial> を省くと PyBullet が仮の値で補う
• リンクの長さは <link> ではなく、次の <joint> の origin に書く
• 円柱は rpy で倒し、xyz で半分ずらして腕の形にする
• 読み込みは useFixedBase=True と URDF_USE_INERTIA_FROM_FILE、手先の位置は getLinkState() の4番目
• 自作の順運動学との差は $1.55 \times 10^{-8}$ m、500通りでも最大 $5.96 \times 10^{-8}$ m

今回のアームは、2本の軸がどちらも z軸まわりなので、平面の中でしか動けません。次回は、軸の向きがばらばらな関節を6本持つ6軸アームを URDF にします。リンク長 $[1.0, 1.0, 1.0, 0.1, 0.1, 0.1]$、回転軸 $[z, y, y, z, y, z]$ の並びを、今回と同じ書き方でタグへ移していきます。

ロボットの自作記事まとめ（2軸・3軸・6軸アームから PyBullet まで）：

役に立ったら いいね・ストック で応援いただけると、次回の励みになります。