4足ロボットの脚1本は、股から足先まで3つの関節角と、股の位置＋3つの長さで書けます。関節角から足先の位置を出す計算を順運動学（forward kinematics：入力側＝関節の角度から、結果側＝足先や手先の位置を求める向きの計算。以下 FK）と呼びます。この記事では Unitree A1（PyBullet に同梱されている4足ロボットのモデル）の前右脚で FK を式に書き、PyBullet（物理シミュレータ）が返す足先位置と突き合わせます。結果は、可動域内のランダム 2000 姿勢で最大誤差 2.97×10⁻⁸ m（0.000 mm） です。式は URDF（Unified Robot Description Format：ロボットの寸法と関節を書いた XML ファイル）に書いてある3つの長さで足ります。
ただし、途中で1か所大きく引っかかりました。PyBullet の getJointInfo が返すオフセット（parentFramePos）を足し合わせて同じ式を組むと、足先が 7〜13 cm ずれます。 式の順序でも、座標系の取り違えでもなく、その値が URDF の <origin> とは別のもの（親リンクの重心基準）だからです。これに気づくまでの切り分けと、コードで直す方法を④に書きます。
必要なのは pip install pybullet numpy の2つだけです（図3・図4・図6 を自分で描き直すなら matplotlib も）。Docker も ROS も実機も要りません。この記事のコードだけで動きます（前の回を読んでいなくて構いません）。
| 引っかかる所 | 何が起きるか | この記事で出る数字 |
|---|---|---|
| ① 脚の関節は「股・膝・足首」ではない | A1 の脚は股ロール・股ピッチ・膝の3つで、足首の関節は無い | 足先リンク FR_toe は FIXED（回らない） |
② getJointInfo のオフセットで式を組む |
足先位置が PyBullet と合わない | ランダム 2000 姿勢で 75.5〜134.0 mm ずれる（平均 103.4 mm） |
| ③ 膝は伸び切らない | 脚の長さは 0.4 m にならない | 股ピッチ軸から足先まで 0.088〜0.359 m |
④ getLinkState(...)[0] で比べる |
[0] は重心位置なので、大腿・下腿では合わない |
リンクフレーム位置 [4] を使う（下腿では 107.6 mm 違う） |
この記事は連載「ヒューマノイドへの道程」の②4足ロボットの1本目です。①移動ロボットの3本（差動二輪の順運動学・逆運動学・経路追従）は下から辿れます。
図1：連載の5段。①で車体の位置と向きを扱い、②から関節を持つ機体に入ります。今回は脚1本の順運動学だけを扱います
動作環境
Python 3.10.9
numpy 1.26.4
pybullet 3.2.7
matplotlib 3.7.0（図の描画にだけ使用。描画コードは記事では省略）
A1 の URDF は pybullet_data に同梱されています。追加のダウンロードは要りません。
import pybullet_data
print(pybullet_data.getDataPath()) # この下の a1/a1.urdf を使う
① A1 の脚は何自由度か：URDF を読む
まず、脚に関節がいくつあるかを PyBullet に聞きます。getJointInfo は関節ごとに名前・種類・回転軸・可動域を返します。前右脚（FR：Front Right）だけを抜き出すとこうなります。
import pybullet as p
import pybullet_data
p.connect(p.DIRECT) # 画面を出さないモード
p.setAdditionalSearchPath(pybullet_data.getDataPath())
robot = p.loadURDF("a1/a1.urdf", [0.0, 0.0, 0.0], useFixedBase=True)
for joint in range(p.getNumJoints(robot)):
info = p.getJointInfo(robot, joint)
if info[1].decode().startswith("FR_"):
print(joint, info[1].decode(), info[2], info[13], info[8], info[9]) # [1]名前 [2]種類 [13]回転軸 [8][9]可動域
| index | name | type | axis | lower [rad] | upper [rad] |
|---|---|---|---|---|---|
| 1 | FR_hip_joint |
REVOLUTE | (1, 0, 0) | −0.803 | 0.803 |
| 2 | FR_hip_fixed |
FIXED | — | — | — |
| 3 | FR_upper_joint |
REVOLUTE | (0, 1, 0) | −1.047 | 4.189 |
| 4 | FR_lower_joint |
REVOLUTE | (0, 1, 0) | −2.697 | −0.916 |
| 5 | FR_toe_fixed |
FIXED | — | — | — |
index 2 の FR_hip_fixed は FR_hip から横に分かれた固定リンク（モータの外殻）で、FR_upper_joint の親は FR_hip です（p.getJointInfo(robot, 3)[16] が 1 を返します）。足先までの鎖には入らないので、以降は出てきません。
回る関節（REVOLUTE）は3つで、足首はありません。教科書の図では「股・膝・足首」と描かれることが多いのですが、A1 の脚は次の3つです。
| 関節 | URDF の名前 | 回転軸 | 動き |
|---|---|---|---|
| 股ロール $q_1$ | FR_hip_joint |
x 軸（前後方向） | 脚を左右に開く・閉じる |
| 股ピッチ $q_2$ | FR_upper_joint |
y 軸（左右方向） | 脚を前後に振る |
| 膝 $q_3$ | FR_lower_joint |
y 軸（左右方向） | 下腿を折る |
足先（FR_toe）は下腿に固定された球で、向きを変える関節を持ちません。つまり足先の「位置」は3つの角で決まりますが、「向き」は選べません。 足首があるロボットとの一番の違いはここです。
図2：前右脚のリンクと関節の並び。関節番号は上の表の index に対応します（1・3・4 が回る関節、5 が足先）
⚠️ 膝の可動域は −2.697〜−0.916 rad（−155°〜−52°）で、0 を含みません。 膝がまっすぐ（0 rad）になる姿勢は A1 では取れません。後で脚の長さを見るときに効いてきます。
② 式：3つの回転と3つの長さ
座標系と長さの出所
胴体の座標系は x が前、y が左、z が上です（URDF の慣例）。3つの長さは URDF の <joint> に書かれた <origin xyz> から写します。
図3：ゼロ姿勢（全関節 0 rad）の前右脚。左の正面図で3つの長さ $L_{\text{hip}}$・$L_{\text{thigh}}$・$L_{\text{calf}}$ と3つの関節軸、右の上面図で股ロール軸の位置 (0.183, −0.047)。値はすべて a1.urdf の <origin xyz> から写しています
| 記号 | 値 [m] | URDF の joint | <origin xyz> |
|---|---|---|---|
| 股ロール軸の位置 | (0.183, −0.047, 0) | FR_hip_joint |
0.183 -0.047 0 |
| $L_{\text{hip}}$ | 0.08505 | FR_upper_joint |
0 -0.08505 0 |
| $L_{\text{thigh}}$ | 0.2 | FR_lower_joint |
0 0 -0.2 |
| $L_{\text{calf}}$ | 0.2 | FR_toe_fixed |
0 0 -0.2 |
a1.urdf をテキストエディタで開いて FR_upper_joint を検索すると、<origin rpy="0 0 0" xyz="0 -0.08505 0"/> の行が見つかります。自分の URDF で同じ数字を指させれば、以下の式はそのまま使えます。
回転行列は2種類だけ
$R_x(\theta)$ は「x 軸のまわりに $\theta$ だけ回す操作」を 3×3 の行列にしたものです。点の座標にこの行列を掛けると、回した後の座標が出ます。中身は覚えなくて構いません。この記事では x 軸まわりと y 軸まわりの2つしか使いません。
\displaylines{
R_x(\theta) =
\begin{pmatrix}
1 & 0 & 0 \\
0 & \cos\theta & -\sin\theta \\
0 & \sin\theta & \cos\theta
\end{pmatrix},
\qquad
R_y(\theta) =
\begin{pmatrix}
\cos\theta & 0 & \sin\theta \\
0 & 1 & 0 \\
-\sin\theta & 0 & \cos\theta
\end{pmatrix}
}
回転行列を使って順運動学を組み立てる手順そのものは、平面2軸アームの記事で一から書いています。3次元になっても組み方は同じです。
式は内側から外側へ組む
足先から股へ向かって、各関節で「その先の部分」を回してから、リンクの長さを足すを繰り返します。結論を先に書くと、足先の位置 $\boldsymbol{p}$ はこの1本です。
\boldsymbol{p} = \boldsymbol{p}_{\text{hip}} + R_x(q_1)\left[\begin{pmatrix}0\\-L_{\text{hip}}\\0\end{pmatrix} + R_y(q_2)\left\{\begin{pmatrix}0\\0\\-L_{\text{thigh}}\end{pmatrix} + R_y(q_3)\begin{pmatrix}0\\0\\-L_{\text{calf}}\end{pmatrix}\right\}\right]
一番内側の $R_y(q_3),(0, 0, -L_{\text{calf}})$ が「膝から見た足先」、その外側が「股ピッチ軸から見た足先」、一番外側が「股ロール軸から見た足先」です。右脚なので $L_{\text{hip}}$ の符号は負（−y 側）です。
図4：立ち姿勢 $(q_1, q_2, q_3) = (0, 0.8, -1.5)$ rad の前右脚。左の側面図で股ピッチと膝が回り、右の正面図で股ロールが回ります。股ロール軸と股ピッチ軸は横に 0.08505 m ずれているだけなので、側面図では重なります
⚠️ 符号に注意してください。 股ピッチ $q_2$ を正にすると脚は後ろへ振れます（y 軸まわりの右ねじ＝右手の親指を +y に向けたとき残りの指が曲がる向きが正で、下向きの脚は −x へ回る）。膝 $q_3$ は可動域が負なので、下腿は常に前へ折れます。図4の立ち姿勢で足先が股のわずか後ろ（x = −0.015 m）に来るのはこのためです。
コードにするとこうなります。leg_fk の p_calf・p_thigh・p_hip の3行が、上の式の内側・中・外側にそのまま対応します。
import numpy as np
# URDF の <origin xyz> から写した3つの長さ（単位 m）
HIP_ORIGIN = np.array([0.183, -0.047, 0.0]) # trunk → FR_hip_joint
L_HIP = 0.08505 # FR_hip_joint → FR_upper_joint（横向き。右脚は -y）
L_THIGH = 0.2 # FR_upper_joint → FR_lower_joint（下向き -z）
L_CALF = 0.2 # FR_lower_joint → FR_toe（下向き -z）
def rot_x(angle: float) -> np.ndarray:
"""x 軸まわりに angle [rad] 回す 3x3 の回転行列"""
c, s = np.cos(angle), np.sin(angle)
return np.array([[1.0, 0.0, 0.0],
[0.0, c, -s],
[0.0, s, c]])
def rot_y(angle: float) -> np.ndarray:
"""y 軸まわりに angle [rad] 回す 3x3 の回転行列"""
c, s = np.cos(angle), np.sin(angle)
return np.array([[c, 0.0, s],
[0.0, 1.0, 0.0],
[-s, 0.0, c]])
def leg_fk(q: np.ndarray, side: float = -1.0) -> np.ndarray:
"""関節角 q = (股ロール, 股ピッチ, 膝) [rad] から足先位置 [m] を返す（胴体座標系）
side は右脚なら -1.0、左脚なら +1.0（股の横向き長さの符号だけが違う）。
"""
q_roll, q_pitch, q_knee = q
p_calf = rot_y(q_knee) @ np.array([0.0, 0.0, -L_CALF]) # 膝から足先（@ は行列の掛け算）
p_thigh = rot_y(q_pitch) @ (np.array([0.0, 0.0, -L_THIGH]) + p_calf) # 股ピッチから足先
p_hip = rot_x(q_roll) @ (np.array([0.0, side * L_HIP, 0.0]) + p_thigh) # 股ロールから足先
return HIP_ORIGIN + p_hip
leg_fk(np.array([0.0, 0.8, -1.5])) は [0.1684, -0.132, -0.2923] を返します。立ち姿勢で足先は胴体の 0.292 m 下、右に 0.132 m、股ロール軸の 0.015 m 後ろです。
③ PyBullet で検算する
式が合っているかを、PyBullet に同じ関節角を入れて確かめます。ここで2つ、先に決めておくことがあります。
胴体は原点に固定します。 loadURDF("a1/a1.urdf", [0, 0, 0], useFixedBase=True) とすると、胴体座標系と世界座標系が一致し、式の答えと getLinkState の世界座標をそのまま比べられます。胴体を浮かせたり地面に置いたりすると、胴体の位置と傾きぶんだけ足先の世界座標が動くので、比較の前にそれを差し引く必要があります。
足先の位置は getLinkState(...)[4] を使います。 getLinkState は7つ以上の値を返し、[0] はリンクの重心の位置、[4] はリンクフレーム（URDF の <origin> で定義した座標系の原点）の位置です。式が出すのは後者です。ここで [0] を取ると、リンクの重心がリンクフレームからずれているぶんだけ合わなくなります。A1 の足先は p.getDynamicsInfo(robot, 5)[3] が (0, 0, 0) を返す（重心がリンクフレームの原点にある）ので偶然一致しますが、下腿では 107.6 mm 違います。
import pybullet as p
import pybullet_data
# PyBullet の関節番号（①の表の index）。リンク番号は「そのリンクを子に持つ関節の番号」と
# 同じ決まりなので、足先リンク FR_toe のリンク番号も 5
J_HIP, J_THIGH, J_CALF, L_TOE = 1, 3, 4, 5
def load_a1() -> int:
"""A1 を原点に固定して読み込む（胴体座標系 = 世界座標系 にするため）"""
p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
return p.loadURDF("a1/a1.urdf", [0.0, 0.0, 0.0], useFixedBase=True)
def foot_pos_pybullet(robot: int, q: np.ndarray) -> np.ndarray:
"""PyBullet に関節角を入れて、足先リンクの位置を返す"""
for joint, angle in zip((J_HIP, J_THIGH, J_CALF), q):
p.resetJointState(robot, joint, float(angle))
state = p.getLinkState(robot, L_TOE, computeForwardKinematics=True)
return np.array(state[4]) # [4] = リンクフレームの位置（[0] は重心位置なので使わない）
resetJointState は物理シミュレーションを進めずに関節角を直接書き換えます。computeForwardKinematics=True を付けると、その書き換えを反映したリンク位置が返ります（付けないと古い値が返ることがあります）。
代表的な4姿勢で突き合わせます。
robot = load_a1()
poses = {
"立ち姿勢": np.array([0.0, 0.8, -1.5]),
"膝を最も曲げる": np.array([0.0, 0.8, -2.697]),
"股を内へ": np.array([0.5, 0.8, -1.5]),
"膝を最も伸ばす": np.array([0.0, 0.8, -0.916]),
}
for name, q in poses.items():
a = leg_fk(q)
b = foot_pos_pybullet(robot, q)
print(f"{name}: 式 {np.round(a, 4)} / PyBullet {np.round(b, 4)} / 差 {np.linalg.norm(a - b) * 1e3:.3f} mm")
| 姿勢 $(q_1, q_2, q_3)$ [rad] | 式 [m] | PyBullet [m] | 差 [mm] |
|---|---|---|---|
| 立ち姿勢 (0, 0.8, −1.5) | (0.1684, −0.1320, −0.2923) | (0.1684, −0.1320, −0.2923) | 0.000 |
| 膝を最も曲げる (0, 0.8, −2.697) | (0.2290, −0.1320, −0.0753) | (0.2290, −0.1320, −0.0753) | 0.000 |
| 股を内へ (0.5, 0.8, −1.5) | (0.1684, 0.0185, −0.2973) | (0.1684, 0.0185, −0.2973) | 0.000 |
| 膝を最も伸ばす (0, 0.8, −0.916) | (0.0627, −0.1320, −0.3380) | (0.0627, −0.1320, −0.3380) | 0.000 |
4姿勢では足りないので、可動域の中からランダムに 2000 姿勢を引いて最大誤差を見ます。
limits = np.array([p.getJointInfo(robot, j)[8:10] for j in (J_HIP, J_THIGH, J_CALF)])
rng = np.random.default_rng(0)
samples = rng.uniform(limits[:, 0], limits[:, 1], size=(2000, 3))
errors = [np.linalg.norm(leg_fk(q) - foot_pos_pybullet(robot, q)) for q in samples]
print(f"最大誤差 {max(errors):.2e} m") # 2.97e-08 m
最大誤差は 2.97×10⁻⁸ m でした。PyBullet が内部で単精度（float32：有効桁が約7桁の浮動小数点）を使っているぶんの丸めで、式の誤りではありません。「0.000 mm」と書いているのはこの値です。
「股を内へ」の行で y が −0.132 から +0.0185 に変わっているのは、股ロール 0.5 rad（約 29°）で脚が胴体の下へ振り込まれ、足先が胴体の中心線を越えて左側に出たためです。式でも PyBullet でも同じ数字が出ているので、これは正しい動きです。
④ 落とし穴：getJointInfo の parentFramePos で組むと 7〜13 cm ずれる
どうやって気づいたか
②の3つの長さは URDF を目で読んで写しました。最初はそうせず、「PyBullet が関節の位置を知っているのだから、そこから取ればよい」と考えました。getJointInfo の戻り値 [14]（parentFramePos：関節の位置を親リンクの座標系で表した値）を、②と同じ順序で足し合わせたのです。
def leg_fk_from_joint_info(robot: int, q: np.ndarray, fix_com: bool = False) -> np.ndarray:
"""getJointInfo の parentFramePos を足し合わせて組んだ順運動学
com は center of mass（重心）の略。
fix_com=False: parentFramePos をそのまま使う（親リンクの重心基準なのでずれる）
fix_com=True : 親リンクの重心位置 getDynamicsInfo[3] を足し戻す（URDF の <origin> に戻る）
"""
offset = {}
for joint in (J_HIP, J_THIGH, J_CALF, L_TOE):
info = p.getJointInfo(robot, joint)
pos = np.array(info[14]) # parentFramePos
if fix_com:
parent = info[16] # 親リンク番号（-1 は胴体）
pos = pos + np.array(p.getDynamicsInfo(robot, parent)[3])
offset[joint] = pos
q_roll, q_pitch, q_knee = q
p_calf = rot_y(q_knee) @ offset[L_TOE]
p_thigh = rot_y(q_pitch) @ (offset[J_CALF] + p_calf)
p_hip = rot_x(q_roll) @ (offset[J_THIGH] + p_thigh)
return offset[J_HIP] + p_hip
これを③と同じ 2000 姿勢にかけると、最大 134.0 mm・最小 75.5 mm・平均 103.4 mm ずれます。最初に試した全関節 0 rad の姿勢（膝の可動域の外なので比較用です）では 137 mm でした。1 cm なら丸めや符号を疑いますが、10 cm 以上はどこかの長さが丸ごと違います。
切り分けの決め手は、全関節 0 rad でも 137 mm ずれることでした。0 rad なら回転行列はすべて単位行列（掛けても何も変わらない行列）なので、ずれは「足している長さの和」そのものです。つまり回転の順序ではなく、長さが違います。そこで URDF の <origin> と parentFramePos を並べました。
3列で並べると差が重心オフセットになる
for joint in (J_HIP, J_THIGH, J_CALF, L_TOE):
info = p.getJointInfo(robot, joint)
com = p.getDynamicsInfo(robot, info[16])[3] # 親リンクの重心位置（親リンク座標系）
print(info[1].decode(), np.round(info[14], 5), np.round(com, 5))
| joint | URDF <origin xyz>
|
getJointInfo[14]（parentFramePos） |
親リンクの重心 getDynamicsInfo[3]
|
|---|---|---|---|
FR_hip_joint |
(0.183, −0.047, 0) | (0.17027, −0.04919, −0.00052) | (0.01273, 0.00219, 0.00052) |
FR_upper_joint |
(0, −0.08505, 0) | (0.00331, −0.08442, −0.00003) | (−0.00331, −0.00064, 0.00003) |
FR_lower_joint |
(0, 0, −0.2) | (0.00324, −0.02233, −0.17267) | (−0.00324, 0.02233, −0.02733) |
FR_toe_fixed |
(0, 0, −0.2) | (−0.00644, 0, −0.09261) | (0.00644, 0, −0.10739) |
どの行も、2列目＋3列目＝1列目です。たとえば FR_lower_joint は (0.00324, −0.02233, −0.17267) + (−0.00324, 0.02233, −0.02733) = (0, 0, −0.2) です。そして4行の3列目を足し合わせると (0.0126, 0.0239, −0.1342)、大きさ 136.9 mm。0 rad でのずれ (0.0126, 0.0239, −0.1342) とベクトルごと一致します。
つまり parentFramePos は「親リンクの重心を原点にした座標系」で測った関節の位置でした。URDF の <origin> は「親リンクのリンクフレームを原点にした座標系」です。PyBullet は内部で各リンクの座標系を重心に置き直して計算しているので、getJointInfo が返すのは置き直した後の値です。大腿の重心はリンクフレームから 2.7 cm 下、下腿の重心は 10.7 cm 下にあり、そのぶんが式に入り込んでいました。
図5：FR_lower_joint の場合。URDF は左の1本の矢印、PyBullet は右の2本の矢印の後半だけを返します。前半（重心オフセット）を足し戻せば同じ点に着きます
コードで直す2通り
URDF を目で読んで数字を写すのは、機体を変えるたびに手作業になります。コードで正しい値を取る方法は2つあります。
(a) 親リンクの重心位置を足し戻す。 上の leg_fk_from_joint_info の fix_com=True がこれです。getDynamicsInfo(robot, parent)[3] が親リンクの重心位置（親リンク座標系）を返すので、parentFramePos に足すと URDF の <origin> に戻ります。親が胴体（parent == -1）のときも同じ式で通ります。
errors_fixed = [np.linalg.norm(leg_fk_from_joint_info(robot, q, fix_com=True) - foot_pos_pybullet(robot, q))
for q in samples]
print(f"最大誤差 {max(errors_fixed):.2e} m") # 2.97e-08 m
(b) URDF を XML として読む。 xml.etree.ElementTree で <joint name="FR_lower_joint"> の <origin xyz> を取れば、PyBullet を介さずに正本の値が手に入ります。
import xml.etree.ElementTree as ET
import os
tree = ET.parse(os.path.join(pybullet_data.getDataPath(), "a1", "a1.urdf"))
for joint in tree.getroot().findall("joint"): # iter() だと <transmission> 内の joint も拾って落ちる
if joint.get("name") in ("FR_hip_joint", "FR_upper_joint", "FR_lower_joint", "FR_toe_fixed"):
print(joint.get("name"), joint.find("origin").get("xyz"))
# FR_hip_joint 0.183 -0.047 0 / FR_upper_joint 0 -0.08505 0 / FR_lower_joint 0 0 -0.2 / FR_toe_fixed 0 0 -0.2
どちらでも③と同じ 2.97×10⁻⁸ m になります。私は (a) を使っています。関節番号だけ分かれば、URDF のパスを知らなくても済むからです。
| 組み方 | 2000 姿勢の最大誤差 |
|---|---|
URDF の <origin> を写した式（②） |
2.97×10⁻⁸ m |
parentFramePos をそのまま足す |
134.0 mm |
parentFramePos ＋ 親リンクの重心（(a)） |
2.97×10⁻⁸ m |
⑤ 脚はどこまで届くか
式が合ったので、足先が届く範囲を式だけで描けます。股ロールを 0 に固定し、股ピッチと膝を可動域いっぱいに振ると、股ピッチ軸を中心にした平面（x–z）で足先はこの範囲を動きます。
図6：股ロール 0 のときに足先が届く範囲（青）。膝が伸び切らないので外側の点線（0.400 m）には届かず、赤の破線（0.359 m）が外周になります。内側の橙の破線（0.088 m）の中は、膝を最も曲げても入れない範囲です。黒は立ち姿勢の脚
股ピッチ軸から足先までの距離 $r$ は、股ピッチに関係なく膝の角度だけで決まります。高校の余弦定理そのものですが、$q_3$ は「伸び切った状態（0）からの折れ角」なので、見慣れた $-2ab\cos$ ではなく $+2ab\cos$ の形になります（膝の内角は $\pi - |q_3|$）。
$$
r = \sqrt{L_{\text{thigh}}^2 + L_{\text{calf}}^2 + 2 L_{\text{thigh}} L_{\text{calf}} \cos q_3}
$$
| 膝 $q_3$ | $r$ | 意味 |
|---|---|---|
| −0.916 rad（可動域の上限） | 0.359 m | 最も伸ばした状態。伸び切り 0.400 m には届かない |
| −1.5 rad（立ち姿勢） | 0.292 m | 図4の姿勢 |
| −2.697 rad（可動域の下限） | 0.088 m | 最も曲げた状態。これより股に近づけない |
立ち姿勢 (0, 0.8, −1.5) で足先は胴体の 0.292 m 下です。4本とも同じ角で地面に立たせると、胴体の高さは地面から約 0.29 m になります（実際には足先の球の半径ぶんだけ高くなります）。
図6で上半分（z > 0）にも青い点があるのは、股ピッチの可動域が −1.047〜4.189 rad（−60°〜240°）と広く、脚を胴体の上まで振り上げられるためです。式の上では届きますが、胴体に当たるので実機では使えない範囲です。式は「関節の角度が取れる範囲」しか見ておらず、リンク同士や地面との干渉は含みません。
この記事の式でできないこと
左脚は股の符号が反転します。 leg_fk(q, side=+1.0) と、股ロール軸の位置を FL_hip_joint の (0.183, 0.047, 0) に差し替えれば前左脚になります。後ろの2本は股ロール軸の x が −0.183 になります（RR_hip_joint の <origin xyz> は -0.183 -0.047 0）。4本ぶんを1つの関数にまとめるのは、歩容（4本の足先を時間で並べる話）の回でやります。
胴体が動く場合は含みません。 この記事は胴体を原点に固定しました。歩いているロボットでは胴体の位置と傾きが変わるので、足先の世界座標を出すには胴体の姿勢（位置3つ＋向き3つ）を先頭に掛ける必要があります。これは連載の④半身ロボットで「浮遊ベース（floating base：胴体が固定されていない運動学ツリー）」として扱います。
足先の位置から関節角を出す逆問題は次回です。 今回の式は「角→位置」の一方向です。「足先をここに置きたい」から3つの角を逆算する逆運動学は、10月7日公開の次回で扱います。①の差動二輪と違って解が2つ（膝の折れる向き）出るので、そこが主題になります。
力・トルク・接触は扱いません。 resetJointState は物理を進めずに角度を書き換えるだけです。実際に地面を蹴って立つには、関節にトルクを与えて接触力を受ける必要があり、それは運動学の外です。
resetJointState は可動域を守りません。 全関節 0 rad（膝の可動域の外）を入れても PyBullet はそのまま受け取り、足先を (0.183, −0.132, −0.400) に置きます。可動域は setJointMotorControl2 で駆動するときに効くものなので、式の検算で可動域外の角を入れても警告は出ません。
まとめ
- A1 の脚1本は股ロール・股ピッチ・膝の3自由度で、足首はありません。足先の位置は決められますが、向きは選べません
- 順運動学は $R_x(q_1)$・$R_y(q_2)$・$R_y(q_3)$ の3つの回転と、URDF の
<origin>に書かれた3つの長さ（0.08505 / 0.2 / 0.2 m）で書けます。可動域内のランダム 2000 姿勢で PyBullet と 2.97×10⁻⁸ m の差です -
getJointInfoのparentFramePosは親リンクの重心基準です。そのまま足すと足先が 75〜134 mm ずれます。getDynamicsInfo(robot, parent)[3]を足し戻すか、URDF を直接読めば直ります - 膝は −0.916 rad までしか伸びないので、脚の長さは最大 0.359 m です。伸び切り 0.400 m の円には届きません
- 検算するときは、胴体を原点に固定し、
getLinkState(...)[4]（リンクフレーム位置）と比べます。[0]は重心位置です
完成スクリプト（1ファイル）
本文のコードは節ごとに分けて載せたので、①の関節一覧と③の load_a1() で p.connect が2回出てきます。上から順に1ファイルへ貼ると2回接続することになり、動きはしますが気持ちが悪いので、②以降を1ファイルにまとめた完成形を置きます（①は関節番号を確かめるための使い捨てです）。python leg_fk.py で、本文の表と同じ数字が出ます。
leg_fk.py（クリックで展開）
"""Unitree A1 の前右脚（FR）の順運動学を式で書き、PyBullet の足先位置と突き合わせる。
pip install pybullet numpy だけで動く（URDF は pybullet_data に同梱）。
"""
import numpy as np
import pybullet as p
import pybullet_data
# ---------- URDF の <origin xyz> から写した3つの長さ（単位 m） ----------
HIP_ORIGIN = np.array([0.183, -0.047, 0.0]) # trunk → FR_hip_joint
L_HIP = 0.08505 # FR_hip_joint → FR_upper_joint（横向き。右脚は -y）
L_THIGH = 0.2 # FR_upper_joint → FR_lower_joint（下向き -z）
L_CALF = 0.2 # FR_lower_joint → FR_toe（下向き -z）
# ---------- PyBullet の関節番号（getJointInfo で確認した値） ----------
J_HIP, J_THIGH, J_CALF, L_TOE = 1, 3, 4, 5
def rot_x(angle: float) -> np.ndarray:
"""x 軸まわりに angle [rad] 回す 3x3 の回転行列"""
c, s = np.cos(angle), np.sin(angle)
return np.array([[1.0, 0.0, 0.0],
[0.0, c, -s],
[0.0, s, c]])
def rot_y(angle: float) -> np.ndarray:
"""y 軸まわりに angle [rad] 回す 3x3 の回転行列"""
c, s = np.cos(angle), np.sin(angle)
return np.array([[c, 0.0, s],
[0.0, 1.0, 0.0],
[-s, 0.0, c]])
def leg_fk(q: np.ndarray, side: float = -1.0) -> np.ndarray:
"""関節角 q = (股ロール, 股ピッチ, 膝) [rad] から足先位置 [m] を返す（胴体座標系）
side は右脚なら -1.0、左脚なら +1.0（股の横向き長さの符号だけが違う）。
"""
q_roll, q_pitch, q_knee = q
p_calf = rot_y(q_knee) @ np.array([0.0, 0.0, -L_CALF]) # 膝から足先
p_thigh = rot_y(q_pitch) @ (np.array([0.0, 0.0, -L_THIGH]) + p_calf) # 股ピッチから足先
p_hip = rot_x(q_roll) @ (np.array([0.0, side * L_HIP, 0.0]) + p_thigh) # 股ロールから足先
return HIP_ORIGIN + p_hip
def load_a1() -> int:
"""A1 を原点に固定して読み込む（胴体座標系 = 世界座標系 にするため）"""
p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
return p.loadURDF("a1/a1.urdf", [0.0, 0.0, 0.0], useFixedBase=True)
def foot_pos_pybullet(robot: int, q: np.ndarray) -> np.ndarray:
"""PyBullet に関節角を入れて、足先リンクの位置を返す"""
for joint, angle in zip((J_HIP, J_THIGH, J_CALF), q):
p.resetJointState(robot, joint, float(angle))
state = p.getLinkState(robot, L_TOE, computeForwardKinematics=True)
return np.array(state[4]) # [4] = リンクフレームの位置（[0] は重心位置なので使わない）
def leg_fk_from_joint_info(robot: int, q: np.ndarray, fix_com: bool = False) -> np.ndarray:
"""getJointInfo の parentFramePos を足し合わせて組んだ順運動学
fix_com=False: parentFramePos をそのまま使う（親リンクの重心基準なのでずれる）
fix_com=True : 親リンクの重心位置 getDynamicsInfo[3] を足し戻す（URDF の <origin> に戻る）
"""
offset = {}
for joint in (J_HIP, J_THIGH, J_CALF, L_TOE):
info = p.getJointInfo(robot, joint)
pos = np.array(info[14]) # parentFramePos
if fix_com:
parent = info[16] # 親リンク番号（-1 は胴体）
pos = pos + np.array(p.getDynamicsInfo(robot, parent)[3])
offset[joint] = pos
q_roll, q_pitch, q_knee = q
p_calf = rot_y(q_knee) @ offset[L_TOE]
p_thigh = rot_y(q_pitch) @ (offset[J_CALF] + p_calf)
p_hip = rot_x(q_roll) @ (offset[J_THIGH] + p_thigh)
return offset[J_HIP] + p_hip
def main() -> None:
robot = load_a1()
# 関節の並びと可動域を表で確認する
print("index | name | type | axis | lower | upper")
for joint in range(p.getNumJoints(robot)):
info = p.getJointInfo(robot, joint)
if not info[1].decode().startswith("FR_"):
continue
kind = {p.JOINT_REVOLUTE: "REV", p.JOINT_FIXED: "FIXED"}[info[2]]
print(f"{joint:5d} | {info[1].decode():15s} | {kind:5s} | {info[13]} | {info[8]:6.3f} | {info[9]:6.3f}")
# 代表4姿勢で突き合わせる
poses = {
"立ち姿勢": np.array([0.0, 0.8, -1.5]),
"膝を最も曲げる": np.array([0.0, 0.8, -2.697]),
"股を内へ": np.array([0.5, 0.8, -1.5]),
"膝を最も伸ばす": np.array([0.0, 0.8, -0.916]),
}
print("\n姿勢 | 式 [m] | PyBullet [m] | 差 [mm] | parentFramePos 版の差 [mm]")
for name, q in poses.items():
a = leg_fk(q)
b = foot_pos_pybullet(robot, q)
c = leg_fk_from_joint_info(robot, q)
print(f"{name} | {np.round(a, 4)} | {np.round(b, 4)} | {np.linalg.norm(a - b) * 1e3:.3f} | {np.linalg.norm(c - b) * 1e3:.1f}")
# 可動域内のランダム 2000 姿勢で最大誤差を見る
limits = np.array([p.getJointInfo(robot, j)[8:10] for j in (J_HIP, J_THIGH, J_CALF)])
rng = np.random.default_rng(0)
samples = rng.uniform(limits[:, 0], limits[:, 1], size=(2000, 3))
err_fk = [np.linalg.norm(leg_fk(q) - foot_pos_pybullet(robot, q)) for q in samples]
err_raw = [np.linalg.norm(leg_fk_from_joint_info(robot, q) - foot_pos_pybullet(robot, q)) for q in samples]
err_fix = [np.linalg.norm(leg_fk_from_joint_info(robot, q, fix_com=True) - foot_pos_pybullet(robot, q)) for q in samples]
print(f"\nランダム2000姿勢の最大誤差")
print(f" 式（URDF の長さ） : {max(err_fk):.2e} m")
print(f" parentFramePos そのまま : {max(err_raw) * 1e3:.1f} mm（最小 {min(err_raw) * 1e3:.1f} mm・平均 {np.mean(err_raw) * 1e3:.1f} mm）")
print(f" parentFramePos + 親の重心: {max(err_fix):.2e} m")
# なぜずれるか：URDF の <origin> と parentFramePos と 親リンク重心 を並べる
print("\njoint | URDF <origin xyz> | parentFramePos | 親リンクの重心")
urdf_origin = {J_HIP: HIP_ORIGIN, J_THIGH: [0, -L_HIP, 0], J_CALF: [0, 0, -L_THIGH], L_TOE: [0, 0, -L_CALF]}
for joint in (J_HIP, J_THIGH, J_CALF, L_TOE):
info = p.getJointInfo(robot, joint)
com = p.getDynamicsInfo(robot, info[16])[3]
print(f"{info[1].decode():15s} | {np.round(urdf_origin[joint], 5)} | {np.round(info[14], 5)} | {np.round(com, 5)}")
# 脚の長さの範囲（股ピッチ軸から足先まで）
reach = np.sqrt(L_THIGH**2 + L_CALF**2 + 2 * L_THIGH * L_CALF * np.cos(limits[2]))
print(f"\n股ピッチ軸から足先までの距離: 膝 {limits[2][0]:.3f} rad で {reach[0]:.3f} m、膝 {limits[2][1]:.3f} rad で {reach[1]:.3f} m（伸び切り {L_THIGH + L_CALF:.3f} m には届かない）")
if __name__ == "__main__":
main()
本シリーズの全体像は、まとめ記事から辿れます。
ロボットの自作記事まとめ（2軸・3軸・6軸アームから PyBullet まで）：
経路生成シリーズのまとめ（ダイクストラ・A*・RRT）：
次回の逆運動学は今回の leg_fk をそのまま前提にするので、ストックしておくとこの式に戻る目次になります。役に立ったら いいね・ストック で応援いただけると、次回の励みになります。