Fix mujoco compiler

Multiverse-Framework · Feb 13, 2025 · b79e54d · b79e54d
1 parent 059985e
commit b79e54d
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/...rs/src/multiverse_simulators/src/mujoco_connector/src/mujoco_connector/mujoco_compiler.py b/...rs/src/multiverse_simulators/src/mujoco_connector/src/mujoco_connector/mujoco_compiler.py
@@ -308,15 +308,18 @@ def add_references(self, references: Dict[str, Dict[str, Any]]):
             equality.type = mujoco.mjtEq.mjEQ_WELD
             equality.name = reference_name
             equality.objtype = mujoco.mjtObj.mjOBJ_BODY
+            equality.data[:3] = [0, 0, 0]
+            equality.data[3:10] = [0, 0, 0, 1, 0, 0, 0]
+            equality.data[10] = 1
             for key, value in reference.items():
                 if key == "body1":
                     equality.name1 = value
                 elif key == "body2":
                     equality.name2 = value
-                elif key == "relpose":
-                    equality.data[:7] = value
                 elif key == "anchor":
                     equality.data[:3] = value
+                elif key == "relpose":
+                    equality.data[3:10] = value
                 elif key == "torquescale":
                     equality.data[10] = value