Update mujoco

Multiverse-Framework · Dec 4, 2024 · 5839c00 · 5839c00
1 parent 70d7e22
commit 5839c00
Show file tree

Hide file tree

Showing 8 changed files with 668 additions and 34 deletions.
diff --git a/build_third_parties.sh b/build_third_parties.sh
@@ -137,7 +137,7 @@ if [ $BUILD_MUJOCO = true ]; then
 
     # Build MuJoCo
 
-    FROM_SRC=false
+    FROM_SRC=true
     MUJOCO_BUILD_DIR=$BUILD_DIR/mujoco
     MUJOCO_EXT_DIR=$EXT_DIR/mujoco
 
@@ -151,9 +151,11 @@ if [ $BUILD_MUJOCO = true ]; then
 
     if [ $FROM_SRC = true ]; then
         # Build MuJoCo
-
-        git submodule update --init $MUJOCO_EXT_DIR
-        (cd $MUJOCO_BUILD_DIR && cmake $MUJOCO_EXT_DIR -DCMAKE_INSTALL_PREFIX=$MUJOCO_BUILD_DIR -Wno-deprecated -Wno-dev && cmake --build . && cmake --install .)
+
+        # git submodule update --init $MUJOCO_EXT_DIR
+        MUJOCO_PLUGIN_DIR=$MUJOCO_BUILD_DIR/bin/mujoco_plugin
+        mkdir -p $MUJOCO_PLUGIN_DIR
+        (cd $MUJOCO_BUILD_DIR && cmake $MUJOCO_EXT_DIR -DCMAKE_INSTALL_PREFIX=$MUJOCO_BUILD_DIR -Wno-deprecated -Wno-dev && cmake --build . && cmake --install . && cp $MUJOCO_BUILD_DIR/lib/libmultiverse_connector.so $MUJOCO_PLUGIN_DIR)
     else
         # Download MuJoCo
 

diff --git a/multiverse/modules/multiverse_connectors/scripts/launch_simulators.py b/multiverse/modules/multiverse_connectors/scripts/launch_simulators.py
@@ -9,6 +9,7 @@
 from multiverse_launch import MultiverseLaunch
 from utils import find_files, run_subprocess
 
+import xml.etree.ElementTree as ET
 
 def parse_mujoco(resources_paths: List[str], mujoco_data: Dict[str, Any]):
     worlds_path = find_files(resources_paths, mujoco_data["world"]["path"])
