wip

Author: yuecideng

embodichain/lab/sim/sensors/camera.py

@@ -263,6 +263,15 @@ def group_id(self) -> int:
             )
             return -1
 
+    @property
+    def is_attached(self) -> bool:
+        """Check if the camera is attached to a parent entity.
+
+        Returns:
+            bool: True if the camera is attached to a parent entity, False otherwise.
+        """
+        return self.cfg.extrinsics.parent is not None
+
     def update(self, **kwargs) -> None:
         """Update the sensor data.
 

embodichain/lab/sim/utility/keyboard_utils.py

@@ -213,8 +213,9 @@ def run_keyboard_control_for_camera(
                 log_info("Reset to initial pose")
             elif key == ord("p"):
                 new_pose_print = new_pose.copy()
-                new_pose_print[:3, 1] = -new_pose_print[:3, 1]
-                new_pose_print[:3, 2] = -new_pose_print[:3, 2]
+                if sensor.is_attached is False:
+                    new_pose_print[:3, 1] = -new_pose_print[:3, 1]
+                    new_pose_print[:3, 2] = -new_pose_print[:3, 2]
                 translation = new_pose_print[:3, 3]
                 rot = R.from_matrix(new_pose_print[:3, :3])
                 quaternion = rot.as_quat()