urdfeus 1.2.2__tar.gz → 1.2.4__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (34) hide show
  1. {urdfeus-1.2.2/urdfeus.egg-info → urdfeus-1.2.4}/PKG-INFO +77 -5
  2. {urdfeus-1.2.2 → urdfeus-1.2.4}/README.md +76 -4
  3. {urdfeus-1.2.2 → urdfeus-1.2.4}/pyproject.toml +1 -1
  4. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/eus2urdf.py +9 -0
  5. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/eus2urdf.py +239 -17
  6. urdfeus-1.2.4/urdfeus/eus_colors.py +950 -0
  7. urdfeus-1.2.4/urdfeus/eus_parse.py +2345 -0
  8. urdfeus-1.2.4/urdfeus/eus_reader.py +281 -0
  9. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/euslisp/eus2urdf-dump.l +31 -6
  10. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/urdf2eus.py +47 -47
  11. {urdfeus-1.2.2 → urdfeus-1.2.4/urdfeus.egg-info}/PKG-INFO +77 -5
  12. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus.egg-info/SOURCES.txt +3 -0
  13. {urdfeus-1.2.2 → urdfeus-1.2.4}/LICENSE +0 -0
  14. {urdfeus-1.2.2 → urdfeus-1.2.4}/setup.cfg +0 -0
  15. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/__init__.py +0 -0
  16. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/__init__.py +0 -0
  17. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/common.py +0 -0
  18. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/mesh2eus.py +0 -0
  19. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/urdf2eus.py +0 -0
  20. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/common.py +0 -0
  21. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/compatibility.py +0 -0
  22. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/grouping_joint.py +0 -0
  23. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/mesh2eus.py +0 -0
  24. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/mesh_cache.py +0 -0
  25. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/provenance.py +0 -0
  26. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/read_yaml.py +0 -0
  27. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/templates/__init__.py +0 -0
  28. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/templates/euscollada-robot.l +0 -0
  29. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/templates/urdf_template.py +0 -0
  30. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/templates/urdfeus-glvertices.l +0 -0
  31. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus.egg-info/dependency_links.txt +0 -0
  32. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus.egg-info/entry_points.txt +0 -0
  33. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus.egg-info/requires.txt +0 -0
  34. {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus.egg-info/top_level.txt +0 -0
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: urdfeus
3
- Version: 1.2.2
3
+ Version: 1.2.4
4
4
  Summary: URDF converter for Euslisp
5
5
  Author-email: Iori Yanokura <yanokura@jsk.imi.i.u-tokyo.ac.jp>
6
6
  License: Copyright (c) 2023 Iori Yanokura.
@@ -59,15 +59,33 @@ URDF ⇄ EusLisp を相互変換するPythonライブラリ
59
59
 
60
60
  ## ブラウザで変換する(インストール不要)
61
61
 
62
- 手元のURDFをその場でEusLispに変換できます → **<https://iory.github.io/urdfeus/convert/>**
62
+ 手元のモデルをその場で相互変換できます → **<https://iory.github.io/urdfeus/convert/>**
63
63
 
64
- `urdf2eus`がWebAssembly(Pyodide)として動くため、Pythonの環境構築が要りません。変換はすべてブラウザ内で完結するので、**ファイルはどこにもアップロードされません**。
64
+ `urdfeus`がWebAssembly(Pyodide)として動くため、Pythonの環境構築が要りません。変換はすべてブラウザ内で完結するので、**ファイルはどこにもアップロードされません**。
65
+
66
+ ### URDF → EusLisp
65
67
 
66
68
  URDFをドラッグ&ドロップすると必要なメッシュが一覧表示されるので、「Select files」か「Select directory」でメッシュを渡してください。ファイル名を手がかりに`package://`のパスへ自動で突き合わせるので、メッシュの入ったフォルダごと指定して構いません。URDFとメッシュが同じフォルダにあるなら、フォルダごとドロップするだけで済みます。
67
69
 
70
+ ### EusLisp → URDF
71
+
72
+ `.l`をドロップすると、その場でURDFパッケージに変換し**3Dで表示**します。関節はスライダで動かせ、`package.xml` + `urdf/` + `meshes/`一式をzipでダウンロードできます。生成モデル(`euscollada`/`urdfeus`のロボット、jskeusの物体モデル)は幾何情報をファイル内に持っているので、メッシュを別途渡す必要はありません。
73
+
74
+ **GitHubのURLを貼るだけでも変換できます。** ファイルページのURL(`https://github.com/<owner>/<repo>/blob/<ref>/<path>.l`)を貼ると`raw.githubusercontent.com`に読み替えて取得します。リンクをドロップしても同じです。
75
+
76
+ `?url=`を付ければ、開いた時点で取得と変換まで走ります(URLはパーセントエンコードしてください)。モデルをそのまま共有できます:
77
+
78
+ ```
79
+ https://iory.github.io/urdfeus/convert/?url=https%3A%2F%2Fgithub.com%2Fowner%2Frepo%2Fblob%2Fmain%2Frobot.l
80
+ ```
81
+
82
+ 取得はブラウザからの直接アクセスなので、CORSを許可しているホストに限られます(GitHubのrawは許可しています)。それ以外のURLは失敗するので、ファイルを落としてドロップしてください。
83
+
84
+ `irteusgl`はネイティブプログラムなのでブラウザでは動きません。代わりに`.l`を直接読む[静的パーサ](#eus2urdfのバックエンド)を使います。コードでリンクを組み立てるモデルや、他ファイルを`load`するシーンはこの経路では読めないので、[インストール](#インストール)した`eus2urdf`(irteusgl経由)を使ってください。
85
+
68
86
  初回のみPython実行環境(約33MB)を取得するため数秒かかります。2回目以降はブラウザのキャッシュが効きます。
69
87
 
70
- ブラウザ版で使えない機能が2つあります。`eus2urdf`(EusLisp → URDF)はEusLisp処理系(`irteusgl`)の起動が必要なため動きません。`--voxel-size`によるメッシュ簡素化もopen3dに依存するため使えません。どちらも[インストール](#インストール)した`urdfeus`を使ってください。
88
+ `--voxel-size`によるメッシュ簡素化はopen3dに依存するため、ブラウザでは使えません。
71
89
 
72
90
  ## 概要
73
91
 
@@ -207,7 +225,7 @@ with open('robot.l', 'w') as f:
207
225
 
208
226
  ### 前提
209
227
 
210
- - `irteusgl`(jskeus)がインストールされていること
228
+ - `irteusgl`(jskeus)がインストールされていること(無い場合は後述の静的パーサが使われます)
211
229
  - メッシュ書き出しに`trimesh` / `pycollada`(依存に含まれます)
212
230
 
213
231
  ### コマンドライン
@@ -250,12 +268,66 @@ urdf_path = eus2urdf('robot.l', 'output_package_dir',
250
268
  - `--mesh-format`: `trimesh.export`が扱う拡張子(既定`glb`)。`glb`/`ply`/`obj`は面ごとの色を保持。`dae`はtrimeshのColladaエクスポータが色をtextureに潰すため**多色メッシュがグレーになる**(単色メッシュは保持)。`stl`は色なし
251
269
  - `--draco`: glbメッシュをDraco圧縮(`KHR_draco_mesh_compression`)。頂点色を保ったまま密なメッシュを概ね1桁小さくする。`glb`固定で`DracoPy`が必要(`pip install urdfeus[draco]`)。読み込み側のglTFローダにはDracoデコーダが要る
252
270
  - `--irteusgl`: 使用する`irteusgl`実行ファイル
271
+ - `--backend`: モデルの読み方(後述)
272
+
273
+ #### 慣性テンソルの扱い
274
+
275
+ EusLispの`:weight`と`:inertia-tensor`はそのままURDFの`<mass>`と`<inertia>`にします(`g`→`kg`、`g*mm^2`→`kg*m^2`)。ただしjskeusのモデルには、質量を持ちながら慣性テンソルが剛体として成立していないものがあります。
276
+
277
+ - h3 / h3s / h4 / h6 / h7 / igoid2 / igoid3 / saizo: `:inertia-tensor`がゼロ行列
278
+ - macra: 1e-40 相当の placeholder(数値誤差で固有値が負になるものもある)
279
+ - human: 長軸まわりだけ`1.0`(他の2軸は`2.07e8`、単位は`g*mm^2`)
280
+
281
+ 質量があって慣性が無い剛体は存在しないので、こういうリンクを渡されたインポータは、そのボディを拒否するか、質量から作った既定テンソルを黙って代入します。後者はロボットと何の関係もない値です。そこでurdfeusは、テンソルが正定値でない、主慣性モーメントが三角不等式を満たさない、メッシュの大きさと質量に対して小さすぎる、のいずれかなら、**そのリンクのメッシュから一様密度で慣性を計算し直します**。質量と重心は宣言された値のままです。
282
+
283
+ 書き換えたリンクは変換時に一覧で警告し、URDFにも理由をコメントで残します。
284
+
285
+ ```xml
286
+ <link name="root">
287
+ <!-- inertia recomputed by urdfeus: all-zero inertia tensor -->
288
+ <inertial>
289
+ ```
290
+
291
+ メッシュが無くて計算もできないリンクは、成立しないテンソルを書く代わりに`<inertial>`自体を出力しません(これも警告します)。
292
+
293
+ #### eus2urdfのバックエンド
294
+
295
+ モデルを読む経路は2つあります。
296
+
297
+ | `--backend` | 読み方 | 対応範囲 |
298
+ | --- | --- | --- |
299
+ | `irteusgl` | `irteusgl`でモデルを実体化してダンプ | どんなモデルでも読める。EusLispのインストールが必要 |
300
+ | `static` | `.l`ファイルを直接パースする(`urdfeus.eus_parse`) | 生成モデル限定。EusLispは不要 |
301
+ | `auto`(既定) | `irteusgl`がPATHにあればそちら、無ければ`static` | |
302
+
303
+ `static`が読めるのは**生成された**モデルです。`euscollada`/`urdfeus`がURDF・colladaから吐いたロボット(`gl::glvertices`で幾何を持つもの)と、jskeusの物体モデル(`faceset`で幾何を持つもの)が該当します。手書きでリンクを計算するモデルや、他ファイルを`load`するシーン(`*-scene.l`)は読めず、その旨のエラーになります(黙って一部だけ変換することはありません)。
304
+
305
+ `static`は`:assoc`や`:newcoords`といったEusLispの座標系の意味論を再現しており、jskeusの全モデル651個で`irteusgl`のダンプと突き合わせて検証しています(641個が完全一致、2個は下限が正のジョイントに関する差でURDFとしては等価、8個はシーンで対象外)。
306
+
307
+ ```bash
308
+ # EusLispが入っていない環境でも変換できる
309
+ eus2urdf robot.l out --backend static
310
+ ```
311
+
312
+ ```python
313
+ from urdfeus.eus_parse import parse_eus_model
314
+
315
+ data = parse_eus_model('robot.l') # irteusgl不要。dumpと同じ辞書が返る
316
+ ```
253
317
 
254
318
  #### ジオメトリの扱い
255
319
 
256
320
  - colladaボディは`glvertices`から、`make-cube`等で生成されたプレーンなbody(例:`:init`で追加される可視化用の脚キューブや吸盤)は各faceを三角形分割してメッシュ化します。いずれもメッシュとして書き出されます。
257
321
  - プレーンbodyのface三角形分割は凸面を仮定します(プリミティブ形状では成立)。
258
322
 
323
+ #### 往復(eus -> URDF -> eus)の検証
324
+
325
+ `tests/urdfeus_tests/test_roundtrip.py`が、モデルをEusLispからURDFへ、URDFからEusLispへ戻し、両端の`irteusgl`ダンプを突き合わせます。単位や規約のずれは片道だけ見ると妥当に見えてしまうので、戻してから比べます(`g*mm^2`と`kg*m^2`の1e9、度とラジアン、mmとm、軸の符号)。
326
+
327
+ `dump-robot`は全関節をゼロにしてからダンプするため、比較対象はゼロ姿勢での運動学パラメータ一式(リンク姿勢・関節軸・種類・可動範囲)です。これが一致すれば他の姿勢の順運動学も一致するので、関節角をランダムに振るテストは置いていません。
328
+
329
+ 往復で変わることを許しているのは2点だけです。名前は`:torso-waist-y`が`torso_waist_y`にサニタイズされるので、`eus2urdf`と同じ対応表で突き合わせます。そして剛体として成立しない慣性テンソルを持つリンクは、計算し直されたテンソルで戻ってきます(URDF内のコメントが付いているリンクを慣性の比較から外しています)。
330
+
259
331
  ### 生成されたEusLispファイルの使用
260
332
 
261
333
  ```lisp
@@ -12,15 +12,33 @@ URDF ⇄ EusLisp を相互変換するPythonライブラリ
12
12
 
13
13
  ## ブラウザで変換する(インストール不要)
14
14
 
15
- 手元のURDFをその場でEusLispに変換できます → **<https://iory.github.io/urdfeus/convert/>**
15
+ 手元のモデルをその場で相互変換できます → **<https://iory.github.io/urdfeus/convert/>**
16
16
 
17
- `urdf2eus`がWebAssembly(Pyodide)として動くため、Pythonの環境構築が要りません。変換はすべてブラウザ内で完結するので、**ファイルはどこにもアップロードされません**。
17
+ `urdfeus`がWebAssembly(Pyodide)として動くため、Pythonの環境構築が要りません。変換はすべてブラウザ内で完結するので、**ファイルはどこにもアップロードされません**。
18
+
19
+ ### URDF → EusLisp
18
20
 
19
21
  URDFをドラッグ&ドロップすると必要なメッシュが一覧表示されるので、「Select files」か「Select directory」でメッシュを渡してください。ファイル名を手がかりに`package://`のパスへ自動で突き合わせるので、メッシュの入ったフォルダごと指定して構いません。URDFとメッシュが同じフォルダにあるなら、フォルダごとドロップするだけで済みます。
20
22
 
23
+ ### EusLisp → URDF
24
+
25
+ `.l`をドロップすると、その場でURDFパッケージに変換し**3Dで表示**します。関節はスライダで動かせ、`package.xml` + `urdf/` + `meshes/`一式をzipでダウンロードできます。生成モデル(`euscollada`/`urdfeus`のロボット、jskeusの物体モデル)は幾何情報をファイル内に持っているので、メッシュを別途渡す必要はありません。
26
+
27
+ **GitHubのURLを貼るだけでも変換できます。** ファイルページのURL(`https://github.com/<owner>/<repo>/blob/<ref>/<path>.l`)を貼ると`raw.githubusercontent.com`に読み替えて取得します。リンクをドロップしても同じです。
28
+
29
+ `?url=`を付ければ、開いた時点で取得と変換まで走ります(URLはパーセントエンコードしてください)。モデルをそのまま共有できます:
30
+
31
+ ```
32
+ https://iory.github.io/urdfeus/convert/?url=https%3A%2F%2Fgithub.com%2Fowner%2Frepo%2Fblob%2Fmain%2Frobot.l
33
+ ```
34
+
35
+ 取得はブラウザからの直接アクセスなので、CORSを許可しているホストに限られます(GitHubのrawは許可しています)。それ以外のURLは失敗するので、ファイルを落としてドロップしてください。
36
+
37
+ `irteusgl`はネイティブプログラムなのでブラウザでは動きません。代わりに`.l`を直接読む[静的パーサ](#eus2urdfのバックエンド)を使います。コードでリンクを組み立てるモデルや、他ファイルを`load`するシーンはこの経路では読めないので、[インストール](#インストール)した`eus2urdf`(irteusgl経由)を使ってください。
38
+
21
39
  初回のみPython実行環境(約33MB)を取得するため数秒かかります。2回目以降はブラウザのキャッシュが効きます。
22
40
 
23
- ブラウザ版で使えない機能が2つあります。`eus2urdf`(EusLisp → URDF)はEusLisp処理系(`irteusgl`)の起動が必要なため動きません。`--voxel-size`によるメッシュ簡素化もopen3dに依存するため使えません。どちらも[インストール](#インストール)した`urdfeus`を使ってください。
41
+ `--voxel-size`によるメッシュ簡素化はopen3dに依存するため、ブラウザでは使えません。
24
42
 
25
43
  ## 概要
26
44
 
@@ -160,7 +178,7 @@ with open('robot.l', 'w') as f:
160
178
 
161
179
  ### 前提
162
180
 
163
- - `irteusgl`(jskeus)がインストールされていること
181
+ - `irteusgl`(jskeus)がインストールされていること(無い場合は後述の静的パーサが使われます)
164
182
  - メッシュ書き出しに`trimesh` / `pycollada`(依存に含まれます)
165
183
 
166
184
  ### コマンドライン
@@ -203,12 +221,66 @@ urdf_path = eus2urdf('robot.l', 'output_package_dir',
203
221
  - `--mesh-format`: `trimesh.export`が扱う拡張子(既定`glb`)。`glb`/`ply`/`obj`は面ごとの色を保持。`dae`はtrimeshのColladaエクスポータが色をtextureに潰すため**多色メッシュがグレーになる**(単色メッシュは保持)。`stl`は色なし
204
222
  - `--draco`: glbメッシュをDraco圧縮(`KHR_draco_mesh_compression`)。頂点色を保ったまま密なメッシュを概ね1桁小さくする。`glb`固定で`DracoPy`が必要(`pip install urdfeus[draco]`)。読み込み側のglTFローダにはDracoデコーダが要る
205
223
  - `--irteusgl`: 使用する`irteusgl`実行ファイル
224
+ - `--backend`: モデルの読み方(後述)
225
+
226
+ #### 慣性テンソルの扱い
227
+
228
+ EusLispの`:weight`と`:inertia-tensor`はそのままURDFの`<mass>`と`<inertia>`にします(`g`→`kg`、`g*mm^2`→`kg*m^2`)。ただしjskeusのモデルには、質量を持ちながら慣性テンソルが剛体として成立していないものがあります。
229
+
230
+ - h3 / h3s / h4 / h6 / h7 / igoid2 / igoid3 / saizo: `:inertia-tensor`がゼロ行列
231
+ - macra: 1e-40 相当の placeholder(数値誤差で固有値が負になるものもある)
232
+ - human: 長軸まわりだけ`1.0`(他の2軸は`2.07e8`、単位は`g*mm^2`)
233
+
234
+ 質量があって慣性が無い剛体は存在しないので、こういうリンクを渡されたインポータは、そのボディを拒否するか、質量から作った既定テンソルを黙って代入します。後者はロボットと何の関係もない値です。そこでurdfeusは、テンソルが正定値でない、主慣性モーメントが三角不等式を満たさない、メッシュの大きさと質量に対して小さすぎる、のいずれかなら、**そのリンクのメッシュから一様密度で慣性を計算し直します**。質量と重心は宣言された値のままです。
235
+
236
+ 書き換えたリンクは変換時に一覧で警告し、URDFにも理由をコメントで残します。
237
+
238
+ ```xml
239
+ <link name="root">
240
+ <!-- inertia recomputed by urdfeus: all-zero inertia tensor -->
241
+ <inertial>
242
+ ```
243
+
244
+ メッシュが無くて計算もできないリンクは、成立しないテンソルを書く代わりに`<inertial>`自体を出力しません(これも警告します)。
245
+
246
+ #### eus2urdfのバックエンド
247
+
248
+ モデルを読む経路は2つあります。
249
+
250
+ | `--backend` | 読み方 | 対応範囲 |
251
+ | --- | --- | --- |
252
+ | `irteusgl` | `irteusgl`でモデルを実体化してダンプ | どんなモデルでも読める。EusLispのインストールが必要 |
253
+ | `static` | `.l`ファイルを直接パースする(`urdfeus.eus_parse`) | 生成モデル限定。EusLispは不要 |
254
+ | `auto`(既定) | `irteusgl`がPATHにあればそちら、無ければ`static` | |
255
+
256
+ `static`が読めるのは**生成された**モデルです。`euscollada`/`urdfeus`がURDF・colladaから吐いたロボット(`gl::glvertices`で幾何を持つもの)と、jskeusの物体モデル(`faceset`で幾何を持つもの)が該当します。手書きでリンクを計算するモデルや、他ファイルを`load`するシーン(`*-scene.l`)は読めず、その旨のエラーになります(黙って一部だけ変換することはありません)。
257
+
258
+ `static`は`:assoc`や`:newcoords`といったEusLispの座標系の意味論を再現しており、jskeusの全モデル651個で`irteusgl`のダンプと突き合わせて検証しています(641個が完全一致、2個は下限が正のジョイントに関する差でURDFとしては等価、8個はシーンで対象外)。
259
+
260
+ ```bash
261
+ # EusLispが入っていない環境でも変換できる
262
+ eus2urdf robot.l out --backend static
263
+ ```
264
+
265
+ ```python
266
+ from urdfeus.eus_parse import parse_eus_model
267
+
268
+ data = parse_eus_model('robot.l') # irteusgl不要。dumpと同じ辞書が返る
269
+ ```
206
270
 
207
271
  #### ジオメトリの扱い
208
272
 
209
273
  - colladaボディは`glvertices`から、`make-cube`等で生成されたプレーンなbody(例:`:init`で追加される可視化用の脚キューブや吸盤)は各faceを三角形分割してメッシュ化します。いずれもメッシュとして書き出されます。
210
274
  - プレーンbodyのface三角形分割は凸面を仮定します(プリミティブ形状では成立)。
211
275
 
276
+ #### 往復(eus -> URDF -> eus)の検証
277
+
278
+ `tests/urdfeus_tests/test_roundtrip.py`が、モデルをEusLispからURDFへ、URDFからEusLispへ戻し、両端の`irteusgl`ダンプを突き合わせます。単位や規約のずれは片道だけ見ると妥当に見えてしまうので、戻してから比べます(`g*mm^2`と`kg*m^2`の1e9、度とラジアン、mmとm、軸の符号)。
279
+
280
+ `dump-robot`は全関節をゼロにしてからダンプするため、比較対象はゼロ姿勢での運動学パラメータ一式(リンク姿勢・関節軸・種類・可動範囲)です。これが一致すれば他の姿勢の順運動学も一致するので、関節角をランダムに振るテストは置いていません。
281
+
282
+ 往復で変わることを許しているのは2点だけです。名前は`:torso-waist-y`が`torso_waist_y`にサニタイズされるので、`eus2urdf`と同じ対応表で突き合わせます。そして剛体として成立しない慣性テンソルを持つリンクは、計算し直されたテンソルで戻ってきます(URDF内のコメントが付いているリンクを慣性の比較から外しています)。
283
+
212
284
  ### 生成されたEusLispファイルの使用
213
285
 
214
286
  ```lisp
@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
4
4
 
5
5
  [project]
6
6
  name = "urdfeus"
7
- version = "1.2.2"
7
+ version = "1.2.4"
8
8
  description = "URDF converter for Euslisp"
9
9
  requires-python = ">=3.10"
10
10
  dependencies = [
@@ -42,6 +42,14 @@ def main():
42
42
  parser.add_argument(
43
43
  "--irteusgl", type=str, default="irteusgl",
44
44
  help="irteusgl executable to use.")
45
+ parser.add_argument(
46
+ "--backend", choices=("auto", "static", "irteusgl"), default="auto",
47
+ help="How to read the model. 'irteusgl' instantiates it in EusLisp, "
48
+ + "which handles any model but needs EusLisp installed. 'static' "
49
+ + "parses the .l file directly, which needs no EusLisp but only "
50
+ + "understands generated models (euscollada/urdfeus robots and "
51
+ + "jskeus objects; not scenes). 'auto' (default) uses irteusgl when "
52
+ + "it is on PATH and parses the file otherwise.")
45
53
  add_diagnostic_arguments(parser)
46
54
  args = parser.parse_args()
47
55
  if handle_doctor(args, parser, ['input_euslisp_path', 'output_dir']):
@@ -56,6 +64,7 @@ def main():
56
64
  mesh_format=args.mesh_format,
57
65
  draco=args.draco,
58
66
  irteusgl=args.irteusgl,
67
+ backend=args.backend,
59
68
  )
60
69
  print(f"Wrote URDF: {urdf_path}")
61
70
 
@@ -19,6 +19,7 @@ import json
19
19
  import os
20
20
  import os.path as osp
21
21
  import re
22
+ import shutil
22
23
  import subprocess
23
24
  import tempfile
24
25
  import xml.etree.ElementTree as ET
@@ -142,6 +143,53 @@ def dump_eus_model(eus_path, constructor=None, irteusgl="irteusgl", timeout=600)
142
143
  return data
143
144
 
144
145
 
146
+ def load_eus_model(eus_path, constructor=None, irteusgl="irteusgl",
147
+ backend="auto", timeout=600):
148
+ """Return the dump of an EusLisp model, via irteusgl or by parsing it.
149
+
150
+ Parameters
151
+ ----------
152
+ eus_path : str
153
+ Path to the ``.l`` model file.
154
+ constructor : str or None
155
+ Constructor function (or class) name. Defaults to the file stem.
156
+ irteusgl : str
157
+ irteusgl executable, used by the ``irteusgl`` and ``auto`` backends.
158
+ backend : str
159
+ ``irteusgl`` instantiates the model in EusLisp, which handles any
160
+ model but needs EusLisp installed. ``static`` reads the file with
161
+ :func:`urdfeus.eus_parse.parse_eus_model`, which needs nothing but
162
+ only understands generated models. ``auto`` (the default) uses
163
+ irteusgl when it is on PATH and falls back to the static parser.
164
+ timeout : float
165
+ irteusgl subprocess timeout in seconds.
166
+
167
+ Returns
168
+ -------
169
+ dict
170
+ The dump described by ``euslisp/eus2urdf-dump.l``.
171
+ """
172
+ if backend not in ("auto", "static", "irteusgl"):
173
+ raise ValueError(
174
+ "backend must be 'auto', 'static' or 'irteusgl', not "
175
+ + repr(backend))
176
+ if backend == "static" or (
177
+ backend == "auto" and shutil.which(irteusgl) is None):
178
+ from urdfeus.eus_parse import EusParseError
179
+ from urdfeus.eus_parse import parse_eus_model
180
+ try:
181
+ return parse_eus_model(eus_path, constructor=constructor)
182
+ except EusParseError as e:
183
+ if backend == "static":
184
+ raise
185
+ raise RuntimeError(
186
+ f"'{irteusgl}' is not on PATH and {eus_path} cannot be read "
187
+ + "without it: " + str(e)
188
+ + "\nInstall EusLisp (jskeus) to convert this model.")
189
+ return dump_eus_model(eus_path, constructor=constructor,
190
+ irteusgl=irteusgl, timeout=timeout)
191
+
192
+
145
193
  def _mat3(rows):
146
194
  return np.array(rows, dtype=np.float64).reshape(3, 3)
147
195
 
@@ -348,6 +396,17 @@ def _classify_joint(joint, is_follower):
348
396
  linear = "linear" in jtype
349
397
  is_master = bool(joint["mimic"])
350
398
  min_v, max_v = joint["min"], joint["max"]
399
+
400
+ # A multi-DOF joint (6dof, sphere) keeps its limits as float-vectors, which
401
+ # the dump writes as null because they are not numbers. There is no 1-DOF
402
+ # URDF joint to map such a joint onto: a free one is URDF's "floating", and
403
+ # anything else -- including the immovable 6dof joint urdf2eus leaves at a
404
+ # model's root -- is honestly just a fixed connection.
405
+ if min_v is None or max_v is None:
406
+ if joint["movable"] and "6dof" in jtype:
407
+ return "floating"
408
+ return "fixed"
409
+
351
410
  inf = abs(min_v) >= _INF_LIMIT or abs(max_v) >= _INF_LIMIT
352
411
 
353
412
  # Fixed joints are emitted by urdf2eus as rotational-joints with 0/0
@@ -363,28 +422,169 @@ def _classify_joint(joint, is_follower):
363
422
  return "revolute"
364
423
 
365
424
 
366
- def _add_inertial(link_el, link):
425
+ #: Each principal moment is compared against ``mass * radius ** 2`` of the
426
+ #: link's own mesh, and anything below this fraction of that is a placeholder
427
+ #: rather than a mass distribution. A rod a thousand times longer than it is
428
+ #: thick, slender past anything a robot link is, still keeps its smallest
429
+ #: moment an order of magnitude above this; macra-robot declares ~1e-40 and
430
+ #: human-robot a single 1.0 g*mm^2 next to two moments of 2.07e8 g*mm^2, both
431
+ #: of which are several orders below it.
432
+ _INERTIA_FLOOR_FRACTION = 1e-7
433
+
434
+
435
+ def _inertia_defect(inertia, mass, mesh):
436
+ """Return why ``inertia`` cannot be a rigid body's tensor, else None.
437
+
438
+ A usable tensor is positive definite and its principal moments obey the
439
+ triangle inequality. Several EusLisp models declare ``:inertia-tensor`` as
440
+ a zero matrix (h3/h3s/h4/h6/h7, igoid2/igoid3, saizo) or placeholders
441
+ (macra) while still declaring a non-zero ``:weight``. Mass without inertia
442
+ is not a rigid body, so an importer handed it either refuses the body or
443
+ silently substitutes a preset tensor that has nothing to do with the
444
+ robot - both of which are worse than saying so here.
445
+
446
+ Parameters
447
+ ----------
448
+ inertia : numpy.ndarray or None
449
+ Candidate tensor in kg*m^2, or None when the link carries no
450
+ ``:inertia-tensor`` at all.
451
+ mass : float
452
+ Link mass in kg.
453
+ mesh : trimesh.Trimesh or None
454
+ The link's mesh in link-local metres. Used only for a length scale, so
455
+ the magnitude check is skipped when the link has no mesh.
456
+
457
+ Returns
458
+ -------
459
+ str or None
460
+ A one-line reason, or None when the tensor is usable as it stands.
461
+ """
462
+ if inertia is None:
463
+ return "no :inertia-tensor in the EusLisp model"
464
+ if not np.all(np.isfinite(inertia)):
465
+ return "inertia tensor holds a non-finite component"
466
+ if not np.any(inertia):
467
+ return "all-zero inertia tensor"
468
+ # Symmetric by construction (EusLisp stores the full 3x3), so the cheaper
469
+ # symmetric eigensolver is the right one and returns ascending moments.
470
+ moments = np.linalg.eigvalsh(0.5 * (inertia + inertia.T))
471
+ if moments[0] <= 0.0:
472
+ return ("inertia tensor is not positive definite (smallest principal"
473
+ + f" moment {moments[0]:.3g})")
474
+ small, mid, large = moments
475
+ if small + mid < large * (1.0 - 1e-9):
476
+ return ("principal moments violate the triangle inequality"
477
+ + f" ({small:.3g} + {mid:.3g} < {large:.3g})")
478
+ if mesh is not None:
479
+ radius = 0.5 * float(np.linalg.norm(mesh.bounding_box.extents))
480
+ floor = _INERTIA_FLOOR_FRACTION * mass * radius ** 2
481
+ if small < floor:
482
+ return (f"smallest principal moment {small:.3g} kg*m^2 is too"
483
+ + f" small for {mass:.3g} kg spread over"
484
+ + f" {2 * radius:.3g} m")
485
+ return None
486
+
487
+
488
+ def _inertia_from_mesh(mesh, mass):
489
+ """Inertia tensor of ``mesh`` at the uniform density that gives ``mass``.
490
+
491
+ The tensor is taken about the mesh's own centre of mass and in link-local
492
+ axes, which is the frame URDF's ``<inertia>`` is read in. Uniform density
493
+ is a guess, but it is a guess derived from the link's actual shape, unlike
494
+ the mass-scaled presets importers substitute when inertia is missing.
495
+
496
+ Parameters
497
+ ----------
498
+ mesh : trimesh.Trimesh
499
+ The link's mesh in link-local metres.
500
+ mass : float
501
+ Link mass in kg, taken from the EusLisp ``:weight``.
502
+
503
+ Returns
504
+ -------
505
+ tuple of (numpy.ndarray, str) or None
506
+ The 3x3 tensor in kg*m^2 and the geometry it came from, or None when
507
+ the mesh encloses no usable volume.
508
+
509
+ Notes
510
+ -----
511
+ glvertices meshes are rarely watertight (they are render geometry, often
512
+ open shells), and the divergence-theorem volume integral trimesh uses is
513
+ only defined on a closed surface. So the raw mesh is used when it is
514
+ watertight and its convex hull, which is closed by construction, when it
515
+ is not.
516
+ """
517
+ if mesh.is_watertight and mesh.volume > 0.0:
518
+ body, source = mesh.copy(), "mesh"
519
+ else:
520
+ body, source = mesh.convex_hull.copy(), "convex hull of the mesh"
521
+ if not body.is_watertight or body.volume <= 0.0:
522
+ return None
523
+ # density, not mass, is what trimesh scales the integral by; mass/volume
524
+ # makes the resulting tensor match the declared mass exactly.
525
+ body.density = mass / body.volume
526
+ inertia = np.asarray(body.moment_inertia, dtype=np.float64)
527
+ inertia = 0.5 * (inertia + inertia.T)
528
+ if not np.all(np.isfinite(inertia)) or np.linalg.eigvalsh(inertia)[0] <= 0:
529
+ return None
530
+ return inertia, source
531
+
532
+
533
+ def _add_inertial(link_el, link, mesh=None):
534
+ """Write ``<inertial>`` for one link, repairing an unusable tensor.
535
+
536
+ Parameters
537
+ ----------
538
+ link_el : xml.etree.ElementTree.Element
539
+ The ``<link>`` element to append to.
540
+ link : dict
541
+ One entry of the dump's ``links``, in EusLisp units (g, mm, g*mm^2).
542
+ mesh : trimesh.Trimesh or None
543
+ The link's mesh in link-local metres, used to recompute the tensor
544
+ when the declared one cannot be used.
545
+
546
+ Returns
547
+ -------
548
+ str or None
549
+ A note naming the link and what was wrong, for the caller to report,
550
+ or None when the declared inertia was written unchanged.
551
+ """
367
552
  weight = link["weight"]
368
553
  if weight is None or weight <= 0.0:
369
- return
554
+ return None
555
+ mass = weight / 1000.0 # g -> kg
556
+ it = link["inertia"]
557
+ inertia = (_mat3(it) / 1e9) if it is not None else None # g*mm^2 -> kg*m^2
558
+
559
+ note = None
560
+ defect = _inertia_defect(inertia, mass, mesh)
561
+ if defect is not None:
562
+ computed = _inertia_from_mesh(mesh, mass) if mesh is not None else None
563
+ if computed is None:
564
+ # Writing mass with no usable inertia is what breaks importers, and
565
+ # there is nothing here to compute one from, so the link is left
566
+ # without <inertial> rather than with an impossible one.
567
+ return (f"{link['name']}: {defect}, and no mesh to compute one"
568
+ + " from, so no <inertial> was written")
569
+ inertia, source = computed
570
+ note = (f"{link['name']}: {defect}; recomputed from the {source}"
571
+ + " at uniform density")
572
+ link_el.append(ET.Comment(
573
+ f" inertia recomputed by urdfeus: {defect} "))
574
+
370
575
  inertial = ET.SubElement(link_el, "inertial")
371
576
  centroid = link["centroid"]
372
577
  xyz = (np.array(centroid) / meter2millimeter) if centroid is not None \
373
578
  else np.zeros(3)
374
579
  ET.SubElement(inertial, "origin", xyz=_fmt_vec(xyz), rpy="0 0 0")
375
- ET.SubElement(inertial, "mass", value=f"{weight / 1000.0:.8g}") # g -> kg
376
- it = link["inertia"]
377
- if it is not None:
378
- # g*mm^2 -> kg*m^2
379
- i = _mat3(it) / 1e9
380
- ET.SubElement(
381
- inertial, "inertia",
382
- ixx=f"{i[0, 0]:.8g}", ixy=f"{i[0, 1]:.8g}", ixz=f"{i[0, 2]:.8g}",
383
- iyy=f"{i[1, 1]:.8g}", iyz=f"{i[1, 2]:.8g}", izz=f"{i[2, 2]:.8g}",
384
- )
385
- else:
386
- ET.SubElement(inertial, "inertia",
387
- ixx="0", ixy="0", ixz="0", iyy="0", iyz="0", izz="0")
580
+ ET.SubElement(inertial, "mass", value=f"{mass:.8g}")
581
+ i = inertia
582
+ ET.SubElement(
583
+ inertial, "inertia",
584
+ ixx=f"{i[0, 0]:.8g}", ixy=f"{i[0, 1]:.8g}", ixz=f"{i[0, 2]:.8g}",
585
+ iyy=f"{i[1, 1]:.8g}", iyz=f"{i[1, 2]:.8g}", izz=f"{i[2, 2]:.8g}",
586
+ )
587
+ return note
388
588
 
389
589
 
390
590
  def _package_xml(package_name):
@@ -433,6 +633,7 @@ def eus2urdf(
433
633
  mesh_format="glb",
434
634
  draco=False,
435
635
  irteusgl="irteusgl",
636
+ backend="auto",
436
637
  ):
437
638
  """Convert an EusLisp model to a URDF ROS package.
438
639
 
@@ -463,13 +664,17 @@ def eus2urdf(
463
664
  to read the result.
464
665
  irteusgl : str
465
666
  irteusgl executable.
667
+ backend : str
668
+ How to read the model: ``irteusgl``, ``static`` (parse the file, no
669
+ EusLisp needed) or ``auto``. See :func:`load_eus_model`.
466
670
 
467
671
  Returns
468
672
  -------
469
673
  str
470
674
  Path to the written ``.urdf`` file.
471
675
  """
472
- data = dump_eus_model(eus_path, constructor=constructor, irteusgl=irteusgl)
676
+ data = load_eus_model(eus_path, constructor=constructor,
677
+ irteusgl=irteusgl, backend=backend)
473
678
  return eus2urdf_from_data(
474
679
  data,
475
680
  output_dir,
@@ -567,13 +772,18 @@ def eus2urdf_from_data(
567
772
 
568
773
  # Links (+ meshes).
569
774
  used_fnames = set()
775
+ inertia_notes = []
570
776
  for link in data["links"]:
571
777
  name = link["name"]
572
778
  link_el = ET.SubElement(robot_el, "link", name=link_names[name])
573
- _add_inertial(link_el, link)
574
779
 
780
+ # The mesh is built before <inertial> because it is what an unusable
781
+ # declared inertia tensor is recomputed from.
575
782
  pos, rot = link_pose[name]
576
783
  mesh = _build_link_mesh(link, pos, rot)
784
+ note = _add_inertial(link_el, link, mesh)
785
+ if note is not None:
786
+ inertia_notes.append(note)
577
787
  if mesh is not None:
578
788
  # _safe_name can collapse distinct link names to the same stem;
579
789
  # disambiguate so meshes never overwrite each other.
@@ -595,6 +805,12 @@ def eus2urdf_from_data(
595
805
  geom = ET.SubElement(el, "geometry")
596
806
  ET.SubElement(geom, "mesh", filename=uri)
597
807
 
808
+ if inertia_notes:
809
+ print(f"Warning: {len(inertia_notes)} link(s) declare a mass the"
810
+ + " EusLisp inertia tensor cannot go with:")
811
+ for note in inertia_notes:
812
+ print(f" {note}")
813
+
598
814
  # Joints. ``joint_unames`` already gives each joint a unique sanitized name
599
815
  # (handling both invalid characters and models that reuse a name across
600
816
  # parts, e.g. a hand reusing :j10/:j11 on every finger).
@@ -679,6 +895,12 @@ def movable_joint_spec(data):
679
895
  if not joint["movable"]:
680
896
  continue
681
897
  jtype = _classify_joint(joint, joint["name"] in follower)
898
+ if jtype not in ("revolute", "continuous", "prismatic"):
899
+ # A multi-DOF joint carries a vector, which no single scale factor
900
+ # describes and no URDF joint here can be driven by. EusLisp's
901
+ # eus2urdf-movable-joints leaves the same ones out, which is what
902
+ # keeps the two lists index-aligned.
903
+ continue
682
904
  scale = (1.0 / meter2millimeter) if jtype == "prismatic" \
683
905
  else float(np.pi / 180.0)
684
906
  spec.append((joint_unames[i], scale))