From 322c3ea819f39d2732705c14cd6d6757fd5bfc68 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 10 Oct 2024 17:19:22 +0200 Subject: [PATCH 1/2] [LoadXMLBugFix] fixes a bug related to loading xml files as object descriptions. --- src/pycram/object_descriptors/mjcf.py | 32 +++++++++++++++++++++++++-- test/test_mjcf.py | 2 +- test/test_multiverse.py | 4 ++++ 3 files changed, 35 insertions(+), 3 deletions(-) diff --git a/src/pycram/object_descriptors/mjcf.py b/src/pycram/object_descriptors/mjcf.py index d4200db1f..c5a9b95e4 100644 --- a/src/pycram/object_descriptors/mjcf.py +++ b/src/pycram/object_descriptors/mjcf.py @@ -1,3 +1,4 @@ +import os import pathlib import numpy as np @@ -5,6 +6,7 @@ from dm_control import mjcf from geometry_msgs.msg import Point from typing_extensions import Union, List, Optional, Dict, Tuple +from xml.etree import ElementTree as ET from ..datastructures.dataclasses import Color, VisualShape, BoxVisualShape, CylinderVisualShape, \ SphereVisualShape, MeshVisualShape @@ -238,6 +240,16 @@ class ObjectDescription(AbstractObjectDescription): A class that represents an object description of an object. """ + COMPILER_TAG = 'compiler' + """ + The tag of the compiler element in the MJCF file. + """ + MESH_DIR_ATTR = 'meshdir' + TEXTURE_DIR_ATTR = 'texturedir' + """ + The attributes of the compiler element in the MJCF file. The meshdir attribute is the directory where the mesh files + are stored and the texturedir attribute is the directory where the texture files are stored.""" + class Link(AbstractObjectDescription.Link, LinkDescription): ... @@ -367,8 +379,24 @@ def generate_from_mesh_file(self, path: str, name: str, color: Optional[Color] = factory.export_to_mjcf(output_file_path=save_path) def generate_from_description_file(self, path: str, save_path: str, make_mesh_paths_absolute: bool = True) -> None: - mjcf_model = mjcf.from_file(path) - self.write_description_to_file(mjcf_model, save_path) + model_str = self.replace_relative_paths_with_absolute_paths(path) + self.write_description_to_file(model_str, save_path) + + def replace_relative_paths_with_absolute_paths(self, model_path: str) -> str: + """ + Replace the relative paths in the xml file to be absolute paths. + + :param model_path: The path to the xml file. + """ + tree = ET.parse(model_path) + root = tree.getroot() + compiler = root.find(self.COMPILER_TAG) + model_dir = pathlib.Path(model_path).parent + for rel_dir_attrib in [self.MESH_DIR_ATTR, self.TEXTURE_DIR_ATTR]: + rel_dir = compiler.get(rel_dir_attrib) + abs_dir = str(pathlib.Path(os.path.join(model_dir, rel_dir)).resolve()) + compiler.set(rel_dir_attrib, abs_dir) + return ET.tostring(root, encoding='unicode', method='xml') def generate_from_parameter_server(self, name: str, save_path: str) -> None: mjcf_string = rospy.get_param(name) diff --git a/test/test_mjcf.py b/test/test_mjcf.py index bcb9e2262..edfbb842b 100644 --- a/test/test_mjcf.py +++ b/test/test_mjcf.py @@ -26,7 +26,6 @@ def setUpClass(cls): joint2 = body3.add('joint', name='joint2', type='slide') cls.model = MJCFObjDesc() - print(model.to_xml_string()) cls.model.update_description_from_string(model.to_xml_string()) def test_child_map(self): @@ -38,3 +37,4 @@ def test_parent_map(self): def test_get_chain(self): self.assertEqual(self.model.get_chain('body1', 'body3'), ['body1', 'joint1', 'body2', 'joint2', 'body3']) + diff --git a/test/test_multiverse.py b/test/test_multiverse.py index 4c1426211..859ff1c2a 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -53,6 +53,10 @@ def tearDownClass(cls): def tearDown(self): self.multiverse.remove_all_objects() + def test_spawn_xml_object(self): + bread = Object("bread_1", ObjectType.GENERIC_OBJECT, "bread_1.xml", pose=Pose([1, 1, 0.1])) + self.assert_poses_are_equal(bread.get_pose(), Pose([1, 1, 0.1])) + def test_spawn_mesh_object(self): milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1])) self.assert_poses_are_equal(milk.get_pose(), Pose([1, 1, 0.1])) From 3ece41aef64fdbe6da07d767400292a70dddc3f8 Mon Sep 17 00:00:00 2001 From: Abdelrhman Bassiouny Date: Thu, 10 Oct 2024 17:56:33 +0200 Subject: [PATCH 2/2] [Multiverse] Added a folder in resources for storing muv files (yaml conf files) for multiverse world configurations. --- resources/worlds/pycram_test.muv | 52 ++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) create mode 100644 resources/worlds/pycram_test.muv diff --git a/resources/worlds/pycram_test.muv b/resources/worlds/pycram_test.muv new file mode 100644 index 000000000..bfe6acb7d --- /dev/null +++ b/resources/worlds/pycram_test.muv @@ -0,0 +1,52 @@ +resources: + - ../cached + - ../robots + - ../worlds + - ../objects + +worlds: + pycram_test: + rtf_desired: 1 + prospection_pycram_test: + rtf_desired: 1 + +simulations: + pycram_test: + simulator: mujoco + world: + name: world + path: apartment/mjcf/apartment.xml + apply: + body: + gravcomp: 1 + config: + max_time_step: 0.002 + min_time_step: 0.001 + prospection_pycram_test: + simulator: mujoco + world: + name: prospection_world + path: apartment/mjcf/apartment.xml + apply: + body: + gravcomp: 1 + config: + max_time_step: 0.002 + min_time_step: 0.001 + +multiverse_server: + host: "tcp://127.0.0.1" + port: 7000 + +multiverse_clients: + pycram_test: + port: 7500 + send: + body: ["position", "quaternion", "relative_velocity"] + joint: ["joint_rvalue", "joint_tvalue", "joint_linear_velocity", "joint_angular_velocity", "joint_force", "joint_torque"] + + prospection_pycram_test: + port: 7600 + send: + body: [ "position", "quaternion", "relative_velocity" ] + joint: [ "joint_rvalue", "joint_tvalue", "joint_linear_velocity", "joint_angular_velocity", "joint_force", "joint_torque" ] \ No newline at end of file