ロボットの URDF を使ってモーター温度を 3D モデル上にマッピングしてみた
はじめに
製造ビジネステクノロジー部のふじい(大)です。
ロボットによっては、軸ごとのモーター温度が取れます。
Universal Robots なら RTDE の joint_temperatures で 6 軸ぶんが摂氏で読めますし、Unitree Go2 なら lowstate の motor_state[i].temperature に 12 軸ぶんが入っています。
ただ、ダッシュボードに 6 個や 12 個の数値が並んでいても、どの軸のどこが熱いのかはすぐには読めません。
第 3 軸が 52 ℃ と出ていたとして、それが肘なのか手首なのかを頭の中で対応づけないと、機体のどこに熱が溜まっているのかは分からないままです。
そこで、温度を機体の 3D モデルの上に色で重ねて、ブラウザで見られるようにしてみました。
1 軸だけ温度が上がっていれば、色を見てどの軸かがその場で分かります。
ビルドしたものを公開しています。温度はダミー値ですが、ブラウザで動かせます。
?robot=ur5e を付けると UR5e に切り替わります。
この記事では、URDF から熱源の座標を取る方法と、その周りだけを塗る方法を書きます。
姿勢は固定で、関節角は流し込んでいません。温度の履歴も残していません。
ビューアーなので温度を読み取るだけで、機体へ指令は送りません。
実機の温度は Unitree Go2 で取りました。伏せと立ち上げを繰り返し、温度が上がっていく様子が観測できました。
他のロボットでも実現できることは、Universal Robots UR5e の URDF を読み込んで確かめました（温度はダミー値です）。
検証環境は次のとおりです。
| 項目 | 値 |
|---|---|
| 機体 | Unitree Go2 R&D+（EDU 相当）、100 TOPS 構成 |
| 3D モデル | unitree_ros の go2_description（commit 7d6075f7）、Universal_Robots_ROS2_Description の UR5e（タグ 4.3.1） |
| ブラウザを動かしたマシン | macOS Tahoe 26.6.2、Google Chrome 152.0.7977.83（arm64） |
| ブラウザ側の依存 | TypeScript 5.9.3、three.js 0.183.2、urdf-loader 0.13.1、Vite 6.4.3 |
| xacro の展開 | ros:jazzy の Docker、xacro 2.1.1 |
| 温度の取得 | Python 3.10、unitree_sdk2py で DDS の rt/lf/lowstate を購読 |
| ブリッジを動かしたマシン | NVIDIA DGX Spark（Go2 と有線接続） |
Unitree Go2 が大阪オフィスに来たときの様子は前回の記事にあります。
URDF とは
URDF は、ロボットの形と構造を書く XML の形式です。
Unified Robot Description Format の略で、ROS で標準的に使われています。
中身は link と joint の 2 つでできています。
link は部品で、形を表すメッシュファイルへの参照や、重さの情報を持ちます。
joint は関節で、2 つの link をつなぎ、どこにどの向きで付くか、どの範囲で動くかを持ちます。
UR5e の肘の joint は次のようになっています。
<joint name="elbow_joint" type="revolute">
<parent link="upper_arm_link"/>
<child link="forearm_link"/>
<origin xyz="-0.425 0 0" rpy="0 0 0"/>
<limit lower="-3.141592653589793" upper="3.141592653589793" effort="150.0" velocity="3.141592653589793"/>
</joint>
origin の 0.425 が上腕の長さです。
ロボットメーカーが公開していることが多く、シミュレーターや動作計画で使われます。
この記事では、link のメッシュを画面に出すことと、joint が持つ位置の情報を使うことの 2 つに使います。
URDF から熱源の座標を取る
joint は <origin> に、親 link の座標系での子 link の原点を書きます。
つまり子 link の原点が、その関節の位置です。
モーターが関節軸と同軸に置かれている機体なら、これをモーターの位置として使えます。
駆動方式によっては一致しません。 ベルトやワイヤ、リンク機構で離れた関節を動かす構造では、関節の位置とモーターの位置が別になります。
Go2 の膝がこれに当たります。
より確かな座標が URDF に入っていることもあります。
Go2 の go2_description には *_rotor という link があり、visual を持たないので画面には何も描かれません。
joint の親と origin が実際の取り付け位置なので、関節の位置から推測する必要がありません。
熱源の座標は、次の順で探すことになります。
- 温度センサーの位置を表す link
*_rotorのようなアクチュエータの link- ベンダーが公開している CAD や仕様
- joint の origin。上のどれも無いときはこれで代用する
ここで決めているのは、3D モデルのどこに色を置くかです。
センサーが実際にどこを測っているかは別の話で、公開されていないことが多く、この記事で扱う 2 機種でも分かっていません。最後の節で書きます。
関節が回っても、子 link の原点は親から見て動きません。
revolute joint が変えるのは軸まわりの回転で、原点の位置ではないためです。
そのため、その関節に接する link のメッシュとの相対位置は、姿勢が変わっても同じままです。
xacro を展開する
産業用ロボットの URDF は、公式リポジトリでは xacro のまま置かれていることが多いです。
Universal Robots、Franka Robotics、ros-industrial の ABB / FANUC / KUKA / Motoman を見ましたが、素の .urdf が入っているものはありませんでした。
ブラウザは xacro を展開できないので、先に URDF にしておく必要があります。
UR5e は urdf/ur.urdf.xacro 1 本がトップレベルで、機種ごとの差は config/ur5e/*.yaml に分かれています。
xacro urdf/ur.urdf.xacro ur_type:=ur5e name:=ur5e > ur5e.urdf
このコマンドを通すには ROS の環境が要ります。
xacro が読む $(find ur_description) は ament のパッケージ検索で解けるので、clone したディレクトリを share/ur_description として見せる必要があります。
ros:jazzy の Docker で組み立てました。まず xacro を入れたイメージを作ります。
FROM ros:jazzy
RUN apt-get update -qq \
&& apt-get install -y -qq ros-jazzy-xacro \
&& rm -rf /var/lib/apt/lists/*
docker build -t ur-xacro .
次に、タグを固定して clone したものをコンテナに渡し、ament のインデックスを組み立ててから展開します。
git clone --depth 1 --branch 4.3.1 \
https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git ur-desc
docker run --rm -v "$PWD/ur-desc:/src:ro" ur-xacro bash -lc '
{
mkdir -p /ament/share/ament_index/resource_index/packages
touch /ament/share/ament_index/resource_index/packages/ur_description
ln -s /src /ament/share/ur_description
} 1>&2
source /opt/ros/jazzy/setup.bash
export AMENT_PREFIX_PATH=/ament:$AMENT_PREFIX_PATH
xacro /src/urdf/ur.urdf.xacro ur_type:=ur5e name:=ur5e
' > ur5e.urdf
準備の出力を 1>&2 で標準エラーに送っているのは、標準出力を URDF だけにするためです。
ここに apt のログが混ざると、そのまま URDF に流れ込みます。
macOS で作業するなら、マウント元のパスに注意が要ります。
/tmp は /private/tmp へのシンボリックリンクなので、/tmp/... を渡すとコンテナ側で空のディレクトリが見えます。
最初これで No such file or directory に当たりました。
展開すると 12 KB ほどの URDF が 1 つできます。
これと meshes/ur5e/visual/ の COLLADA 7 ファイル（合計 9.3 MB）があれば、ブラウザ側はそれだけで足ります。
mesh の参照は package://ur_description/meshes/ur5e/visual/base.dae の形なので、urdf-loader の packages でパッケージ名を配信先に読み替えます。
読み込んだあとに姿勢を当てるかどうかは、機種によって変わります。
UR5e は全関節の可動域が 0 を含むので、そのままでも形は崩れません。
Go2 は *_calf_joint の limit が [-2.7227, -0.83776] で 0 が範囲の外にあり、読み込んだだけでは 4 本の脚がまっすぐ伸びた棒になります。立ち姿勢を当ててから接地させています。
メッシュを読む部分は自前に差し替えてあります。
urdf-loader の loadMeshCb は 0.13.0 で第 3 引数に material が入って 4 引数になりました。
検証に使った 0.13.1 でもこの形です。
type MeshLoadFn = (
url: string,
manager: THREE.LoadingManager,
material: THREE.Material,
onLoad: (mesh: THREE.Object3D, err?: Error) => void,
) => void;
README を含めて出回っている作例は 3 引数のままなので、そのまま貼るとコールバックが呼ばれません。
読み込みに失敗しても console.error で握って先へ進むため、放っておくと無音でモデルが欠けます。
熱源に指定する link を決める
UR5e は 6 軸で、駆動される link を熱源に指定します。
| 軸 | joint | 熱源に指定する link |
|---|---|---|
| 1 | shoulder_pan_joint |
shoulder_link |
| 2 | shoulder_lift_joint |
upper_arm_link |
| 3 | elbow_joint |
forearm_link |
| 4 | wrist_1_joint |
wrist_1_link |
| 5 | wrist_2_joint |
wrist_2_link |
| 6 | wrist_3_joint |
wrist_3_link |
色を乗せる先は、指定した link 自身と、その親の link の 2 つにします。
関節をまたぐと link が変わるので、片側だけにすると反対側が素の色のまま残ります。
実機では関節の両側が熱くなるので、両方に乗せるほうが実物に近くなります。
片側だけでは色が乗らない軸も出ます。
軸 1 の joint は肩の回転中心にあって、高さは 163 mm です。
一方その親である土台のメッシュは高さ 99 mm までしかないので、joint の位置はメッシュの外にあります。
親側だけを対象にすると、この軸には色が乗りません。
座標が一致する軸もあります。
0 姿勢で base_link を原点に測ると、軸 1 と軸 2 の熱源はどちらも z=+0.163 です。
ショルダーの 2 軸は回転中心が同じ位置にあるためで、座標では区別できません。
いちばん近い 1 個が勝つ形にしているので、両方が乗る shoulder_link は軸 1 の色になり、軸 2 の色は上腕側にだけ乗ります。
この形は異変に気付く用途には向きません。 軸 2 のほうが熱くても、shoulder_link には軸 1 の色が出ます。
1 つの面に 1 色しか持てないので、同じ位置にある複数の温度を色だけで表すことはできません。
そのため軸ごとの数値を併せて出しています。温度の高い側を勝たせる、熱源の位置に軸ごとのマーカーを少しずらして置く、といった手もあります。
塗る半径は、熱源から部材の表面までの距離で決まります。
熱源は関節の回転中心にあり、部材はその周りの中空の筒です。
UR5e で熱源からいちばん近い頂点までを測ると、ショルダーの 2 軸が 56 mm、肘が 22 mm、手首の 3 軸が 16 〜 38 mm でした。
半径がこの距離を下回ると、その熱源には 1 頂点も色が乗りません。
60 mm から始めたときは、ショルダーの周りがほとんど塗られませんでした。100 mm にすると 6 軸すべてに乗りました。
この距離は機体ごとに違います。
Go2 は熱源が筐体の中にあり、いちばん近い頂点までが thigh で 15 mm、calf で 40 mm、hip で 46 〜 50 mm でした。
70 mm で足りているのはこのためです。
自分の URDF で決めるときは、まず熱源からいちばん近い頂点までを測ってみてください。
link 全体を塗ると熱源から離れる
最初は link ごとに色を塗れば済むと思っていました。
ある軸の温度が来たらその軸の link を塗る、という素直な対応です。
最初に当たったのは、material の実体が link をまたいで共有されることでした。
Go2 の hip.dae は 4 つの link から参照されています。
同じファイルを何度も parse しないよう、読み込んだ結果を clone() して使い回していました。
Object3D.clone() は、geometry と material の参照を引き継ぎます。
オブジェクトの階層は複製されますが、material は同じ実体を指したままです。
そのため右前脚の hip の色を変えたら、4 本の脚の hip が全部同じ色になりました。
分けるには material そのものを複製します。
mesh.material = Array.isArray(mesh.material)
? mesh.material.map((m) => m.clone())
: mesh.material.clone();
1 つの .dae の中でも Mesh 同士が material を共有するので、元の material を鍵にして複製先を覚えておくと無駄が出ません。
塗り分けができたところで、位置が合っていないことに気付きました。
Go2 の URDF を立ち姿勢で測ると、1 脚ぶんのモーター 3 個はどれも胴体と同じ高さにあって、最大間隔は 103 mm しかありません。
膝を動かすモーターも thigh の付け根にあり、リンク機構で calf を駆動しています。
モーターは calf にも足にも入っていません。
calf の温度で calf の link を塗ると、熱源から 215 mm 離れた場所が赤くなってしまいます。
足まで塗れば 341 mm です。
URDF の外形は前後方向に 704 mm なので、全長の 3 割ほどずれた位置に色が乗ります。
すねが赤くなる絵は分かりやすいのですが、そこは熱くなっていません。
冒頭に貼った画像と同じ温度です。
熱源の周りだけを塗ればモーターのある付け根が赤くなり、link 単位に塗るとすねが赤くなります。
Go2 のずれがここまで大きいのは駆動方式のためです。
減速機とモーターが関節にある 6 軸アームなら、joint の位置とモーターの位置はほぼ一致するので、これほど離れません。
それでも link 全体を塗る限り、熱源から離れた先まで色が乗ります。
UR5e の上腕を丸ごと塗れば、モーターがある付け根から 425 mm 先の肘側まで同じ色になります。
どこが熱いのかを見たいなら、塗る範囲を熱源の周りに絞る必要があります。
熱源の周りだけを塗る
頂点ごとに、いちばん近い熱源の添字と、距離から決めた重みを求めます。
for (let i = 0; i < count; i++) {
vertex.fromBufferAttribute(position, i).applyMatrix4(toWorld);
let bestDistance = Infinity;
let bestIndex = 0;
for (const { site, world } of sites) {
const distance = vertex.distanceTo(world);
if (distance < bestDistance) {
bestDistance = distance;
bestIndex = site.slot;
}
}
if (bestDistance >= radius) continue;
motor[i] = bestIndex;
// 中心から 70 % までは重み 1 で、そこから radius までで 0 に落ちる
heat[i] = 1 - THREE.MathUtils.smoothstep(bestDistance, radius * 0.7, radius);
}
求めた添字と重みは頂点属性として持たせ、色はシェーダで混ぜます。
温度が更新されるたびにやることはユニフォームの書き換えだけで、頂点は一度計算したら触りません。
候補にする熱源は、そのメッシュが属する link に固定されているものだけです。
モデル全体から最近傍を選ぶと、機体が折り畳まれて別の関節が近づいたときに割り当てが変わってしまいます。
ただし今は姿勢を固定して出しているので、動かして確かめてはいません。
シェーダに渡すのは温度の値で、色はフラグメントごとにカラーマップから引きます。
色を頂点で決めて補間すると、カラーマップの上に無い色を通ってしまいます。
その代わり、隣り合う頂点が別の軸に割り当てられている境目では、2 つの軸の温度が混ざった値になります。
この割り当ては初期化のときに 1 回だけ走ります。
計算量は頂点数と熱源の数の積で、Go2 は塗る 9 メッシュで頂点が 100 万を超えるので、読み込み時間の一部をここが占めます。
距離はワールド座標で測っています。
メッシュの座標系は、モデルの作り方によって単位や軸の向きが変わります。
COLLADA は単位の宣言とノードの行列を別に持てるので、宣言がメートルでも頂点がミリメートルのまま残っていることがあります。
ワールド座標に揃えれば単位はメートル、軸は URDF のもので固定されるので、半径をそのまま当てられます。
重み付き平均にすると、103 mm の中に固まった 3 個が混ざって区別できなくなるので、いちばん近い 1 個が勝つ形にしています。
塗る範囲が広すぎないかは、色が乗る面積で見ます。
頂点の数は面積の代わりになりません。メッシュの三角形はモーターの筐体に集中しているので、頂点で数えると実際に見える面積からかけ離れます。
面積は三角形ごとに、3 頂点の重みを平均して按分しています。
こうして測ると、UR5e は次のとおりでした。
| 部位 | 表面積 | 色が乗る割合 |
|---|---|---|
base_link_inertia |
745 cm² | 8.2 % |
shoulder_link |
841 cm² | 64.9 % |
upper_arm_link |
2,427 cm² | 2.7 % |
forearm_link |
1,427 cm² | 26.4 % |
wrist_1_link |
482 cm² | 96.8 % |
wrist_2_link |
424 cm² | 100.0 % |
wrist_3_link |
167 cm² | 100.0 % |
手首の 3 部材が 96 % を超えるのは、部材そのものがモーターの筐体に近い寸法だからです。
上腕が 2.7 % なのは、425 mm の長さに対して色が乗るのが付け根だけだからです。
Go2 は base が 17.1 %、4 つの *_hip が 77.7 %、4 つの *_thigh が 75.1 % でした。
*_hip が高いのは、hip の円筒部分が腿を動かすモーターの筐体そのものだからです。
値が来ていない軸を最低温として描かない
温度を 1 本の配列で持っていると、値が来ていない軸は 0 のまま残ります。
レンジの下端が 20 ℃ なら、画面には濃い紫の「冷たい軸」が描かれます。
これでは、20 ℃ まで冷えている軸と、そもそも値が届いていない軸を画面で見分けられません。
軸ごとに値が来たかどうかを持つユニフォームを足して、来ていないものは重みを 0 にしました。
素の色のまま残るので、色が付いていないことが見て分かります。
軸数が URDF と合わないときや、一部の軸だけ温度が取れないときに、ここを踏みます。
色の範囲をどう決めるか
保護閾値まであとどれくらいか見るなら、20 〜 80 ℃ のような絶対値で範囲を決めます。
ただ、この範囲だとどの軸が熱いかは読めません。
Go2 の待機中のモーター温度は 33 〜 38 ℃ で、伏せと立ち上げを繰り返せば上がりますが、60 ℃ の幅の中では数ミリしか色が動きません。
どの軸に負荷が寄っているかを見たいときは、範囲を 30 〜 45 ℃ のように狭めます。
見たいものが違えば範囲も変わるので、画面の入力欄で変えられるようにしてあります。
配色は turbo の 256 段です。
three.js の Lut には rainbow が入っていますが、これは jet と同じ並びで、データにない段差を見せます。
turbo はその差し替えとして作られた配色で、こちらのほうが局所的な差を読み取りやすくなります。
20 ℃ から 80 ℃ を割り当てた場合、両端は思うほど鮮やかではありません。
下端が #30123B の暗い紫、上端が #7A0403 の暗い赤です。
凡例と画面の色が合っているかは、2 つに分けて確かめます。
1 つは、カラーマップの数値がそのまま material に入っているかです。
turbo の表は sRGB の値で、three.js の material が持つ色は作業色空間なので、変換を 1 回どこかで挟む必要があります。
256 段すべてを往復させて元の値に戻ることをスクリプトで確かめました。
もう 1 つは、画面に出た画素が凡例と合っているかです。
こちらは material の色が正しくても、照明や tone mapping が入るので一致しません。
画面で赤が明るく見えるのは照明で持ち上がっているぶんです。
比べるときは陰影を消すモードに切り替えて、画素が material の色そのものになる状態にします。
そこで明るく出れば変換漏れ、暗く出れば二重変換です。
色は時定数 0.25 秒で滑らかに動かしています。
ここで動かすのも温度の値です。理由は面の中の補間と同じで、色を混ぜるとカラーマップの上に無い色を通ります。
温度データをつなぐ
画面に渡すのは、軸ごとの温度の配列と受信時刻だけです。
すでに温度を取得できているなら、その形に直すところだけ書けば画面に載ります。
Go2 では DDS の rt/lf/lowstate を購読して、温度だけをブラウザへ流す小さなブリッジを書きました。
片方向で足りるので Server-Sent Events にしてあります。
UR なら RTDE の joint_temperatures（VECTOR6D、摂氏）がここに入ります。
温度を流す側を書いていて、外せないと分かったことが 3 つあります。
1 つは、値が変わらなくても毎回送ることです。
受信が途切れたことは最後に届いた時刻で判定しているので、重複を省くと静止した機体で「受信が古い」と誤って出ます。
2 つめは、途切れたら古い値を新しい値として見せないことです。
ブリッジは lowstate が 2 秒途切れた時点でデータの送出をやめ、SSE のコメント行に切り替えます。
EventSource はコメント行を message として配らないので、途切れがそのまま画面に出ます。
機体をシャットダウンして確かめると、色も数値も出ず「受信を待っている」のまま止まりました。
古い値が残って、あたかも今の温度であるかのように見えることはありません。
3 つめは、1 バイトの符号です。
lowstate の温度は通信では 1 バイトで送られますが、unitree_ros2 の .msg は int8、unitree_sdk2 の IDL は uint8 で食い違っています。
unitree_sdk2py は後者なので、そのまま読むと値域の外が 200 ℃ 台になります。購読する側で符号を戻しました。
def signed(value: int) -> int:
return value - 256 if value > 127 else value
配列の何番目がどの軸かを間違えると、画面は正常に見えたまま別の軸の温度が出ます。
Go2 の並びは unitree_ros2 の motor_crc.h の定義に合わせ、ダミーの温度で 1 軸だけ熱くして両端と中間の対応を画面で確かめました。
つないでいていちばん分かりにくかったのは、購読のコールバックで例外を投げたときです。
unitree_sdk2py は受信スレッドの中でコールバックを呼ぶので、例外が出るとそのスレッドが終わります。
以後 lowstate は二度と届かず、画面には「受信が途切れている」としか出ないので、原因にたどり着けません。
切り分けは流す側で、受け取った件数と最後に届いた時刻を見ることになります。
なお、この構成は検証用です。
ブリッジは既定で 127.0.0.1 にだけ bind し、機体と有線でつないだ閉じたネットワークで動かしています。
外に出すなら、bind するインターフェース、認証、CORS の扱いを決めることになります。温度や稼働状況も、外に流したい情報ではありません。
温度の意味はどこまで分かっているか
motor_state[i].temperature を、ステーター、ドライバ基板、筐体のどこで測っているかは公開されていません。
色はモーターの位置に置いていますが、センサーがそこにあるかどうかは分かりません。
Go2 の lowstate には temperature_ntc1 と temperature_ntc2 もありますが、どこの温度かは未確認です。
unitree_ros2 の .msg にも README にもコメントがなく、隣の power_v と power_a には電池の電圧と電流だと書いてあるのに、この 2 つだけ空欄になっています。
「ntc1 がメインボード中心、ntc2 が自動充電部」という説明を見かけますが、出どころは Unitree の H1 開発者ドキュメントのミラーで、Go2 について書かれたものではありませんでした。
そのため画面では ntc1 と ntc2 というフィールド名のまま出し、部位の名前は当てていません。
基板、電池、充電部に相当する link は URDF に 1 つもないので、推測で座標を置くと、*_rotor から取った位置と見た目で区別がつかなくなります。
imu_state.temperature には 79 という値が入っていました。
伏せて待機しているだけで、同じ機体のモーターが 33 〜 38 ℃ のときの値です。
imu link は URDF にあって座標が確定しているので色にしていますが、この値のまま出すと胴体中央が常に赤く見えます。
モーター温度の保護閾値も公表されていません。
画面に置いた 50 ℃ と 70 ℃ の目印は、他の実装が使っている値に合わせただけで、保護閾値ではありません。
範囲の既定を 20 〜 80 ℃ にしているのも仮置きです。
伏せと立ち上げを繰り返して温度が上がることは見ましたが、上端に近づくところまでは上がりませんでした。
上端をどこに置くかの根拠は、まだ得られていません。
測定点が分からない値は、色にせず数値だけ出しました。
temperature_ntc1 のように部位の名前も当てず、範囲は後から変えられるようにしてあります。
産業用ロボットのコントローラも測定点を公開していないことが多いので、同じ判断がどこかで必要になります。
おわりに
URDF があれば、軸ごとのモーター温度を機体の 3D モデルの上に色で置くことができました。
1 軸だけ温度が上がっていれば、色を見てどの軸かがその場で分かります。
要点は 4 つです。
- joint の
<origin>が表すのは関節の位置で、モーターの実装位置とは限らない。同軸に置かれた機体ならそのまま使える *_rotorのようなアクチュエータの link や、ベンダーが公開している CAD があるなら、そちらを先に探す- link 全体を塗ると熱源から離れた先まで色が乗る。頂点ごとにいちばん近い熱源を持たせて、その周りだけを塗る
- 色を割り当てる範囲は、保護閾値への余裕を見るのか軸間の差を見るのかで変わる。同じ位置に複数の軸があるときは、面の色だけでは足りないので数値を併せて出す
よりロボットの情報を反映させたいなら、関節角を流し込んで実機と同じ姿勢にすることもできると思います。
今回は温度データの可視化がメインテーマだったのでそこまでやっていません。この記事を参考に 3D モデルを使ったロボットデータの可視化を行う際はそういったところまでやってみるとおもしろいかもしれません。
ロボットの状態を可視化したい方の参考になれば幸いです。
