Merge remote-tracking branch 'pycram/dev' into user_save_state

# Conflicts:
#	test/test_multiverse.py

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" ]

src/pycram/object_descriptors/mjcf.py

@@ -1,10 +1,12 @@
+import os
 import pathlib
 
 import numpy as np
 import rospy
 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
@@ -239,6 +241,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):
         ...
 
@@ -368,8 +380,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)

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'])
+

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_get_axis_aligned_bounding_box_for_one_link_object(self):
         position = [1, 1, 0.1]
         milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, 1, 0.1],
@@ -380,7 +384,7 @@ def spawn_big_bowl() -> Object:
     def spawn_milk(position: List, orientation: Optional[List] = None, frame="map") -> Object:
         if orientation is None:
             orientation = [0, 0, 0, 1]
-        milk = Object("milk_box", ObjectType.MILK, "milk_box.urdf",
+        milk = Object("milk_box", ObjectType.MILK, "milk_box.xml",
                       pose=Pose(position, orientation, frame=frame))
         return milk