@@ -96,30 +97,86 @@ def run_simulator(self, compiler_result: subprocess.CompletedProcess, simulation
             raise RuntimeError(f"Failed to compile {simulation_name}")
 
         scene_xml_path = re.search(r"Scene:\s*([^\n]+)", compiler_result.stdout).group(1)
-        cmd = [f"{scene_xml_path}"]
-
         world = simulation_data.get("world")
         world_name = "world" if world is None else world["name"]
-        rtf_desired = 1 if world_name not in self.worlds else self.worlds[world_name].get("rtf_desired", 1)
-
-        config_dict = simulation_data.get("config", {})
-        config_dict["rtf_desired"] = rtf_desired
-        cmd += [f"{config_dict}".replace(" ", "").replace("'", '"')]
-
-        if self.multiverse_clients.get(simulation_name) is not None:
-            self.multiverse_clients[simulation_name]["meta_data"] = {
-                "world_name": world_name,
-                "simulation_name": simulation_name,
-            }
-
-            multiverse_dict = {"multiverse_server": self.multiverse_server,
-                               "multiverse_client": self.multiverse_clients[simulation_name]}
-            multiverse_dict["multiverse_client"]["resources"] = self.resources_paths
-
-            cmd += [f"{multiverse_dict}".replace(" ", "").replace("'", '"')]
-
-        suffix = "_headless" if simulation_data.get("headless", False) else ""
-        cmd = [f"{simulation_data['simulator']}{suffix}"] + cmd
+
+        if simulation_data.get("multiverse_as_plugin", False):
+            tree = ET.parse(scene_xml_path)
+            root = tree.getroot()
+
+            extension_element = ET.Element("extension")
+            root.append(extension_element)
+
+            plugin_element = ET.Element("plugin")
+            extension_element.append(plugin_element)
+            plugin_element.set("plugin", "mujoco.multiverse_connector")
+
+            instance_element = ET.Element("instance")
+            plugin_element.append(instance_element)
+            instance_element.set("name", "multiverse_client")
+
+            config_server_host_element = ET.Element("config")
+            instance_element.append(config_server_host_element)
+            config_server_host_element.set("key", "server_host")
+            config_server_host_element.set("value", self.multiverse_server.get("host", "tcp://127.0.0.1"))
+
+            config_server_port_element = ET.Element("config")
+            instance_element.append(config_server_port_element)
+            config_server_port_element.set("key", "server_port")
+            config_server_port_element.set("value", str(self.multiverse_server.get("port", 7000)))
+
+            config_client_port_element = ET.Element("config")
+            instance_element.append(config_client_port_element)
+            config_client_port_element.set("key", "client_port")
+            config_client_port_element.set("value", str(self.multiverse_clients[simulation_name].get("port", 7500)))
+
+            config_world_name_element = ET.Element("config")
+            instance_element.append(config_world_name_element)
+            config_world_name_element.set("key", "world_name")
+            config_world_name_element.set("value", world_name)
+
+            config_simulation_name_element = ET.Element("config")
+            instance_element.append(config_simulation_name_element)
+            config_simulation_name_element.set("key", "simulation_name")
+            config_simulation_name_element.set("value", simulation_name)
+
+            send_objects = self.multiverse_clients[simulation_name].get("send", {})
+            config_send_element = ET.Element("config")
+            instance_element.append(config_send_element)
+            config_send_element.set("key", "send")
+            config_send_element.set("value", str(send_objects))
+
+            receive_objects = self.multiverse_clients[simulation_name].get("receive", {})
+            config_receive_element = ET.Element("config")
+            instance_element.append(config_receive_element)
+            config_receive_element.set("key", "receive")
+            config_receive_element.set("value", str(receive_objects))
+
+            tree.write(scene_xml_path)
+
+            cmd = ["simulate"] + [f"{scene_xml_path}"]
+        else:
+            cmd = [f"{scene_xml_path}"]
+            rtf_desired = 1 if world_name not in self.worlds else self.worlds[world_name].get("rtf_desired", 1)
+
+            config_dict = simulation_data.get("config", {})
+            config_dict["rtf_desired"] = rtf_desired
+            cmd += [f"{config_dict}".replace(" ", "").replace("'", '"')]
+
+            if self.multiverse_clients.get(simulation_name) is not None:
+                self.multiverse_clients[simulation_name]["meta_data"] = {
+                    "world_name": world_name,
+                    "simulation_name": simulation_name,
+                }
+
+                multiverse_dict = {"multiverse_server": self.multiverse_server,
+                                "multiverse_client": self.multiverse_clients[simulation_name]}
+                multiverse_dict["multiverse_client"]["resources"] = self.resources_paths
+
+                cmd += [f"{multiverse_dict}".replace(" ", "").replace("'", '"')]
+
+            suffix = "_headless" if simulation_data.get("headless", False) else ""
+            cmd = [f"{simulation_data['simulator']}{suffix}"] + cmd
 
         return run_subprocess(cmd)
 

diff --git a/multiverse/modules/multiverse_connectors/src/mujoco/scripts/mujoco_compile.py b/multiverse/modules/multiverse_connectors/src/mujoco/scripts/mujoco_compile.py
@@ -438,7 +438,7 @@ def add_mocap(save_xml_path: str,
         worldbody_element.append(body_element)
 
 
-def apply_references(save_xml_path: str, references: Dict[str, Dict[str, any]]) -> None:
+def apply_references(save_xml_path: str, references: Dict[str, Dict[str, any]], with_cursor: bool = True) -> None:
     m = mujoco.MjModel.from_xml_path(save_xml_path)
     d = mujoco.MjData(m)
     mujoco.mj_step(m, d)
@@ -483,8 +483,8 @@ def apply_references(save_xml_path: str, references: Dict[str, Dict[str, any]])
 
     mujoco.mj_resetDataKeyframe(m, d, 0)
     mujoco.mj_step(m, d)
-    mpos_list_str = "0 0 0"
-    mquat_list_str = "1 0 0 0"
+    mpos_list_str = "0 0 0" if with_cursor else ""
+    mquat_list_str = "1 0 0 0" if with_cursor else ""
     for mocap_name, (body_id, _, _) in mocap_dict.items():
         body_pos = d.xpos[body_id]
         body_quat = d.xquat[body_id]
@@ -502,7 +502,6 @@ def apply_references(save_xml_path: str, references: Dict[str, Dict[str, any]])
 
     indent(tree.getroot(), space="\t", level=0)
     tree.write(save_xml_path, encoding="utf-8", xml_declaration=True)
-    tree.write(save_xml_path + "2", encoding="utf-8", xml_declaration=True)
 
 
 def parse_references(references) -> Dict[str, any]:
@@ -583,12 +582,12 @@ def build_world_xml(self, robots: Dict[str, Robot], objects: Dict[str, Object]):
             add_key_frame_element(root, self.keyframe_dict)
             tree.write(self.save_xml_path, encoding="utf-8", xml_declaration=True)
 
-        self.add_visual_and_cursor_element(root)
-        indent(tree.getroot(), space="\t", level=0)
-        tree.write(self.save_xml_path, encoding="utf-8", xml_declaration=True)
+        # self.add_visual_and_cursor_element(root)
+        # indent(tree.getroot(), space="\t", level=0)
+        # tree.write(self.save_xml_path, encoding="utf-8", xml_declaration=True)
 
         if self.references is not None:
-            apply_references(self.save_xml_path, self.references)
+            apply_references(self.save_xml_path, self.references, False)
 
     def create_world_xml(self):
         if not os.path.exists(self.save_dir_path):

diff --git a/multiverse/resources/muv/table_with_bowling_new.muv b/multiverse/resources/muv/table_with_bowling_new.muv
@@ -0,0 +1,87 @@
+resources: 
+  - ../robots
+  - ../worlds
+  - ../objects
+
+worlds:
+  bowling_simulation:
+    rtf_desired: 1
+  table_simulation:
+    rtf_desired: 1
+  table_with_bowling_simulation:
+    rtf_desired: 1
+
+simulations:
+  table_with_bowling_simulation:
+    simulator: mujoco
+    multiverse_as_plugin: true
+    world:
+      name: world
+      path: table_with_bowling/table_with_bowling_no_cursor.xml
+    config:
+      max_time_step: 0.01
+      min_time_step: 0.001
+
+  bowling_simulation:
+    simulator: mujoco
+    multiverse_as_plugin: true
+    world:
+      name: world
+      path: table_with_bowling/bowling_no_cursor.xml
+    config:
+      max_time_step: 0.01
+      min_time_step: 0.001
+
+  table_simulation:
+    simulator: mujoco
+    multiverse_as_plugin: true
+    world:
+      name: world
+      path: table_with_bowling/table_no_cursor.xml
+    config:
+      max_time_step: 0.01
+      min_time_step: 0.001
+
+  cube_simulation:
+    simulator: mujoco
+    multiverse_as_plugin: true
+    world:
+      name: world
+      path: table_with_bowling/cube_no_cursor.xml
+    config:
+      max_time_step: 0.01
+      min_time_step: 0.001
+
+multiverse_server:
+  host: "tcp://127.0.0.1"
+  port: 7000
+
+multiverse_clients:
+  table_with_bowling_simulation:
+    port: 7501
+    send:
+      bowling: ["force", "torque"]
+      table: ["force", "torque"]
+    receive:
+      bowling: ["relative_velocity", "position", "quaternion"]
+      table: ["relative_velocity", "position", "quaternion"]
+      cube: ["position", "quaternion"]
+
+  bowling_simulation:
+    port: 7502
+    send:
+      bowling: ["relative_velocity", "position", "quaternion"]
+    receive:
+      bowling: ["force", "torque"]
+
+  table_simulation:
+    port: 7503
+    send:
+      table: ["relative_velocity", "position", "quaternion"]
+    receive:
+      table: ["force", "torque"]
+
+  cube_simulation:
+    port: 7504
+    send:
+      cube: ["position", "quaternion"]
diff --git a/multiverse/resources/worlds/table_with_bowling/bowling_no_cursor.xml b/multiverse/resources/worlds/table_with_bowling/bowling_no_cursor.xml
@@ -0,0 +1,13 @@
+<mujoco>
+    <option timestep="0.001" integrator="RK4" gravity="0 0 -9.81">
+        <flag energy="enable" contact="disable" />
+    </option>
+    <compiler angle="radian" />
+
+    <worldbody>
+        <body name="bowling" pos=".0 .0 16" >
+            <freejoint />
+            <geom type="sphere" size=".1 .1 .1" rgba="0 .9 0 1" mass="10" />
+        </body>
+    </worldbody>
+</mujoco>
diff --git a/multiverse/resources/worlds/table_with_bowling/cube_no_cursor.xml b/multiverse/resources/worlds/table_with_bowling/cube_no_cursor.xml
@@ -0,0 +1,16 @@
+<mujoco>
+    <option timestep="0.001" integrator="RK4" gravity="0 0 -9.81">
+        <flag energy="enable" contact="disable" />
+    </option>
+    <compiler angle="radian" />
+
+    <worldbody>
+        <body name="cube" pos="1.5 0 1" mocap="true">
+            <geom type="box" size=".1 .1 .1" rgba="0 0 .9 1" mass="10" />
+        </body>
+    </worldbody>
+
+    <visual>
+		<global fovy="45" azimuth="225" elevation="-30" />
+	</visual>
+</mujoco>
diff --git a/multiverse/resources/worlds/table_with_bowling/table_no_cursor.xml b/multiverse/resources/worlds/table_with_bowling/table_no_cursor.xml
@@ -0,0 +1,21 @@
+<mujoco>
+    <option timestep="0.001" integrator="RK4" gravity="0 0 -9.81">
+        <flag energy="enable" contact="disable" />
+    </option>
+    <compiler angle="radian" />
+
+    <worldbody>
+        <body name="table" pos="0 0 8">
+            <freejoint />
+            <geom type="box" size=".5 .5 .05" rgba="0 0 .9 1" mass="6" />
+            <geom type="box" pos=".45 .45 -.25" size=".05 .05 .25" rgba="0 0 .9 1" mass="1" />
+            <geom type="box" pos=".45 -.45 -.25" size=".05 .05 .25" rgba="0 0 .9 1" mass="1" />
+            <geom type="box" pos="-.45 .45 -.25" size=".05 .05 .25" rgba="0 0 .9 1" mass="1" />
+            <geom type="box" pos="-.45 -.45 -.25" size=".05 .05 .25" rgba="0 0 .9 1" mass="1" />
+        </body>
+    </worldbody>
+
+    <visual>
+		<global fovy="45" azimuth="225" elevation="-30" />
+	</visual>
+</mujoco>