fix(sim): tcp offset in robot mjcf

Author: juelg

assets/fr3/mjcf/fr3_0.xml

@@ -1,4 +1,10 @@
 <mujoco>
+    <!-- custom tcp offset -->
+    <custom>
+		<numeric name="tcp_offset_translation" data="0.0 0.0 0.1034"/>
+        <!-- rotation matrix is row major -->
+		<numeric name="tcp_offset_rotation_matrix" data="0.707 0.707 0 -0.707 0.707 0 0 0 1"/>
+    </custom>
     <default>
         <default class="fr3">
             <joint armature="0.1" damping="1"/>

assets/fr3/mjcf/fr3_unnamed.xml

@@ -1,4 +1,10 @@
 <mujoco>
+    <!-- custom tcp offset -->
+    <custom>
+		<numeric name="tcp_offset_translation" data="0.0 0.0 0.1034"/>
+        <!-- rotation matrix is row major -->
+		<numeric name="tcp_offset_rotation_matrix" data="0.707 0.707 0 -0.707 0.707 0 0 0 1"/>
+    </custom>
     <default>
         <default class="fr3">
             <joint armature="0.1" damping="1"/>

python/rcs/envs/utils.py

@@ -88,12 +88,14 @@ def get_tcp_offset(mjcf: str | Path) -> rcs.common.Pose:
         elif mjcf.suffix == ".mjb":
             model = mj.MjModel.from_binary_path(str(mjcf))
         else:
-            msg = f"Expected .mjb, .mjcf or.xml, got {mjcf.suffix}"
+            msg = f"Expected .mjb, .mjcf or.xml, got {mjcf.suffix} and {mjcf}"
             raise AssertionError(msg)
     try:
-        tcp_offset = np.array(model.numeric("tcp_offset").data)
-        pose_offset = rcs.common.Pose(translation=tcp_offset)
-        return pose_offset
+        tcp_offset_translation = np.array(model.numeric("tcp_offset_translation").data)
+        tcp_offset_rotation_matrix = np.array(model.numeric("tcp_offset_rotation_matrix").data)
+        return rcs.common.Pose(
+            translation=tcp_offset_translation, rotation=tcp_offset_rotation_matrix.reshape((3, 3))
+        )  #  type: ignore
     except KeyError:
         msg = "No tcp offset found in the model. Using the default tcp offset."
         logging.info(msg)