From 24d7aaa5b7716e0faea17689e77e76fad3a439fd Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Thu, 7 Nov 2024 12:00:19 +0100 Subject: [PATCH 1/4] feat(scene_management)!: add mjcf & assets The rcsss.scenes dictionary now contains: - a key "xml" with the path to the "scene.xml" - a key "mjb" with the path to the "scene.mjb" - a key "urdfs" with a dictionary mapping file stem to file path (e.g. `rcsss.scenes["lab"]["urdf"]["fr3"]` to get the path to `fr3.urdf` --- CMakeLists.txt | 2 +- cmake/compile_scenes.cmake | 10 ++++++++++ python/examples/fr3.py | 2 +- python/rcsss/__init__.py | 9 ++++++++- python/rcsss/envs/factories.py | 18 ++++++++++++++---- src/sim/gui_client.cpp | 25 +++++++++++++++++++++++++ 6 files changed, 59 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4b6a9371..0e290705 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -96,7 +96,7 @@ FetchContent_MakeAvailable(libfranka rl pybind11) if (${INCLUDE_UTN_MODELS}) if (DEFINED GITLAB_MODELS_TOKEN) include(download_scenes) - set(ref 77e8782a3488cbf01361d2322f02e75c0d820644) + set(ref b13a8d55bacc6470ece0788c654739e5cdef9265) FetchContent_Declare( scenes URL "https://gitos.rrze.fau.de/api/v4/projects/1100/repository/archive?path=scenes&sha=${ref}" diff --git a/cmake/compile_scenes.cmake b/cmake/compile_scenes.cmake index 2c155013..06f14f61 100644 --- a/cmake/compile_scenes.cmake +++ b/cmake/compile_scenes.cmake @@ -40,6 +40,16 @@ foreach(scene_dir ${scene_dirs}) foreach(urdf ${urdfs}) install(FILES ${urdf} DESTINATION rcsss/scenes/${scene_name} COMPONENT python_package) endforeach() + + # Install xml files + foreach(xml ${xml_files}) + install(FILES ${xml} DESTINATION rcsss/scenes/${scene_name} COMPONENT python_package) + endforeach() + + # Install assets + foreach(asset ${asset_files}) + install(FILES ${asset} DESTINATION rcsss/scenes/${scene_name}/assets COMPONENT python_package) + endforeach() endforeach() # Create a custom target that depends on all generated .mjb files diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 254ee83c..4c69a5a6 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -65,7 +65,7 @@ def main(): with resource_manger: if ROBOT_INSTANCE == RobotInstance.SIMULATION: - simulation = sim.Sim(rcsss.scenes["fr3_empty_world"]) + simulation = sim.Sim(rcsss.scenes["fr3_empty_world"]["mjb"]) urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False) ik = rcsss.common.IK(urdf_path) robot = rcsss.sim.FR3(simulation, "0", ik) diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index 515972bc..cd517a19 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -7,7 +7,14 @@ from rcsss._core import __version__, common, hw scenes = { - path.stem: path / "scene.mjb" for path in (pathlib.Path(site.getsitepackages()[0]) / "rcsss" / "scenes").glob("*") + path.stem: { + "xml": path / "scene.xml" if (path / "scene.xml").exists() else None, + "mjb": path / "scene.mjb" if (path / "scene.mjb").exists() else None, + "urdfs": { + urdf.stem: urdf for urdf in path.glob("*.urdf") + } if tuple(path.glob("*.urdf")) else None + } + for path in (pathlib.Path(site.getsitepackages()[0]) / "rcsss" / "scenes").glob("*") } __all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs"] diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index ccea92ee..d15763d3 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -59,7 +59,7 @@ def default_realsense(name2id: dict[str, str]): def get_urdf_path(urdf_path: str | PathLike | None, allow_none_if_not_found: bool = False) -> str | None: if urdf_path is None and "lab" in rcsss.scenes: - urdf_path = rcsss.scenes["lab"].parent / "fr3.urdf" + urdf_path = rcsss.scenes["lab"]["urdfs"]["fr3"] assert urdf_path.exists(), "Automatic deduced urdf path does not exist. Corrupted models directory." logger.info("Using automatic found urdf.") elif urdf_path is None and not allow_none_if_not_found: @@ -125,7 +125,11 @@ def fr3_hw_env( assert urdf_path is not None env = CollisionGuard.env_from_xml_paths( env, - str(rcsss.scenes.get(str(collision_guard), collision_guard)), + ( + rcsss.scenes[collision_guard]["mjb"] + if collision_guard in scenes + else collision_guard + ), urdf_path, gripper=True, check_home_collision=False, @@ -198,8 +202,10 @@ def fr3_sim_env( assert urdf_path is not None if mjcf not in rcsss.scenes: logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") + else: + mjcf = rcsss.scenes[mjcf]["mjb"] - simulation = sim.Sim(rcsss.scenes.get(str(mjcf), mjcf)) + simulation = sim.Sim(mjcf) ik = rcsss.common.IK(urdf_path) robot = rcsss.sim.FR3(simulation, "0", ik) robot.set_parameters(robot_cfg) @@ -217,7 +223,11 @@ def fr3_sim_env( if collision_guard: env = CollisionGuard.env_from_xml_paths( env, - str(rcsss.scenes.get(str(mjcf), mjcf)), + ( + rcsss.scenes[collision_guard]["mjb"] + if collision_guard in scenes + else collision_guard + ), urdf_path, gripper=gripper_cfg is not None, check_home_collision=False, diff --git a/src/sim/gui_client.cpp b/src/sim/gui_client.cpp index 8a2e99bd..15f09481 100644 --- a/src/sim/gui_client.cpp +++ b/src/sim/gui_client.cpp @@ -1,4 +1,5 @@ #include +#include #include #include @@ -72,14 +73,28 @@ void GuiClient::render_loop() { this->platform_ui->state().userdata = this; this->platform_ui->state().nrect = 1; this->platform_ui->SetEventCallback(event_callback); + auto last_cam_distance = this->cam.distance; while (!this->platform_ui->ShouldCloseWindow()) { this->platform_ui->PollEvents(); + if (this->cam.distance != last_cam_distance) { + std::cout << "cam.distance after PollEvents: " << this->cam.distance + << std::endl; + } this->shm.state_lock.lock_sharable(); mj_setState(this->m, this->d, this->shm.state.ptr, MJ_PHYSICS_SPEC); this->shm.state_lock.unlock_sharable(); mj_step(this->m, this->d); + if (this->cam.distance != last_cam_distance) { + std::cout << "cam.distance after step: " << this->cam.distance + << std::endl; + } mjv_updateScene(this->m, this->d, &this->opt, NULL, &this->cam, mjCAT_ALL, &this->scn); + if (this->cam.distance != last_cam_distance) { + std::cout << "cam.distance after update_scene: " << this->cam.distance + << std::endl; + } + last_cam_distance = this->cam.distance; std::tie(viewport.width, viewport.height) = this->platform_ui->GetFramebufferSize(); mjr_render(viewport, &this->scn, &this->platform_ui->mjr_context()); @@ -99,10 +114,20 @@ void event_callback(mjuiState* state) { mjrRect viewport = {0, 0, 0, 0}; std::tie(viewport.width, viewport.height) = client->platform_ui->GetFramebufferSize(); + auto reldy = -0.02 * state->sy; switch (state->type) { case mjEVENT_SCROLL: + std::cout << "cam.distance: " << client->cam.distance << std::endl; + std::cout << "m->stat.extent: " << client->m->stat.extent << std::endl; + std::cout << "reldy: " << reldy << std::endl; + std::cout << mju_log(1 + + client->cam.distance / client->m->stat.extent / 3) * + reldy * 9 * client->m->stat.extent + << std::endl; mjv_moveCamera(client->m, mjMOUSE_ZOOM, 0, -0.02 * state->sy, &client->scn, &client->cam); + std::cout << "cam.distance after cam move: " << client->cam.distance + << std::endl; case mjEVENT_MOVE: // determine action based on mouse button mjtMouse action; From e911f8bd420e56f25a0db1f9fe842a81582e9167 Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Thu, 7 Nov 2024 13:39:01 +0100 Subject: [PATCH 2/4] Fix linting errors & formatting pass --- python/rcsss/__init__.py | 24 ++++++++++++++++++------ python/rcsss/envs/factories.py | 16 ++++++---------- 2 files changed, 24 insertions(+), 16 deletions(-) diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index cd517a19..e89ccbc6 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -2,19 +2,31 @@ import pathlib import site +from typing import TypedDict from rcsss import camera, control, envs, sim from rcsss._core import __version__, common, hw -scenes = { + +class ScenesDict(TypedDict): + xml: pathlib.Path + mjb: pathlib.Path + urdfs: dict[str, pathlib.Path] + + +scenes: dict[str, ScenesDict] = { path.stem: { - "xml": path / "scene.xml" if (path / "scene.xml").exists() else None, - "mjb": path / "scene.mjb" if (path / "scene.mjb").exists() else None, - "urdfs": { - urdf.stem: urdf for urdf in path.glob("*.urdf") - } if tuple(path.glob("*.urdf")) else None + "xml": path / "scene.xml", + "mjb": path / "scene.mjb", + "urdfs": {urdf.stem: urdf for urdf in path.glob("*.urdf")}, } for path in (pathlib.Path(site.getsitepackages()[0]) / "rcsss" / "scenes").glob("*") } +for scene in scenes.values(): + assert scene["xml"].exists() + assert scene["mjb"].exists() + for urdf in scene["urdfs"].values(): + assert urdf.exists() +del scene __all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs"] diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index d15763d3..3d8c63fc 100644 --- a/python/rcsss/envs/factories.py +++ b/python/rcsss/envs/factories.py @@ -125,9 +125,9 @@ def fr3_hw_env( assert urdf_path is not None env = CollisionGuard.env_from_xml_paths( env, - ( + str( rcsss.scenes[collision_guard]["mjb"] - if collision_guard in scenes + if isinstance(collision_guard, str) and collision_guard in rcsss.scenes else collision_guard ), urdf_path, @@ -200,10 +200,10 @@ def fr3_sim_env( """ urdf_path = get_urdf_path(urdf_path, allow_none_if_not_found=False) assert urdf_path is not None - if mjcf not in rcsss.scenes: - logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") - else: + if isinstance(mjcf, str) and mjcf in rcsss.scenes: mjcf = rcsss.scenes[mjcf]["mjb"] + else: + logger.warning("mjcf not found as key in scenes, interpreting mjcf as path the mujoco scene xml") simulation = sim.Sim(mjcf) ik = rcsss.common.IK(urdf_path) @@ -223,11 +223,7 @@ def fr3_sim_env( if collision_guard: env = CollisionGuard.env_from_xml_paths( env, - ( - rcsss.scenes[collision_guard]["mjb"] - if collision_guard in scenes - else collision_guard - ), + str(rcsss.scenes[mjcf]["mjb"] if isinstance(mjcf, str) and mjcf in rcsss.scenes else mjcf), urdf_path, gripper=gripper_cfg is not None, check_home_collision=False, From 9ebe571c4db8e42016dc94cd2e8a4b48a18f4fd8 Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Wed, 8 Jan 2025 18:42:25 +0100 Subject: [PATCH 3/4] Use warning instead of assertions removed some acctidentaly commited debug code --- python/rcsss/__init__.py | 16 ++++++++++------ src/sim/gui_client.cpp | 25 ------------------------- 2 files changed, 10 insertions(+), 31 deletions(-) diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index 89e2aeaa..52a88f6b 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -3,6 +3,7 @@ import pathlib import site from typing import TypedDict +from warnings import warn from gymnasium import register from rcsss import camera, control, envs, sim @@ -24,12 +25,15 @@ class SceneData(TypedDict): } for path in (pathlib.Path(site.getsitepackages()[0]) / "rcsss" / "scenes").glob("*") } -# available mujoco scenes -for scene in scenes.values(): - assert scene["xml"].exists() - assert scene["mjb"].exists() - for urdf in scene["urdfs"].values(): - assert urdf.exists() + +for _ in scenes.values(): + if not _["xml"].exists(): + warn(f"Missing XML scene file {str(_['xml'])}") + if not _["mjb"].exists(): + warn(f"Missing mjb scene file {str(_['mjb'])}") + for urdf in _["urdfs"].values(): + if not urdf.exists(): + warn(f"Missing urdf file {str(urdf)}") # make submodules available __all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs"] diff --git a/src/sim/gui_client.cpp b/src/sim/gui_client.cpp index 15f09481..8a2e99bd 100644 --- a/src/sim/gui_client.cpp +++ b/src/sim/gui_client.cpp @@ -1,5 +1,4 @@ #include -#include #include #include @@ -73,28 +72,14 @@ void GuiClient::render_loop() { this->platform_ui->state().userdata = this; this->platform_ui->state().nrect = 1; this->platform_ui->SetEventCallback(event_callback); - auto last_cam_distance = this->cam.distance; while (!this->platform_ui->ShouldCloseWindow()) { this->platform_ui->PollEvents(); - if (this->cam.distance != last_cam_distance) { - std::cout << "cam.distance after PollEvents: " << this->cam.distance - << std::endl; - } this->shm.state_lock.lock_sharable(); mj_setState(this->m, this->d, this->shm.state.ptr, MJ_PHYSICS_SPEC); this->shm.state_lock.unlock_sharable(); mj_step(this->m, this->d); - if (this->cam.distance != last_cam_distance) { - std::cout << "cam.distance after step: " << this->cam.distance - << std::endl; - } mjv_updateScene(this->m, this->d, &this->opt, NULL, &this->cam, mjCAT_ALL, &this->scn); - if (this->cam.distance != last_cam_distance) { - std::cout << "cam.distance after update_scene: " << this->cam.distance - << std::endl; - } - last_cam_distance = this->cam.distance; std::tie(viewport.width, viewport.height) = this->platform_ui->GetFramebufferSize(); mjr_render(viewport, &this->scn, &this->platform_ui->mjr_context()); @@ -114,20 +99,10 @@ void event_callback(mjuiState* state) { mjrRect viewport = {0, 0, 0, 0}; std::tie(viewport.width, viewport.height) = client->platform_ui->GetFramebufferSize(); - auto reldy = -0.02 * state->sy; switch (state->type) { case mjEVENT_SCROLL: - std::cout << "cam.distance: " << client->cam.distance << std::endl; - std::cout << "m->stat.extent: " << client->m->stat.extent << std::endl; - std::cout << "reldy: " << reldy << std::endl; - std::cout << mju_log(1 + - client->cam.distance / client->m->stat.extent / 3) * - reldy * 9 * client->m->stat.extent - << std::endl; mjv_moveCamera(client->m, mjMOUSE_ZOOM, 0, -0.02 * state->sy, &client->scn, &client->cam); - std::cout << "cam.distance after cam move: " << client->cam.distance - << std::endl; case mjEVENT_MOVE: // determine action based on mouse button mjtMouse action; From 7736deff720ccdee33bdb1041f147bee2cd9fdb1 Mon Sep 17 00:00:00 2001 From: Pierre Krack Date: Thu, 9 Jan 2025 16:31:00 +0100 Subject: [PATCH 4/4] fix linting errors --- python/rcsss/__init__.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/rcsss/__init__.py b/python/rcsss/__init__.py index 52a88f6b..9620c528 100644 --- a/python/rcsss/__init__.py +++ b/python/rcsss/__init__.py @@ -28,12 +28,12 @@ class SceneData(TypedDict): for _ in scenes.values(): if not _["xml"].exists(): - warn(f"Missing XML scene file {str(_['xml'])}") + warn(f"Missing XML scene file {_['xml']}", stacklevel=2) if not _["mjb"].exists(): - warn(f"Missing mjb scene file {str(_['mjb'])}") - for urdf in _["urdfs"].values(): - if not urdf.exists(): - warn(f"Missing urdf file {str(urdf)}") + warn(f"Missing mjb scene file {_['mjb']}", stacklevel=2) + for __ in _["urdfs"].values(): + if not __.exists(): + warn(f"Missing urdf file {__}", stacklevel=2) # make submodules available __all__ = ["__doc__", "__version__", "common", "hw", "sim", "camera", "scenes", "control", "envs"]