Первоначальная настройка

2026-09-13 13:56:29 +03:00
commit 397e33bd08
46 changed files with 13809 additions and 0 deletions
@@ -0,0 +1,8 @@
+.venv/
+__pycache__/
+.pytest_cache/
+outputs/
+.cache/
+.codex/
+.agents/
+iiwa_description
@@ -0,0 +1,103 @@
+# KUKA iiwa в Genesis World
+
+Standalone Python-приложение: стол, iiwa7 и одна или две RGB-D камеры.
+ROS 2 не используется. Рабочие описания — обычные URDF с локальными meshes.
+
+## Установка и запуск
+
+Все Python-зависимости устанавливает uv:
+
+```bash
+uv sync --locked
+uv run python run_scene.py --config configs/scene.yaml
+```
+
+Для машины без NVIDIA можно выбрать CPU-сборку PyTorch. Сохраняйте `--extra cpu`
+и в следующих вызовах `uv run`, чтобы uv не переключил окружение обратно:
+
+```bash
+uv sync --extra cpu --locked
+uv run --extra cpu python run_scene.py --headless --steps 4 --save-frames
+uv run --extra cpu pytest
+```
+
+В `configs/scene.yaml` переключите `scene.mode` между `without_gripper` и
+`with_gripper`. Второй режим автоматически выбирает URDF с инструментом и
+добавляет камеру на нём. Верхняя камера присутствует всегда. Отдельно менять
+путь URDF или `enabled` не требуется; выбор режима имеет приоритет над этими
+устаревшими полями из первоначального плана.
+
+```bash
+uv run python run_scene.py --viewer
+uv run python run_scene.py --headless --steps 120 --save-frames
+uv run --extra cuda python run_scene.py --backend gpu --viewer
+uv run pytest
+IIWA_GENESIS_SMOKE=1 uv run pytest tests/test_genesis_smoke.py
+```
+
+Headless по умолчанию выполняет 120 шагов, viewer работает до закрытия окна,
+Ctrl+C или Q/Esc в окне кадров. `output.preview` включает окна RGB и глубины.
+30 FPS — частота по времени симуляции; реальная скорость зависит от оборудования.
+Headless всё равно требует рабочий OpenGL/EGL для RGB. Используется EGL, если
+`PYOPENGL_PLATFORM` не задан пользователем. GPU требует совместимого драйвера;
+`backend: cpu` выбран для переносимого первого запуска. Системный драйвер и
+библиотеки OpenGL предоставляются ОС, их uv не устанавливает.
+Обычные тесты проверяют URDF, конфигурацию и математику; opt-in smoke-тесты
+реально строят оба мира и сохраняют RGB-D на CPU/EGL. Первичная компиляция
+Genesis может занимать несколько минут. Launcher игнорирует унаследованные
+Python overlays, а pytest не загружает внешние плагины автоматически.
+
+## Конфигурация и координаты
+
+Положения задаются в метрах, углы `euler` — в градусах, суставы и
+`horizontal_fov` — в радианах. Начальные суставы проверяются по URDF limits.
+Верх столешницы находится на 0,800 м, верх монтажной пластины — на 0,815 м;
+поэтому основание iiwa по умолчанию расположено на `[-0.263, 0, 0.815]`.
+При перемещении стола настройте положение робота согласованно.
+
+Параметры верхней камеры заданы в мировой системе. Параметры камеры инструмента
+заданы относительно `camera_optical_frame`: вперёд +Z, вверх изображения -Y.
+Трансформы крепления сохранены из исходного описания. Направление оптической
+оси можно уточнять через локальные `lookat` и `up` без правки URDF.
+Нулевая гравитация удерживает выбранную позу без контроллера: v1 предназначена
+для осмотра сцены и получения изображений, а не динамического управления.
+
+## Камеры и запись
+
+Это **приближение D455**, использующее профиль из согласованного плана:
+640×480, 30 FPS, HFOV 75°, диапазон 0,01–6 м, квантование глубины 1 мм.
+RGB и depth используют одинаковую идеальную pinhole-калибровку. Не моделируются
+ИК-проектор, стереосопоставление, IMU, реальные искажения и индивидуальная заводская
+калибровка D455. Исходный Xacro задавал другую нижнюю границу depth — 0,52 м;
+выбранные 0,01 м следуют утверждённому плану и изменяются через YAML.
+
+Каждая камера имеет RGB-сенсор Genesis и DepthCamera raycaster. Дальность по
+лучу преобразуется в optical Z. Пропуски и значения вне диапазона дают depth=0
+и valid=false. Depth использует геометрию raycaster; упрощения мешей/коллизий
+могут отличаться от RGB. Gaussian noise задаётся отдельно для RGB (доля 255)
+и глубины (метры), выключен по умолчанию и использует фиксированный seed.
+
+`--save-frames` или `output.save_frames` сохраняет в уникальную папку запуска:
+
+- `overhead_000000.png` — RGB;
+- `overhead_000000.npz` — float32 depth в метрах и маска valid;
+- `overhead_000000.json` — intrinsics, обе extrinsics, время симуляции и профиль.
+
+Камера инструмента использует префикс `tool`. При `output.pointcloud: true`
+NPZ содержит также `xyz_optical`; невалидные точки равны NaN. Центры пикселей
+располагаются в `(u+0.5, v+0.5)`, оси optical: X вправо, Y вниз, Z вперёд.
+Относительный `output.directory` разрешается относительно YAML-файла.
+Каждый запуск создаёт отдельный каталог, существующие записи сохраняются.
+
+Genesis закреплён на 1.4.1, версии остальных зависимостей фиксирует `uv.lock`.
+Xacro явно исключён из транзитивных зависимостей через `tool.uv.exclude-dependencies`.
+Genesis 1.4.1, однако, импортирует его безусловно. Модуль приложения `runtime.py`
+при импорте Genesis убирает этот единственный импорт из AST модуля загрузки URDF
+и заменяет `load_xacro` явной ошибкой. Это адаптация в памяти, ограниченная
+версией 1.4.1: файлы установленного Genesis не меняются, заглушка Xacro не создаётся.
+Обновление Genesis требует пересмотра адаптации и повторного smoke-теста.
+Экспорт описаний также выполнен без Xacro; runtime читает только plain URDF.
+
+Документация: [Genesis cameras](https://genesis-world.readthedocs.io/en/latest/user_guide/sensing/camera_sensors.html),
+[raycaster](https://genesis-world.readthedocs.io/en/latest/user_guide/sensing/raycaster.html),
+[uv exclusions](https://docs.astral.sh/uv/concepts/resolution/#dependency-exclusions).
@@ -0,0 +1,60 @@
+scene:
+  mode: without_gripper # with_gripper automatically adds the tool camera
+  backend: cpu # gpu on a machine with a supported GPU
+  viewer: true
+  dt: 0.008333333333333333 # 120 Hz simulation clock
+  seed: 42
+
+robot:
+  position: [-0.263, 0.0, 0.815]
+  euler: [0.0, 0.0, 0.0] # degrees
+  joints: # radians
+    joint1: 0.0
+    joint2: 0.0
+    joint3: 0.0
+    joint4: 0.0
+    joint5: 0.0
+    joint6: 0.0
+    joint7: 0.0
+
+table:
+  position: [0.0, 0.0, 0.0]
+  euler: [0.0, 0.0, 0.0]
+
+cameras:
+  overhead:
+    position: [-0.263, 0.0, 2.8]
+    lookat: [-0.263, 0.0, 1.2]
+    up: [0.0, 1.0, 0.0]
+    resolution: [640, 480]
+    fps: 30
+    horizontal_fov: 1.3089969 # radians, legacy simulated profile
+    min_range: 0.01
+    max_range: 6.0
+    depth_resolution: 0.001
+    noise:
+      enabled: false
+      rgb_stddev: 0.005 # fraction of full RGB scale
+      depth_stddev: 0.02 # metres
+  tool:
+    link: camera_optical_frame
+    # Local camera axes in the existing link: forward +Z, image up -Y.
+    position: [0.0, 0.0, 0.0]
+    lookat: [0.0, 0.0, 1.0]
+    up: [0.0, -1.0, 0.0]
+    resolution: [640, 480]
+    fps: 30
+    horizontal_fov: 1.3089969
+    min_range: 0.01
+    max_range: 6.0
+    depth_resolution: 0.001
+    noise:
+      enabled: false
+      rgb_stddev: 0.005
+      depth_stddev: 0.02
+
+output:
+  save_frames: false
+  directory: ../outputs # relative to this YAML
+  preview: true # RGB and colour-mapped depth windows when viewer is enabled
+  pointcloud: false # optional optical-frame XYZ in each NPZ
@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
@@ -0,0 +1,17 @@
+# Genesis descriptions
+
+`urdf/iiwa_without_gripper.urdf` и `urdf/iiwa_with_gripper.urdf` экспортированы
+из `iiwa_description` с сохранением кинематики, инерций, суставных ограничений
+и геометрии. `table/table.urdf` — копия стола с локальными путями.
+Meshes находятся в `urdf/meshes`; исходная папка не изменена.
+
+Воспроизводимый экспорт именно этих исходных макросов:
+
+```bash
+uv run python tools/export_descriptions.py
+```
+
+Скрипт использует стандартную библиотеку Python и не является универсальным
+Xacro-интерпретатором. Gazebo/Webots/ros2_control не попадают в выходные URDF.
+Настройки сенсоров находятся в YAML. Все fixed links сохранены при импорте.
+Лицензия исходных геометрических ресурсов находится в `LICENSE`.
@@ -0,0 +1,169 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot name="robo_table">
+  <link name="world" />
+  <link name="table_frame">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <mass value="30" />
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <mesh filename="../urdf/meshes/table/TableFrame.obj" />
+      </geometry>
+      <material name="table_metal">
+        <color rgba="0.57 0.57 0.57 1.0" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.395" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="1.28 1.08 0.79" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="wood_palete">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <mass value="10.0" />
+      <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.5" />
+    </inertial>
+    <visual name="top_surface">
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="1.28 1.08 0.02" />
+      </geometry>
+      <material name="table_top">
+        <color rgba="0.92 0.92 0.92 1.0" />
+      </material>
+    </visual>
+    <visual name="border_front">
+      <origin xyz="0.0 0.53 0.005" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="1.28 0.02 0.014" />
+      </geometry>
+      <material name="orange_border">
+        <color rgba="1.0 0.38 0.0 1.0" />
+      </material>
+    </visual>
+    <visual name="border_back">
+      <origin xyz="0.0 -0.53 0.005" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="1.28 0.02 0.014" />
+      </geometry>
+      <material name="orange_border">
+        <color rgba="1.0 0.38 0.0 1.0" />
+      </material>
+    </visual>
+    <visual name="border_left">
+      <origin xyz="0.63 0.0 0.005" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="0.02 1.04 0.014" />
+      </geometry>
+      <material name="orange_border">
+        <color rgba="1.0 0.38 0.0 1.0" />
+      </material>
+    </visual>
+    <visual name="border_right">
+      <origin xyz="-0.63 0.0 0.005" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="0.02 1.04 0.014" />
+      </geometry>
+      <material name="orange_border">
+        <color rgba="1.0 0.38 0.0 1.0" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="1.28 1.08 0.02" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="robot_palete">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <mass value="3" />
+      <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <mesh filename="../urdf/meshes/table/PlateUnderRobot.obj" />
+      </geometry>
+      <material name="table_metal">
+        <color rgba="0.57 0.57 0.57 1.0" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="0.47 0.4 0.01" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="cabinet_frame">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <mass value="5.0" />
+      <inertia ixx="0.2" ixy="0.0" ixz="0.0" iyy="0.2" iyz="0.0" izz="0.2" />
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="1.57 0.0 1.57" />
+      <geometry>
+        <mesh filename="../urdf/meshes/table/CabinetFrame.obj" />
+      </geometry>
+      <material name="table_metal">
+        <color rgba="0.57 0.57 0.57 1.0" />
+      </material>
+    </visual>
+  </link>
+  <link name="cabinet">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <mass value="15.0" />
+      <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.5" />
+    </inertial>
+    <visual>
+      <origin xyz="0.0 0.31 0.0" rpy="1.57 0.0 -1.57" />
+      <geometry>
+        <mesh filename="../urdf/meshes/table/ElectricCabinet.obj" />
+      </geometry>
+      <material name="cabinet_dark">
+        <color rgba="0.2 0.2 0.22 1.0" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.12 0 0.4" rpy="1.57 0.0 1.57" />
+      <geometry>
+        <box size="0.65 0.83 0.32" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="world_frame" type="fixed">
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+    <parent link="world" />
+    <child link="table_frame" />
+  </joint>
+  <joint name="frame_palete" type="fixed">
+    <origin xyz="0.0 0.0 0.79" rpy="0.0 0.0 0.0" />
+    <parent link="table_frame" />
+    <child link="wood_palete" />
+  </joint>
+  <joint name="paletes" type="fixed">
+    <origin xyz="-0.263 0.0 0.02" rpy="0.0 0.0 0.0" />
+    <parent link="wood_palete" />
+    <child link="robot_palete" />
+  </joint>
+  <joint name="frames" type="fixed">
+    <origin xyz="-0.66 0.0 0.09" rpy="0.0 0.0 0.0" />
+    <parent link="table_frame" />
+    <child link="cabinet_frame" />
+  </joint>
+  <joint name="cabinets" type="fixed">
+    <origin xyz="-0.022 0 0.2" rpy="0.0 0.0 0.0" />
+    <parent link="cabinet_frame" />
+    <child link="cabinet" />
+  </joint>
+</robot>
@@ -0,0 +1,368 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot name="iiwa7">
+  <link name="world" />
+  <link name="base_link">
+    <inertial>
+      <origin xyz="-0.012856764088336 7.05371084939836E-07 0.0679655592591469" rpy="0 0 0" />
+      <mass value="4.86" />
+      <inertia ixx="0.111239565538359" ixy="-3.81097437253422E-07" ixz="0.00486659750522703" iyy="0.139017104291389" iyz="7.87871531503135E-07" izz="0.133028621926226" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/base_link.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link1">
+    <inertial>
+      <origin xyz="-4.78790441849064E-07 -0.0348192058752328 -0.0692021315991948" rpy="0 0 0" />
+      <mass value="4" />
+      <inertia ixx="0.133321610552378" ixy="-5.3830316300325E-07" ixz="9.16989471201082E-07" iyy="0.127213841326705" iyz="0.0236704924620788" izz="0.047155320135412" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link1.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link2">
+    <inertial>
+      <origin xyz="-5.2906167189587E-07 -0.0869460684741495 0.0287906957294968" rpy="0 0 0" />
+      <mass value="4" />
+      <inertia ixx="0.197631241693207" ixy="9.50708464039702E-08" ixz="-3.51963818841194E-07" iyy="0.0602720613821397" iyz="-0.0367196702125505" izz="0.187557269753206" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link2.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link3">
+    <inertial>
+      <origin xyz="8.94264666317923E-07 0.0343188244798595 -0.0692020521907963" rpy="0 0 0" />
+      <mass value="3" />
+      <inertia ixx="0.133322417523041" ixy="-2.72387851582384E-07" ixz="-2.31579646685682E-07" iyy="0.127213838333813" iyz="-0.0236710393468367" izz="0.0471557500559207" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link3.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link4">
+    <inertial>
+      <origin xyz="9.9343785475714E-08 -0.0869462342636022 -0.0292907044657978" rpy="0 0 0" />
+      <mass value="2.7" />
+      <inertia ixx="0.197630839812683" ixy="-3.9082887807781E-07" ixz="-3.24564289064112E-07" iyy="0.0602723071447973" iyz="0.0367195588674331" izz="0.187556898167347" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link4.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link5">
+    <inertial>
+      <origin xyz="-1.5059645121912E-07 -0.0298223596156005 -0.116233672352317" rpy="0 0 0" />
+      <mass value="1.7" />
+      <inertia ixx="0.0529038960536523" ixy="6.86274869084724E-08" ixz="3.57472026918582E-07" iyy="0.0444942713885967" iyz="0.0174914068345989" izz="0.0239916848880951" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link5.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link6">
+    <inertial>
+      <origin xyz="3.09820422451923E-06 -0.000588422963829549 0.000348569863276505" rpy="0 0 0" />
+      <mass value="1.8" />
+      <inertia ixx="0.0384515762635731" ixy="-6.70279497724119E-08" ixz="2.19173188415509E-06" iyy="0.0280010940276735" iyz="-0.00192249677468735" izz="0.0368124806983828" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link6.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link7">
+    <inertial>
+      <origin xyz="5.28878934317328E-07 3.37199922245852E-07 -0.0274928177649973" rpy="0 0 0" />
+      <mass value="0.3" />
+      <inertia ixx="0.00177365937026673" ixy="-1.2410521430667E-07" ixz="-2.95409038849194E-08" iyy="0.00177328722902027" iyz="-1.57567641084958E-08" izz="0.00272748710623445" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link7.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_ee" />
+  <joint name="world_base_joint" type="fixed">
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+    <parent link="world" />
+    <child link="base_link" />
+  </joint>
+  <joint name="joint1" type="revolute">
+    <origin xyz="0 0 0.3375" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="link1" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.97" upper="2.97" effort="200" velocity="1.71" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.8827" soft_upper_limit="2.8827" k_velocity="10" />
+  </joint>
+  <joint name="joint2" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="link1" />
+    <child link="link2" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.09" upper="2.09" effort="200" velocity="1.71" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.0027" soft_upper_limit="2.0027" k_velocity="10" />
+  </joint>
+  <joint name="joint3" type="revolute">
+    <origin xyz="0 -0.3993 0" rpy="1.5708 0 0" />
+    <parent link="link2" />
+    <child link="link3" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.97" upper="2.97" effort="200" velocity="1.75" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.8827" soft_upper_limit="2.8827" k_velocity="10" />
+  </joint>
+  <joint name="joint4" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="link3" />
+    <child link="link4" />
+    <axis xyz="0 0 -1" />
+    <limit lower="-2.09" upper="2.09" effort="200" velocity="2.27" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.0027" soft_upper_limit="2.0027" k_velocity="10" />
+  </joint>
+  <joint name="joint5" type="revolute">
+    <origin xyz="0 -0.3993 0" rpy="1.5708 0 0" />
+    <parent link="link4" />
+    <child link="link5" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.97" upper="2.97" effort="200" velocity="2.44" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.8827" soft_upper_limit="2.8827" k_velocity="10" />
+  </joint>
+  <joint name="joint6" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="link5" />
+    <child link="link6" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.09" upper="2.09" effort="200" velocity="3.14" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.0027" soft_upper_limit="2.0027" k_velocity="10" />
+  </joint>
+  <joint name="joint7" type="revolute">
+    <origin xyz="0 -0.126 0" rpy="1.5708 0 0" />
+    <parent link="link6" />
+    <child link="link7" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.05" upper="3.05" effort="200" velocity="3.14" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.9627" soft_upper_limit="2.9627" k_velocity="10" />
+  </joint>
+  <joint name="tools_joint" type="fixed">
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+    <parent link="link7" />
+    <child link="link_ee" />
+  </joint>
+  <link name="camera_holder">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <mass value="0.15" />
+      <inertia ixx="0.000073" ixy="0.0" ixz="0.0" iyy="0.000136" iyz="0.0" izz="0.000186" />
+    </inertial>
+    <visual name="">
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <mesh filename="meshes/tools/CameraHolder.dae" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.015" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <cylinder radius="0.035" length="0.03" />
+      </geometry>
+    </collision>
+    <collision>
+      <origin xyz="0.0 0.08 0.015" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="0.03 0.1 0.03" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="patron">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <mass value="0.35" />
+      <inertia ixx="0.000127" ixy="0.0" ixz="0.0" iyy="0.000127" iyz="0.0" izz="0.000077" />
+    </inertial>
+    <visual name="">
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <mesh filename="meshes/tools/patron.dae" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.0 0.027" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <cylinder radius="0.021" length="0.055" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="camera_corner">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <mass value="0.1" />
+      <inertia ixx="0.0000094" ixy="0.0" ixz="0.0" iyy="0.000131" iyz="0.0" izz="0.000135" />
+    </inertial>
+    <visual name="">
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <mesh filename="meshes/tools/camera_corner.dae" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0.116 0.022" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="0.124 0.029 0.017" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="camera_hand">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <mass value="0.2" />
+      <inertia ixx="0.000021" ixy="0.0" ixz="0.0" iyy="0.000250" iyz="0.0" izz="0.000250" />
+    </inertial>
+    <visual>
+      <origin xyz="-0.06 0.0 0.0" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <mesh filename="meshes/tools/cameraD455.dae" />
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.0 0.014 0.0125" rpy="0.0 0.0 0.0" />
+      <geometry>
+        <box size="0.12 0.025 0.025" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="camera_optical_frame" />
+  <link name="tcp" />
+  <joint name="tool" type="fixed">
+    <parent link="link_ee" />
+    <child link="camera_holder" />
+    <origin xyz="0 0 0" rpy="0 0 1.5708" />
+  </joint>
+  <joint name="camera_holder_patron" type="fixed">
+    <origin xyz="0.0 0.0 0.032" rpy="0.0 0.0 0.0" />
+    <parent link="camera_holder" />
+    <child link="patron" />
+  </joint>
+  <joint name="patron_tcp" type="fixed">
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5708" />
+    <parent link="patron" />
+    <child link="tcp" />
+  </joint>
+  <joint name="camera_holder_corner" type="fixed">
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+    <parent link="camera_holder" />
+    <child link="camera_corner" />
+  </joint>
+  <joint name="camera_corner_camera" type="fixed">
+    <origin xyz="0.0 0.1 0.014" rpy="0.61 0.0 0.0" />
+    <parent link="camera_corner" />
+    <child link="camera_hand" />
+  </joint>
+  <joint name="camera_hand_to_optical" type="fixed">
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+    <parent link="camera_hand" />
+    <child link="camera_optical_frame" />
+  </joint>
+</robot>
@@ -0,0 +1,254 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot name="iiwa7">
+  <link name="world" />
+  <link name="base_link">
+    <inertial>
+      <origin xyz="-0.012856764088336 7.05371084939836E-07 0.0679655592591469" rpy="0 0 0" />
+      <mass value="4.86" />
+      <inertia ixx="0.111239565538359" ixy="-3.81097437253422E-07" ixz="0.00486659750522703" iyy="0.139017104291389" iyz="7.87871531503135E-07" izz="0.133028621926226" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/base_link.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link1">
+    <inertial>
+      <origin xyz="-4.78790441849064E-07 -0.0348192058752328 -0.0692021315991948" rpy="0 0 0" />
+      <mass value="4" />
+      <inertia ixx="0.133321610552378" ixy="-5.3830316300325E-07" ixz="9.16989471201082E-07" iyy="0.127213841326705" iyz="0.0236704924620788" izz="0.047155320135412" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link1.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link2">
+    <inertial>
+      <origin xyz="-5.2906167189587E-07 -0.0869460684741495 0.0287906957294968" rpy="0 0 0" />
+      <mass value="4" />
+      <inertia ixx="0.197631241693207" ixy="9.50708464039702E-08" ixz="-3.51963818841194E-07" iyy="0.0602720613821397" iyz="-0.0367196702125505" izz="0.187557269753206" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link2.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link3">
+    <inertial>
+      <origin xyz="8.94264666317923E-07 0.0343188244798595 -0.0692020521907963" rpy="0 0 0" />
+      <mass value="3" />
+      <inertia ixx="0.133322417523041" ixy="-2.72387851582384E-07" ixz="-2.31579646685682E-07" iyy="0.127213838333813" iyz="-0.0236710393468367" izz="0.0471557500559207" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link3.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link4">
+    <inertial>
+      <origin xyz="9.9343785475714E-08 -0.0869462342636022 -0.0292907044657978" rpy="0 0 0" />
+      <mass value="2.7" />
+      <inertia ixx="0.197630839812683" ixy="-3.9082887807781E-07" ixz="-3.24564289064112E-07" iyy="0.0602723071447973" iyz="0.0367195588674331" izz="0.187556898167347" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link4.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link5">
+    <inertial>
+      <origin xyz="-1.5059645121912E-07 -0.0298223596156005 -0.116233672352317" rpy="0 0 0" />
+      <mass value="1.7" />
+      <inertia ixx="0.0529038960536523" ixy="6.86274869084724E-08" ixz="3.57472026918582E-07" iyy="0.0444942713885967" iyz="0.0174914068345989" izz="0.0239916848880951" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link5.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link6">
+    <inertial>
+      <origin xyz="3.09820422451923E-06 -0.000588422963829549 0.000348569863276505" rpy="0 0 0" />
+      <mass value="1.8" />
+      <inertia ixx="0.0384515762635731" ixy="-6.70279497724119E-08" ixz="2.19173188415509E-06" iyy="0.0280010940276735" iyz="-0.00192249677468735" izz="0.0368124806983828" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link6.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link7">
+    <inertial>
+      <origin xyz="5.28878934317328E-07 3.37199922245852E-07 -0.0274928177649973" rpy="0 0 0" />
+      <mass value="0.3" />
+      <inertia ixx="0.00177365937026673" ixy="-1.2410521430667E-07" ixz="-2.95409038849194E-08" iyy="0.00177328722902027" iyz="-1.57567641084958E-08" izz="0.00272748710623445" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/visual/link7.DAE" />
+      </geometry>
+      <material name="iiwa_material">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/iiwa/collision/link7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_ee" />
+  <joint name="world_base_joint" type="fixed">
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+    <parent link="world" />
+    <child link="base_link" />
+  </joint>
+  <joint name="joint1" type="revolute">
+    <origin xyz="0 0 0.3375" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="link1" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.97" upper="2.97" effort="200" velocity="1.71" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.8827" soft_upper_limit="2.8827" k_velocity="10" />
+  </joint>
+  <joint name="joint2" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="link1" />
+    <child link="link2" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.09" upper="2.09" effort="200" velocity="1.71" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.0027" soft_upper_limit="2.0027" k_velocity="10" />
+  </joint>
+  <joint name="joint3" type="revolute">
+    <origin xyz="0 -0.3993 0" rpy="1.5708 0 0" />
+    <parent link="link2" />
+    <child link="link3" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.97" upper="2.97" effort="200" velocity="1.75" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.8827" soft_upper_limit="2.8827" k_velocity="10" />
+  </joint>
+  <joint name="joint4" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="link3" />
+    <child link="link4" />
+    <axis xyz="0 0 -1" />
+    <limit lower="-2.09" upper="2.09" effort="200" velocity="2.27" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.0027" soft_upper_limit="2.0027" k_velocity="10" />
+  </joint>
+  <joint name="joint5" type="revolute">
+    <origin xyz="0 -0.3993 0" rpy="1.5708 0 0" />
+    <parent link="link4" />
+    <child link="link5" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.97" upper="2.97" effort="200" velocity="2.44" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.8827" soft_upper_limit="2.8827" k_velocity="10" />
+  </joint>
+  <joint name="joint6" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="link5" />
+    <child link="link6" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.09" upper="2.09" effort="200" velocity="3.14" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.0027" soft_upper_limit="2.0027" k_velocity="10" />
+  </joint>
+  <joint name="joint7" type="revolute">
+    <origin xyz="0 -0.126 0" rpy="1.5708 0 0" />
+    <parent link="link6" />
+    <child link="link7" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.05" upper="3.05" effort="200" velocity="3.14" />
+    <dynamics damping="10.0" friction="0.1" />
+    <safety_controller soft_lower_limit="-2.9627" soft_upper_limit="2.9627" k_velocity="10" />
+  </joint>
+  <joint name="tools_joint" type="fixed">
+    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+    <parent link="link7" />
+    <child link="link_ee" />
+  </joint>
+</robot>
@@ -0,0 +1,194 @@
+#OBJ
+#T-FLEX CAD ObjGenerator generate date:20.11.2024 23:59:52
+v 0.27 0.23 -0.019999
+v 0.27 0.949999 -0.019999
+v 0.31 0.99 -0.019999
+v -0.31 0.99 -0.019999
+v -0.27 0.949999 -0.019999
+v -0.27 0.23 -0.019999
+v 0.27 0.189999 -0.019999
+v -0.31 0 -0.019999
+v -0.27 0.189999 -0.019999
+v 0.31 0 -0.019999
+v 0.27 0 -0.019999
+v -0.27 -0 -0.019999
+v -0.27 0.23 0.019999
+v -0.27 0.189999 0.019999
+v 0.27 0.189999 0.019999
+v -0.31 0.99 0.019999
+v -0.31 0 0.019999
+v -0.27 -0 0.019999
+v 0.27 0.949999 0.019999
+v 0.31 0.99 0.019999
+v 0.27 0.23 0.019999
+v 0.31 0 0.019999
+v -0.27 0.949999 0.019999
+v 0.27 0 0.019999
+v 0.27 0.23 0.019999
+v 0.27 0.23 -0.019999
+v -0.27 0.23 -0.019999
+v -0.27 0.23 0.019999
+v -0.31 0.99 0.019999
+v -0.31 0.99 -0.019999
+v -0.31 0 -0.019999
+v -0.31 0 0.019999
+v -0.31 0.99 0.019999
+v 0.31 0.99 0.019999
+v 0.31 0.99 -0.019999
+v -0.31 0.99 -0.019999
+v 0.31 0 0.019999
+v 0.31 0 -0.019999
+v 0.31 0.99 -0.019999
+v 0.31 0.99 0.019999
+v 0.27 0 0.019999
+v 0.27 0 -0.019999
+v 0.31 0 -0.019999
+v 0.31 0 0.019999
+v 0.27 0 -0.019999
+v 0.27 0 0.019999
+v 0.27 0.189999 0.019999
+v 0.27 0.189999 -0.019999
+v -0.27 0.189999 0.019999
+v -0.27 0.189999 -0.019999
+v 0.27 0.189999 -0.019999
+v 0.27 0.189999 0.019999
+v -0.27 -0 0.019999
+v -0.27 -0 -0.019999
+v -0.27 0.189999 -0.019999
+v -0.27 0.189999 0.019999
+v -0.31 0 0.019999
+v -0.31 0 -0.019999
+v -0.27 -0 -0.019999
+v -0.27 -0 0.019999
+v 0.27 0.23 -0.019999
+v 0.27 0.23 0.019999
+v 0.27 0.949999 0.019999
+v 0.27 0.949999 -0.019999
+v 0.27 0.949999 -0.019999
+v 0.27 0.949999 0.019999
+v -0.27 0.949999 0.019999
+v -0.27 0.949999 -0.019999
+v -0.27 0.949999 -0.019999
+v -0.27 0.949999 0.019999
+v -0.27 0.23 0.019999
+v -0.27 0.23 -0.019999
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 1 -0 0
+vn 1 -0 0
+vn 1 -0 0
+vn 1 -0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn -0 -1 0
+vn -0 -1 0
+vn -0 -1 0
+vn -0 -1 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+f 1//1 2//2 3//3
+f 4//4 5//5 6//6
+f 4//4 3//3 2//2
+f 2//2 5//5 4//4
+f 3//3 7//7 1//1
+f 6//6 1//1 7//7
+f 8//8 6//6 9//9
+f 7//7 9//9 6//6
+f 6//6 8//8 4//4
+f 10//10 11//11 7//7
+f 7//7 3//3 10//10
+f 9//9 12//12 8//8
+f 13//13 14//14 15//15
+f 14//14 16//16 17//17
+f 17//17 18//18 14//14
+f 19//19 20//20 16//16
+f 21//21 15//15 20//20
+f 15//15 21//21 13//13
+f 22//22 20//20 15//15
+f 20//20 19//19 21//21
+f 16//16 23//23 19//19
+f 16//16 14//14 13//13
+f 13//13 23//23 16//16
+f 15//15 24//24 22//22
+f 25//25 26//26 27//27
+f 27//27 28//28 25//25
+f 29//29 30//30 31//31
+f 31//31 32//32 29//29
+f 33//33 34//34 35//35
+f 35//35 36//36 33//33
+f 37//37 38//38 39//39
+f 39//39 40//40 37//37
+f 41//41 42//42 43//43
+f 43//43 44//44 41//41
+f 45//45 46//46 47//47
+f 47//47 48//48 45//45
+f 49//49 50//50 51//51
+f 51//51 52//52 49//49
+f 53//53 54//54 55//55
+f 55//55 56//56 53//53
+f 57//57 58//58 59//59
+f 59//59 60//60 57//57
+f 61//61 62//62 63//63
+f 63//63 64//64 61//61
+f 65//65 66//66 67//67
+f 67//67 68//68 65//65
+f 69//69 70//70 71//71
+f 71//71 72//72 69//69
@@ -0,0 +1,222 @@
+#OBJ
+#T-FLEX CAD ObjGenerator generate date:20.11.2024 23:59:6
+v 0.625 0.541 0.019999
+v 0.625 0.764999 0.019999
+v 0.675 0.721 0.019999
+v 0.634999 0.764999 0.019999
+v 0.675 0.725 0.019999
+v 0.634999 0.541 0.019999
+v 0.675 0.721 0.208
+v 0.625 0.764999 0.208
+v 0.625 0.541 0.208
+v 0.675 0.725 0.208
+v 0.634999 0.764999 0.208
+v 0.634999 0.541 0.208
+v 0.634999 0.764999 0.208
+v 0.675 0.725 0.208
+v 0.675 0.725 0.019999
+v 0.634999 0.764999 0.019999
+v 0.675 0.721 0.208
+v 0.634999 0.541 0.208
+v 0.634999 0.541 0.019999
+v 0.675 0.721 0.019999
+v 0.625 0.541 0.208
+v 0.625 0.541 0.019999
+v 0.634999 0.541 0.019999
+v 0.634999 0.541 0.208
+v 0.675 0.725 0.208
+v 0.675 0.721 0.208
+v 0.675 0.721 0.019999
+v 0.675 0.725 0.019999
+v 0.634999 0.764999 0.208
+v 0.634999 0.764999 0.019999
+v 0.625 0.764999 0.019999
+v 0.625 0.764999 0.208
+v 0.6125 0.787999 0.261999
+v 0.0125 0.787999 0.261999
+v 0.0125 0.012 0.261999
+v 0.6125 0.012 0.261999
+v 0.0125 0.787999 0.25
+v 0.0125 0.787999 0.261999
+v 0.6125 0.787999 0.261999
+v 0.6125 0.787999 0.25
+v 0.6125 0.787999 0.25
+v 0.6125 0.787999 0.261999
+v 0.6125 0.012 0.261999
+v 0.6125 0.012 0.25
+v 0.6125 0.012 0.25
+v 0.6125 0.012 0.261999
+v 0.0125 0.012 0.261999
+v 0.0125 0.012 0.25
+v 0.0125 0.012 0.25
+v 0.0125 0.012 0.261999
+v 0.0125 0.787999 0.261999
+v 0.0125 0.787999 0.25
+v 0.625 0 0
+v 0 0 0
+v 0 0.8 0
+v 0.625 0.8 0
+v 0 0 0.25
+v 0.0125 0.012 0.25
+v 0.0125 0.787999 0.25
+v 0.625 0 0.25
+v 0.6125 0.012 0.25
+v 0.625 0.8 0.25
+v 0.6125 0.787999 0.25
+v 0 0.8 0.25
+v 0.625 0.8 0.25
+v 0.625 0.8 0
+v 0 0.8 0
+v 0 0.8 0.25
+v 0 0 0.25
+v 0 0 0
+v 0.625 0 0
+v 0.625 0 0.25
+v 0 0 0
+v 0 0 0.25
+v 0 0.8 0.25
+v 0 0.8 0
+v 0.625 0.8 0
+v 0.625 0.764999 0.019999
+v 0.625 0.541 0.019999
+v 0.625 0.541 0.208
+v 0.625 0.764999 0.208
+v 0.625 0.8 0.25
+v 0.625 0 0
+v 0.625 0 0.25
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0.707106 0.707106 0
+vn 0.707106 0.707106 0
+vn 0.707106 0.707106 0
+vn 0.707106 0.707106 0
+vn 0.976187 -0.21693 0
+vn 0.976187 -0.21693 0
+vn 0.976187 -0.21693 0
+vn 0.976187 -0.21693 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn -0 1 0
+vn -0 1 0
+vn -0 1 0
+vn -0 1 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn -1 -0 0
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+f 1//1 2//2 3//3
+f 2//2 4//4 5//5
+f 5//5 3//3 2//2
+f 3//3 6//6 1//1
+f 7//7 8//8 9//9
+f 7//7 10//10 11//11
+f 9//9 12//12 7//7
+f 11//11 8//8 7//7
+f 13//13 14//14 15//15
+f 15//15 16//16 13//13
+f 17//17 18//18 19//19
+f 19//19 20//20 17//17
+f 21//21 22//22 23//23
+f 23//23 24//24 21//21
+f 25//25 26//26 27//27
+f 27//27 28//28 25//25
+f 29//29 30//30 31//31
+f 31//31 32//32 29//29
+f 33//33 34//34 35//35
+f 35//35 36//36 33//33
+f 37//37 38//38 39//39
+f 39//39 40//40 37//37
+f 41//41 42//42 43//43
+f 43//43 44//44 41//41
+f 45//45 46//46 47//47
+f 47//47 48//48 45//45
+f 49//49 50//50 51//51
+f 51//51 52//52 49//49
+f 53//53 54//54 55//55
+f 55//55 56//56 53//53
+f 57//57 58//58 59//59
+f 60//60 61//61 58//58
+f 58//58 57//57 60//60
+f 60//60 62//62 63//63
+f 64//64 59//59 63//63
+f 59//59 64//64 57//57
+f 63//63 62//62 64//64
+f 63//63 61//61 60//60
+f 65//65 66//66 67//67
+f 67//67 68//68 65//65
+f 69//69 70//70 71//71
+f 71//71 72//72 69//69
+f 73//73 74//74 75//75
+f 75//75 76//76 73//73
+f 77//77 78//78 79//79
+f 80//80 81//81 82//82
+f 79//79 83//83 77//77
+f 81//81 78//78 77//77
+f 80//80 84//84 83//83
+f 77//77 82//82 81//81
+f 83//83 79//79 80//80
+f 82//82 84//84 80//80
@@ -0,0 +1,47 @@
+[project]
+name = "iiwa-genesis"
+version = "0.1.0"
+requires-python = ">=3.12,<3.14"
+dependencies = [
+    "genesis-world==1.4.1", 
+    "torch>=2.8", 
+    "numpy>=2", 
+    "scipy>=1.14", 
+    "pyyaml>=6", 
+    "pillow>=11", 
+    "opencv-python>=4.10"
+]
+
+[tool.uv]
+exclude-dependencies = ["xacro"]
+conflicts = [
+    [{ extra = "cpu" }, { extra = "cuda" }]
+]
+
+[project.optional-dependencies]
+cpu = ["torch>=2.8"]
+cuda = ["torch>=2.8"]
+
+[tool.uv.sources]
+torch = [
+    { index = "pytorch-cpu", extra = "cpu" }, 
+    { index = "pytorch-cuda", extra = "cuda" }
+]
+
+[[tool.uv.index]]
+name = "pytorch-cpu"
+url = "https://download.pytorch.org/whl/cpu"
+explicit = true
+
+[[tool.uv.index]]
+name = "pytorch-cuda"
+url = "https://pypi.org/simple"
+explicit = true
+
+[dependency-groups]
+dev = ["pytest>=8.4"]
+
+[tool.pytest.ini_options]
+addopts = "--disable-plugin-autoload"
+pythonpath = ["src"]
+testpaths = ["tests"]
@@ -0,0 +1,14 @@
+from pathlib import Path
+import os
+import sys
+
+# Ignore inherited Python overlays (for example a shell that sourced a ROS workspace).
+# All runtime dependencies belong to uv's virtual environment.
+overlays = {str(Path(p).resolve()) for p in os.environ.get("PYTHONPATH", "").split(os.pathsep) if p}
+sys.path[:] = [p for p in sys.path if str(Path(p).resolve()) not in overlays]
+sys.path.insert(0, str(Path(__file__).resolve().parent / "src"))
+
+from iiwa_genesis.cli import main
+
+if __name__ == "__main__":
+    main()
@@ -0,0 +1 @@
+"""Standalone iiwa scene and RGB-D capture."""
@@ -0,0 +1,119 @@
+"""Aligned pinhole RGB-D approximation, with explicit optical coordinate conventions."""
+from dataclasses import dataclass
+import json
+import math
+
+import numpy as np
+from PIL import Image
+from scipy.spatial.transform import Rotation
+
+
+def array(value):
+    return value.detach().cpu().numpy() if hasattr(value, "detach") else np.asarray(value)
+
+
+def optical_pose(position, lookat, up):
+    z = np.subtract(lookat, position).astype(float)
+    z /= np.linalg.norm(z)
+    x = np.cross(z, up)
+    x /= np.linalg.norm(x)
+    y = np.cross(z, x)
+    pose = np.eye(4)
+    pose[:3, :3] = np.column_stack((x, y, z))
+    pose[:3, 3] = position
+    return pose
+
+
+def calibration(cfg):
+    w, h = cfg["resolution"]
+    f = w / (2 * math.tan(cfg["horizontal_fov"] / 2))
+    return np.array([[f, 0, w / 2], [0, f, h / 2], [0, 0, 1]], dtype=float)
+
+
+def optical_rays(cfg):
+    w, h = cfg["resolution"]
+    k = calibration(cfg)
+    u, v = np.meshgrid(np.arange(w) + 0.5, np.arange(h) + 0.5)
+    return np.stack(((u - k[0, 2]) / k[0, 0], (v - k[1, 2]) / k[1, 1], np.ones_like(u)), axis=-1)
+
+
+def process_depth(distance, cfg, rng):
+    rays = optical_rays(cfg)
+    depth = np.asarray(distance) / np.linalg.norm(rays, axis=-1)
+    valid = np.isfinite(depth) & (depth >= cfg["min_range"]) & (depth <= cfg["max_range"])
+    if cfg["noise"]["enabled"]:
+        depth = depth + rng.normal(0, cfg["noise"]["depth_stddev"], depth.shape)
+    depth = np.round(depth / cfg["depth_resolution"]) * cfg["depth_resolution"]
+    valid &= (depth >= cfg["min_range"]) & (depth <= cfg["max_range"])
+    return np.where(valid, depth, 0).astype(np.float32), valid
+
+
+@dataclass
+class Frame:
+    rgb: np.ndarray
+    depth: np.ndarray
+    valid: np.ndarray
+    metadata: dict
+
+
+class D455:
+    def __init__(self, gs, scene, name, cfg, seed, robot=None):
+        self.name, self.cfg, self.robot = name, cfg, robot
+        self.rng = np.random.default_rng(seed)
+        self.local_pose = optical_pose(cfg["position"], cfg["lookat"], cfg["up"])
+        self.link = robot.get_link(cfg["link"]) if robot is not None else None
+        mount = {} if self.link is None else {"entity_idx": robot.idx, "link_idx_local": self.link.idx_local}
+        w, h = cfg["resolution"]
+        vfov = math.degrees(2 * math.atan(h / w * math.tan(cfg["horizontal_fov"] / 2)))
+        self.rgb = scene.add_sensor(gs.sensors.RasterizerCameraOptions(
+            res=cfg["resolution"], pos=cfg["position"], lookat=cfg["lookat"], up=cfg["up"],
+            fov=vfov, near=cfg["min_range"], far=cfg["max_range"], **mount))
+        # Raycaster axes: X forward, Y left, Z up. Optical axes: X right, Y down, Z forward.
+        ray_to_optical = np.eye(4)
+        ray_to_optical[:3, :3] = [[0, -1, 0], [0, 0, -1], [1, 0, 0]]
+        ray_pose = self.local_pose @ ray_to_optical
+        self.depth = scene.add_sensor(gs.sensors.DepthCamera(
+            pattern=gs.sensors.DepthCameraPattern(res=cfg["resolution"], fov_horizontal=math.degrees(cfg["horizontal_fov"])),
+            pos_offset=tuple(ray_pose[:3, 3]),
+            euler_offset=tuple(Rotation.from_matrix(ray_pose[:3, :3]).as_euler("xyz", degrees=True)),
+            min_range=0.0, max_range=cfg["max_range"] * np.linalg.norm(optical_rays(cfg), axis=-1).max(),
+            no_hit_value=float("inf"), return_points=False, **mount))
+
+    def world_pose(self):
+        if self.link is None:
+            return self.local_pose.copy()
+        # Genesis public pose uses (w, x, y, z).
+        w, x, y, z = array(self.link.get_quat(relative=False))
+        transform = np.eye(4)
+        transform[:3, :3] = [[1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)],
+                              [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)],
+                              [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)]]
+        transform[:3, 3] = array(self.link.get_pos(relative=False))
+        return transform @ self.local_pose
+
+    def read(self, timestamp):
+        rgb = array(self.rgb.read().rgb).copy()
+        depth, valid = process_depth(array(self.depth.read_image()), self.cfg, self.rng)
+        if self.cfg["noise"]["enabled"]:
+            rgb = np.clip(rgb.astype(float) + self.rng.normal(0, 255 * self.cfg["noise"]["rgb_stddev"], rgb.shape), 0, 255).astype(np.uint8)
+        pose = self.world_pose()
+        return Frame(rgb, depth, valid, {
+            "camera": self.name, "model": "D455 pinhole approximation", "simulation_time": timestamp,
+            "resolution": self.cfg["resolution"], "fps": self.cfg["fps"],
+            "min_range": self.cfg["min_range"], "max_range": self.cfg["max_range"],
+            "depth_resolution": self.cfg["depth_resolution"],
+            "intrinsics": calibration(self.cfg).tolist(), "world_from_optical": pose.tolist(),
+            "optical_from_world": np.linalg.inv(pose).tolist(), "depth_units": "metres",
+            "depth_convention": "optical Z; zero means invalid", "pixel_convention": "centres at (u+0.5,v+0.5)",
+            "optical_axes": "X right, Y down, Z forward", "noise": self.cfg["noise"],
+        })
+
+    def save(self, frame, directory, index, pointcloud=False):
+        directory.mkdir(parents=True, exist_ok=True)
+        stem = directory / f"{self.name}_{index:06d}"
+        Image.fromarray(frame.rgb).save(stem.with_suffix(".png"))
+        payload = {"depth": frame.depth, "valid": frame.valid}
+        if pointcloud:
+            payload["xyz_optical"] = np.where(frame.valid[..., None], optical_rays(self.cfg) * frame.depth[..., None], np.nan).astype(np.float32)
+        np.savez_compressed(stem.with_suffix(".npz"), **payload)
+        stem.with_suffix(".json").write_text(json.dumps(frame.metadata, indent=2))
@@ -0,0 +1,85 @@
+import argparse
+from datetime import datetime, timezone
+import os
+from pathlib import Path
+import time
+from yaml import YAMLError
+
+from .config import ROOT, load_config
+
+
+def main():
+    parser = argparse.ArgumentParser(description="Standalone iiwa RGB-D world; no ROS runtime")
+    parser.add_argument("--config", type=Path, default=ROOT / "configs/scene.yaml")
+    view = parser.add_mutually_exclusive_group()
+    view.add_argument("--viewer", dest="viewer", action="store_true")
+    view.add_argument("--headless", dest="viewer", action="store_false")
+    parser.set_defaults(viewer=None)
+    parser.add_argument("--save-frames", action="store_true")
+    parser.add_argument("--steps", type=int, help="finite simulation steps (headless default: 120)")
+    parser.add_argument("--backend", choices=("cpu", "gpu"))
+    args = parser.parse_args()
+    try:
+        cfg = load_config(args.config)
+    except (ValueError, KeyError, TypeError, OSError, YAMLError) as exc:
+        parser.error(f"Invalid configuration: {exc}")
+    if args.steps is not None and args.steps < 1:
+        parser.error("--steps must be positive")
+    viewer = cfg["scene"]["viewer"] if args.viewer is None else args.viewer
+    steps = args.steps if args.steps is not None else (None if viewer else 120)
+    save = args.save_frames or cfg["output"]["save_frames"]
+    if not viewer:
+        os.environ.setdefault("PYOPENGL_PLATFORM", "egl")
+    from .runtime import import_genesis
+    gs = import_genesis()
+    from .scene import build_scene
+
+    gs.init(backend=getattr(gs, args.backend or cfg["scene"]["backend"]), seed=cfg["scene"]["seed"], logging_level="warning")
+    preview = viewer and cfg["output"]["preview"]
+    if preview:
+        import cv2
+    try:
+        scene, robot, cameras = build_scene(gs, cfg, viewer)
+        directory = cfg["output"]["directory"] / datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%S_%fZ")
+        print(f"Mode: {cfg['scene']['mode']}; cameras: {', '.join(c.name for c in cameras)}; Genesis {gs.__version__}; backend {gs.backend}")
+        if save:
+            print(f"Frames: {directory}")
+        next_capture = {c.name: 1.0 / c.cfg["fps"] for c in cameras}
+        counts = {c.name: 0 for c in cameras}
+        dt = cfg["scene"]["dt"]
+        start = time.monotonic()
+        step = 0
+        while steps is None or step < steps:
+            if viewer and not scene.viewer.is_alive():
+                break
+            scene.step()
+            step += 1
+            timestamp = step * dt
+            for camera in cameras:
+                if timestamp + 1e-10 < next_capture[camera.name]:
+                    continue
+                frame = camera.read(timestamp)
+                if save:
+                    camera.save(frame, directory, counts[camera.name], cfg["output"]["pointcloud"])
+                if preview:
+                    import numpy as np
+                    depth_view = cv2.applyColorMap(np.uint8(np.clip(frame.depth / camera.cfg["max_range"], 0, 1) * 255), cv2.COLORMAP_TURBO)
+                    depth_view[~frame.valid] = 0
+                    cv2.imshow(camera.name, np.hstack((cv2.cvtColor(frame.rgb, cv2.COLOR_RGB2BGR), depth_view)))
+                counts[camera.name] += 1
+                next_capture[camera.name] = (counts[camera.name] + 1) / camera.cfg["fps"]
+            if preview and cv2.waitKey(1) & 0xff in (27, ord("q")):
+                break
+            if viewer:
+                time.sleep(max(0, min(dt, start + timestamp - time.monotonic())))
+        print(f"Completed {step} steps; captured {counts}")
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if preview:
+            cv2.destroyAllWindows()
+        gs.destroy()
+
+
+if __name__ == "__main__":
+    main()
@@ -0,0 +1,66 @@
+from pathlib import Path
+import math
+import xml.etree.ElementTree as ET
+
+import numpy as np
+import yaml
+
+ROOT = Path(__file__).resolve().parents[2]
+DESCRIPTION = ROOT / "iiwa_genesis_description"
+
+
+def vector(value, name):
+    a = np.asarray(value, dtype=float)
+    if a.shape != (3,) or not np.isfinite(a).all():
+        raise ValueError(f"{name}: expected three finite numbers")
+    return tuple(a)
+
+
+def load_config(path):
+    path = Path(path).resolve()
+    cfg = yaml.safe_load(path.read_text())
+    mode = cfg["scene"]["mode"]
+    if mode not in ("without_gripper", "with_gripper"):
+        raise ValueError("scene.mode must be without_gripper or with_gripper")
+    cfg["robot"]["urdf"] = DESCRIPTION / f"urdf/iiwa_{mode}.urdf"
+    cfg["table"]["urdf"] = DESCRIPTION / "table/table.urdf"
+    if cfg["scene"]["backend"] not in ("cpu", "gpu"):
+        raise ValueError("scene.backend must be cpu or gpu")
+    dt = float(cfg["scene"]["dt"])
+    if not math.isfinite(dt) or dt <= 0:
+        raise ValueError("scene.dt must be positive")
+    for name in ("robot", "table"):
+        for key in ("position", "euler"):
+            cfg[name][key] = vector(cfg[name][key], f"{name}.{key}")
+    limits = {j.get("name"): j.find("limit") for j in ET.parse(cfg["robot"]["urdf"]).getroot().findall("joint") if j.get("type") == "revolute"}
+    if set(cfg["robot"]["joints"]) != set(limits):
+        raise ValueError("robot.joints must contain joint1 through joint7 exactly")
+    for name, value in cfg["robot"]["joints"].items():
+        if not float(limits[name].get("lower")) <= float(value) <= float(limits[name].get("upper")):
+            raise ValueError(f"{name}: angle outside URDF limits")
+    # The mode is the single source of truth, including when a legacy enabled flag exists.
+    cfg["cameras"] = {name: cfg["cameras"][name] for name in (["overhead", "tool"] if mode == "with_gripper" else ["overhead"])}
+    for name, cam in cfg["cameras"].items():
+        for key in ("position", "lookat", "up"):
+            cam[key] = vector(cam[key], f"cameras.{name}.{key}")
+        forward = np.subtract(cam["lookat"], cam["position"])
+        if np.linalg.norm(np.cross(forward, cam["up"])) < 1e-8:
+            raise ValueError(f"{name}: look direction and up must be nonzero and nonparallel")
+        res = cam["resolution"]
+        if len(res) != 2 or any(type(x) is not int or x <= 0 for x in res):
+            raise ValueError(f"{name}: invalid resolution")
+        cam["resolution"] = tuple(res)
+        if not 0 < cam["horizontal_fov"] < math.pi:
+            raise ValueError(f"{name}: FOV must be in radians between zero and pi")
+        if not 0 < cam["fps"] <= 1 / dt or not math.isfinite(cam["fps"]):
+            raise ValueError(f"{name}: FPS must be positive and at most simulation frequency")
+        if not 0 < cam["min_range"] < cam["max_range"] < math.inf:
+            raise ValueError(f"{name}: invalid range")
+        if not 0 < cam["depth_resolution"] < cam["max_range"]:
+            raise ValueError(f"{name}: invalid depth resolution")
+        for key in ("rgb_stddev", "depth_stddev"):
+            if not 0 <= cam["noise"][key] < math.inf:
+                raise ValueError(f"{name}: invalid noise standard deviation")
+    out = Path(cfg["output"]["directory"])
+    cfg["output"]["directory"] = out if out.is_absolute() else path.parent / out
+    return cfg
@@ -0,0 +1,48 @@
+"""Version-scoped adaptation: Genesis 1.4.1 imports an unused Xacro dependency eagerly.
+
+Transform only that module in memory. No third-party files are changed and no
+Xacro module (real or stub) is installed. Plain URDF loading remains upstream code.
+"""
+import ast
+from importlib import import_module, metadata
+from importlib.abc import MetaPathFinder
+from importlib.machinery import PathFinder, SourceFileLoader
+import sys
+
+
+def plain_urdf_source(source, filename):
+    tree = ast.parse(source, filename)
+    imports = [n for n in tree.body if isinstance(n, ast.Import) and any(a.name == "xacro" for a in n.names)]
+    functions = [n for n in tree.body if isinstance(n, ast.FunctionDef) and n.name == "load_xacro"]
+    if len(imports) != 1 or len(functions) != 1:
+        raise RuntimeError("Genesis URDF loader changed; review the ROS-free adaptation before upgrading")
+    tree.body.remove(imports[0])
+    functions[0].body = ast.parse('raise RuntimeError("Xacro is disabled: use a plain .urdf file")').body
+    return compile(ast.fix_missing_locations(tree), filename, "exec")
+
+
+class _PlainURDFLoader(SourceFileLoader):
+    def get_code(self, fullname):
+        return plain_urdf_source(self.get_source(fullname), self.path)
+
+
+class _PlainURDFFinder(MetaPathFinder):
+    def find_spec(self, fullname, path=None, target=None):
+        if fullname != "genesis.utils.urdf":
+            return None
+        spec = PathFinder.find_spec(fullname, path)
+        if spec is None or not isinstance(spec.loader, SourceFileLoader):
+            raise RuntimeError("Expected Genesis Python source installation")
+        spec.loader = _PlainURDFLoader(fullname, spec.origin)
+        return spec
+
+
+def import_genesis():
+    if metadata.version("genesis-world") != "1.4.1":
+        raise RuntimeError("This application requires the locked Genesis 1.4.1; run uv sync --locked")
+    finder = _PlainURDFFinder()
+    sys.meta_path.insert(0, finder)
+    try:
+        return import_module("genesis")
+    finally:
+        sys.meta_path.remove(finder)
@@ -0,0 +1,39 @@
+from .camera import D455
+from .config import DESCRIPTION
+
+
+def build_scene(gs, cfg, viewer):
+    scene = gs.Scene(
+        show_viewer=viewer,
+        # v1 is a pose inspection scene: no falling arm and no active controller.
+        sim_options=gs.options.SimOptions(dt=cfg["scene"]["dt"], gravity=(0, 0, 0)),
+        viewer_options=gs.options.ViewerOptions(camera_pos=(2.8, -3, 2.6), camera_lookat=(0, 0, 1.1)),
+        vis_options=gs.options.VisOptions(show_world_frame=True),
+    )
+    scene.add_entity(gs.morphs.Plane())
+    for name in ("table", "robot"):
+        options = cfg[name]
+        entity = scene.add_entity(gs.morphs.URDF(
+            file=str(options["urdf"]), fixed=True, pos=options["position"], euler=options["euler"],
+            merge_fixed_links=False, decimate=False))
+        if name == "robot":
+            robot = entity
+    # Reuse the supplied D455 CAD mesh. Its lens face is at mesh Z=0; body extends +Z.
+    from .camera import optical_pose
+    from scipy.spatial.transform import Rotation
+    import numpy as np
+    overhead = cfg["cameras"]["overhead"]
+    pose = optical_pose(overhead["position"], overhead["lookat"], overhead["up"])
+    mesh_pos = pose[:3, 3] + pose[:3, :3] @ [-0.062, 0.0155, -0.001]
+    q = Rotation.from_matrix(pose[:3, :3] @ np.diag([1., -1., -1.])).as_quat(scalar_first=True)
+    scene.add_entity(gs.morphs.Mesh(file=str(DESCRIPTION / "urdf/meshes/tools/cameraD455.dae"),
+                                   pos=tuple(mesh_pos), quat=tuple(q), fixed=True, decimate=False))
+    cameras = [D455(gs, scene, name, cam, cfg["scene"]["seed"] + i,
+                    robot if name == "tool" else None)
+               for i, (name, cam) in enumerate(cfg["cameras"].items())]
+    scene.build()
+    indices = [robot.get_joint(f"joint{i}").dof_idx_local for i in range(1, 8)]
+    if robot.n_dofs != 7:
+        raise ValueError(f"Expected seven robot DOFs, got {robot.n_dofs}")
+    robot.set_dofs_position([cfg["robot"]["joints"][f"joint{i}"] for i in range(1, 8)], dofs_idx_local=indices)
+    return scene, robot, cameras
@@ -0,0 +1,49 @@
+import json
+
+import numpy as np
+
+from iiwa_genesis.camera import D455, Frame, calibration, optical_pose, optical_rays, process_depth
+from iiwa_genesis.config import ROOT, load_config
+
+
+def camera_config():
+    return load_config(ROOT / "configs/scene.yaml")["cameras"]["overhead"]
+
+
+def test_downward_pose():
+    cfg = camera_config()
+    pose = optical_pose(cfg["position"], cfg["lookat"], cfg["up"])
+    np.testing.assert_allclose(pose[:3, :3].T @ pose[:3, :3], np.eye(3))
+    np.testing.assert_allclose(pose[:3, 2], [0, 0, -1])
+    assert np.linalg.det(pose[:3, :3]) > 0.999
+
+
+def test_plane_range_converts_to_constant_z():
+    cfg = camera_config()
+    distances = 2 * np.linalg.norm(optical_rays(cfg), axis=-1)
+    depth, valid = process_depth(distances, cfg, np.random.default_rng(1))
+    np.testing.assert_allclose(depth, 2)
+    assert valid.all()
+    distances[0, :3] = [np.inf, 0, 100]
+    depth, valid = process_depth(distances, cfg, np.random.default_rng(1))
+    assert not valid[0, :3].any()
+    assert not depth[0, :3].any()
+
+
+def test_seeded_noise_and_serialization(tmp_path):
+    cfg = camera_config()
+    cfg["noise"]["enabled"] = True
+    distances = 2 * np.linalg.norm(optical_rays(cfg), axis=-1)
+    depth, valid = process_depth(distances, cfg, np.random.default_rng(7))
+    second, _ = process_depth(distances, cfg, np.random.default_rng(7))
+    np.testing.assert_array_equal(depth, second)
+    assert np.std(depth) > 0
+    camera = object.__new__(D455)
+    camera.name, camera.cfg = "overhead", cfg
+    frame = Frame(np.zeros((480, 640, 3), dtype=np.uint8), depth, valid,
+                  {"intrinsics": calibration(cfg).tolist(), "world_from_optical": np.eye(4).tolist()})
+    camera.save(frame, tmp_path, 0, pointcloud=True)
+    data = np.load(tmp_path / "overhead_000000.npz")
+    assert data["depth"].shape == (480, 640)
+    assert data["xyz_optical"].shape == (480, 640, 3)
+    assert json.loads((tmp_path / "overhead_000000.json").read_text()) == frame.metadata
@@ -0,0 +1,76 @@
+import xml.etree.ElementTree as ET
+
+import pytest
+import yaml
+
+from iiwa_genesis.config import ROOT, DESCRIPTION, load_config
+
+
+def config_file(tmp_path, mode="without_gripper"):
+    data = yaml.safe_load((ROOT / "configs/scene.yaml").read_text())
+    data["scene"]["mode"] = mode
+    file = tmp_path / "scene.yaml"
+    file.write_text(yaml.safe_dump(data))
+    return file
+
+
+@pytest.mark.parametrize("mode,count", [("without_gripper", 1), ("with_gripper", 2)])
+def test_mode_selects_urdf_and_cameras(tmp_path, mode, count):
+    cfg = load_config(config_file(tmp_path, mode))
+    assert len(cfg["cameras"]) == count
+    assert cfg["robot"]["urdf"].name == f"iiwa_{mode}.urdf"
+
+
+@pytest.mark.parametrize("mode", ["without_gripper", "with_gripper"])
+def test_urdf_tree_and_assets(mode):
+    path = DESCRIPTION / f"urdf/iiwa_{mode}.urdf"
+    root = ET.parse(path).getroot()
+    links = {l.get("name") for l in root.findall("link")}
+    joints = root.findall("joint")
+    assert sum(j.get("type") == "revolute" for j in joints) == 7
+    children = [j.find("child").get("link") for j in joints]
+    assert len(children) == len(set(children))
+    assert links - set(children) == {"world"}
+    assert all(j.find("parent").get("link") in links for j in joints)
+    assert ("camera_optical_frame" in links) == (mode == "with_gripper")
+    for mesh in root.iter("mesh"):
+        assert (path.parent / mesh.get("filename")).is_file()
+    text = path.read_text()
+    for prohibited in ("package://", "xacro", "ros2_control", "<gazebo", "<webots", "$(find"):
+        assert prohibited not in text
+
+
+def test_tool_transforms_are_preserved():
+    original = ET.parse(ROOT / "iiwa_description/urdf/tools/patron.xacro").getroot()
+    exported = ET.parse(DESCRIPTION / "urdf/iiwa_with_gripper.urdf").getroot()
+    for joint in original.findall("joint"):
+        actual = exported.find(f"joint[@name='{joint.get('name')}']")
+        assert actual is not None
+        assert actual.attrib == joint.attrib
+        for tag in ("origin", "parent", "child"):
+            assert actual.find(tag).attrib == joint.find(tag).attrib
+
+
+@pytest.mark.parametrize("bad", ["mode", "joints", "up", "fps", "range"])
+def test_invalid_config(tmp_path, bad):
+    file = config_file(tmp_path)
+    data = yaml.safe_load(file.read_text())
+    if bad == "mode":
+        data["scene"]["mode"] = "invalid"
+    elif bad == "joints":
+        data["robot"]["joints"]["joint1"] = 100
+    elif bad == "up":
+        data["cameras"]["overhead"]["up"] = [0, 0, 1]
+    elif bad == "fps":
+        data["cameras"]["overhead"]["fps"] = 0
+    else:
+        data["cameras"]["overhead"]["min_range"] = 10
+    file.write_text(yaml.safe_dump(data))
+    with pytest.raises(ValueError):
+        load_config(file)
+
+
+def test_table_mesh_paths():
+    path = DESCRIPTION / "table/table.urdf"
+    for mesh in ET.parse(path).iter("mesh"):
+        assert (path.parent / mesh.get("filename")).is_file()
@@ -0,0 +1,43 @@
+"""Explicit opt-in: compiles Genesis kernels and needs an EGL-capable OpenGL stack."""
+import json
+import os
+import subprocess
+import sys
+
+import numpy as np
+from PIL import Image
+import pytest
+import yaml
+
+from iiwa_genesis.config import ROOT
+
+
+@pytest.mark.skipif(os.environ.get("IIWA_GENESIS_SMOKE") != "1", reason="set IIWA_GENESIS_SMOKE=1 to run real CPU/EGL tests")
+@pytest.mark.parametrize("mode,names", [("without_gripper", ["overhead"]), ("with_gripper", ["overhead", "tool"])])
+def test_cpu_rgbd_scene(tmp_path, mode, names):
+    cfg = yaml.safe_load((ROOT / "configs/scene.yaml").read_text())
+    cfg["scene"]["mode"] = mode
+    cfg["output"]["directory"] = str(tmp_path / "frames")
+    path = tmp_path / "scene.yaml"
+    path.write_text(yaml.safe_dump(cfg))
+    env = os.environ.copy()
+    env.pop("PYTHONPATH", None)
+    result = subprocess.run([sys.executable, str(ROOT / "run_scene.py"), "--config", str(path),
+                             "--headless", "--backend", "cpu", "--steps", "4", "--save-frames"],
+                            env=env, capture_output=True, text=True, timeout=600)
+    assert result.returncode == 0, result.stdout + result.stderr
+    captures = list((tmp_path / "frames").glob("*/*.npz"))
+    assert len(captures) == len(names)
+    for name in names:
+        capture = next(p for p in captures if p.stem.startswith(name))
+        data = np.load(capture)
+        assert np.array(Image.open(capture.with_suffix(".png"))).shape == (480, 640, 3)
+        assert data["depth"].shape == (480, 640)
+        valid = data["valid"]
+        assert np.isfinite(data["depth"]).all()
+        assert (data["depth"][~valid] == 0).all()
+        assert (data["depth"][valid] >= .01).all() and (data["depth"][valid] <= 6).all()
+        meta = json.loads(capture.with_suffix(".json").read_text())
+        np.testing.assert_allclose(np.array(meta["optical_from_world"]) @ meta["world_from_optical"], np.eye(4), atol=1e-6)
+        if name == "overhead":
+            assert valid.any()
@@ -0,0 +1,18 @@
+import pytest
+
+from iiwa_genesis.runtime import plain_urdf_source
+
+
+def test_adapter_disables_only_xacro_path():
+    source = "import xacro\ndef load_xacro(path, mappings):\n return xacro.process_file(path)\ndef load_plain():\n return 7\n"
+    namespace = {}
+    exec(plain_urdf_source(source, "test.py"), namespace)
+    assert namespace["load_plain"]() == 7
+    assert "xacro" not in namespace
+    with pytest.raises(RuntimeError, match="disabled"):
+        namespace["load_xacro"]("foo.xacro", {})
+
+
+def test_adapter_rejects_changed_upstream():
+    with pytest.raises(RuntimeError, match="changed"):
+        plain_urdf_source("def load_plain(): pass", "test.py")
@@ -0,0 +1,77 @@
+"""One-time, ROS-free export of this repository's known description macros.
+
+This is deliberately not a general Xacro interpreter. Runtime uses the emitted URDFs.
+"""
+from copy import deepcopy
+from pathlib import Path
+import re
+import shutil
+import xml.etree.ElementTree as ET
+
+ROOT = Path(__file__).resolve().parents[1]
+SOURCE = ROOT / "iiwa_description"
+DEST = ROOT / "iiwa_genesis_description"
+NS = "{http://www.ros.org/wiki/xacro}"
+
+
+def export():
+    props = {}
+    for name in ("params", "meshes"):
+        for node in ET.parse(SOURCE / f"urdf/{name}.xacro").getroot():
+            if node.tag == NS + "property":
+                props[node.get("name")] = node.get("value")
+
+    def substitute(value, variables):
+        for _ in range(8):
+            updated = re.sub(r"\$\{([^}]+)\}", lambda m: variables[m[1]], value)
+            if updated == value:
+                return value
+            value = updated
+        raise ValueError("Unresolved variable")
+
+    macros = {n.get("name"): n for n in ET.parse(SOURCE / "urdf/macros.xacro").getroot()}
+
+    def expand(node):
+        if not node.tag.startswith(NS):
+            return [deepcopy(node)]
+        name = node.tag[len(NS):]
+        if name == "include":
+            return []
+        result = []
+        for template in macros[name]:
+            item = deepcopy(template)
+            for child in item.iter():
+                child.attrib = {k: substitute(v, props | node.attrib) for k, v in child.attrib.items()}
+            result.append(item)
+        return result
+
+    def write(robot, path):
+        path.parent.mkdir(parents=True, exist_ok=True)
+        for mesh in robot.iter("mesh"):
+            source = mesh.get("filename")
+            prefix = "package://iiwa_description/resource/meshes/"
+            if not source.startswith(prefix):
+                raise ValueError(source)
+            relative = source[len(prefix):]
+            target = DEST / "urdf/meshes" / relative
+            target.parent.mkdir(parents=True, exist_ok=True)
+            shutil.copy2(SOURCE / "resource/meshes" / relative, target)
+            mesh.set("filename", ("meshes/" if path.parent.name == "urdf" else "../urdf/meshes/") + relative)
+        ET.indent(robot)
+        ET.ElementTree(robot).write(path, encoding="utf-8", xml_declaration=True)
+
+    base = ET.Element("robot", name="iiwa7")
+    for name in ("links", "joints"):
+        for node in ET.parse(SOURCE / f"urdf/{name}.xacro").getroot():
+            base.extend(expand(node))
+    write(deepcopy(base), DEST / "urdf/iiwa_without_gripper.urdf")
+    for node in ET.parse(SOURCE / "urdf/tools/patron.xacro").getroot():
+        if node.tag in ("link", "joint"):
+            base.append(deepcopy(node))
+    write(base, DEST / "urdf/iiwa_with_gripper.urdf")
+    write(ET.parse(SOURCE / "urdf/table/table.urdf").getroot(), DEST / "table/table.urdf")
+    shutil.copy2(SOURCE / "LICENSE", DEST / "LICENSE")
+
+
+if __name__ == "__main__":
+    export()
				`@@ -0,0 +1 @@`
				`"""Standalone iiwa scene and RGB-D capture."""`