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.
- {urdfeus-1.2.2/urdfeus.egg-info → urdfeus-1.2.4}/PKG-INFO +77 -5
- {urdfeus-1.2.2 → urdfeus-1.2.4}/README.md +76 -4
- {urdfeus-1.2.2 → urdfeus-1.2.4}/pyproject.toml +1 -1
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/eus2urdf.py +9 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/eus2urdf.py +239 -17
- urdfeus-1.2.4/urdfeus/eus_colors.py +950 -0
- urdfeus-1.2.4/urdfeus/eus_parse.py +2345 -0
- urdfeus-1.2.4/urdfeus/eus_reader.py +281 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/euslisp/eus2urdf-dump.l +31 -6
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/urdf2eus.py +47 -47
- {urdfeus-1.2.2 → urdfeus-1.2.4/urdfeus.egg-info}/PKG-INFO +77 -5
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus.egg-info/SOURCES.txt +3 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/LICENSE +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/setup.cfg +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/__init__.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/__init__.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/common.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/mesh2eus.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/apps/urdf2eus.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/common.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/compatibility.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/grouping_joint.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/mesh2eus.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/mesh_cache.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/provenance.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/read_yaml.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/templates/__init__.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/templates/euscollada-robot.l +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/templates/urdf_template.py +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus/templates/urdfeus-glvertices.l +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus.egg-info/dependency_links.txt +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus.egg-info/entry_points.txt +0 -0
- {urdfeus-1.2.2 → urdfeus-1.2.4}/urdfeus.egg-info/requires.txt +0 -0
- {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.
|
|
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
|
-
|
|
62
|
+
手元のモデルをその場で相互変換できます → **<https://iory.github.io/urdfeus/convert/>**
|
|
63
63
|
|
|
64
|
-
`
|
|
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
|
-
|
|
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
|
-
|
|
15
|
+
手元のモデルをその場で相互変換できます → **<https://iory.github.io/urdfeus/convert/>**
|
|
16
16
|
|
|
17
|
-
`
|
|
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
|
-
|
|
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
|
|
@@ -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
|
-
|
|
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"{
|
|
376
|
-
|
|
377
|
-
|
|
378
|
-
|
|
379
|
-
i =
|
|
380
|
-
|
|
381
|
-
|
|
382
|
-
|
|
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 =
|
|
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))
|